timo.menzel
/
test_pie
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
test_pie/external/exact_ccd/rootparitycollisiontest.cpp

2416 lines
98 KiB
C++
Raw Permalink Normal View History

Add texturemerge code.
2023-09-14 11:12:02 +02:00
#include "rootparitycollisiontest.h"
#include <cstdlib>
namespace CCD
{
namespace rootparity
{
namespace // unnamed namespace for local functions
{
///
/// Local helper functions
///
template<unsigned int N, class T>
inline void make_vector( const Vec<N,double>& in, Vec<N,T>& out );
template<class T>
void point_triangle_collision_function(const Vec3d &d_x0, const Vec3d &d_x1, const Vec3d &d_x2, const Vec3d &d_x3,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool d_t, bool d_u, bool d_v,
Vec<3,T>& out );
template<class T>
void edge_edge_collision_function(const Vec3d &d_x0, const Vec3d &d_x1, const Vec3d &d_x2, const Vec3d &d_x3,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool d_t, bool d_u, bool d_v,
Vec<3,T>& out );
template<class T>
void get_quad_vertices(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec4b& ts, const Vec4b& us, const Vec4b& vs,
Vec<3,T>& q0, Vec<3,T>& q1, Vec<3,T>& q2, Vec<3,T>& q3 );
template<class T>
void get_triangle_vertices(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec3b& ts, const Vec3b& us, const Vec3b& vs,
Vec<3,T>& q0, Vec<3,T>& q1, Vec<3,T>& q2 );
template<class T>
static T orientation2d(const Vec<2,T>& x0,
const Vec<2,T>& x1,
const Vec<2,T>& x2);
template<class T>
T orientation3d(const Vec<3,T>& x0,
const Vec<3,T>& x1,
const Vec<3,T>& x2,
const Vec<3,T>& x3);
int expansion_simplex_intersection1d(int k,
const expansion& x0,
const expansion& x1,
const expansion& x2,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2);
int expansion_simplex_intersection2d(int k,
const Vec2e& x0,
const Vec2e& x1,
const Vec2e& x2,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2 );
int expansion_simplex_intersection2d(int k,
const Vec2e& x0,
const Vec2e& x1,
const Vec2e& x2,
const Vec2e& x3,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3);
bool expansion_simplex_intersection3d(int k,
const Vec3e& x0,
const Vec3e& x1,
const Vec3e& x2,
const Vec3e& x3,
double* alpha0,
double* alpha1,
double* alpha2,
double* alpha3);
int degenerate_point_tetrahedron_intersection(const Vec3e& x0,
const Vec3e& x1,
const Vec3e& x2,
const Vec3e& x3,
const Vec3e& x4,
double* alpha0,
double* alpha1,
double* alpha2,
double* alpha3,
double* alpha4);
int degenerate_point_tetrahedron_intersection(const Vec3Interval& x0,
const Vec3Interval& x1,
const Vec3Interval& x2,
const Vec3Interval& x3,
const Vec3Interval& x4,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3,
double* out_alpha4);
template<class T>
int point_tetrahedron_intersection(const Vec<3,T>& x0,
const Vec<3,T>& x1,
const Vec<3,T>& x2,
const Vec<3,T>& x3,
const Vec<3,T>& x4,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3,
double* out_alpha4 );
int degenerate_edge_triangle_intersection(const Vec3e& x0,
const Vec3e& x1,
const Vec3e& x2,
const Vec3e& x3,
const Vec3e& x4,
double* alpha0,
double* alpha1,
double* alpha2,
double* alpha3,
double* alpha4);
int degenerate_edge_triangle_intersection(const Vec3Interval& x0,
const Vec3Interval& x1,
const Vec3Interval& x2,
const Vec3Interval& x3,
const Vec3Interval& x4,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3,
double* out_alpha4);
template<class T>
int edge_triangle_intersection(const Vec<3,T>& x0,
const Vec<3,T>& x1,
const Vec<3,T>& x2,
const Vec<3,T>& x3,
const Vec<3,T>& x4,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3,
double* out_alpha4 );
bool edge_triangle_intersection(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec3b& ts, const Vec3b& us, const Vec3b& vs,
const Vec3d& d_s0, const Vec3d& d_s1,
double* alphas );
template<class T>
T plane_dist( const Vec<3,T>& x, const Vec<3,T>& q, const Vec<3,T>& r, const Vec<3,T>& p );
template<class T>
void implicit_surface_function( const Vec<3,T>& x, const Vec<3,T>& q0, const Vec<3,T>& q1, const Vec<3,T>& q2, const Vec<3,T>& q3, T& out );
bool test_with_triangles(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec4b& ts, const Vec4b& us, const Vec4b& vs,
const Vec3d& s0, const Vec3d& s1,
bool use_positive_triangles, bool& edge_hit );
//
// Local function definitions
//
// --------------------------------------------------------
template<unsigned int N, class T>
inline void make_vector( const Vec<N,double>& in, Vec<N,T>& out )
{
for ( unsigned int i = 0; i < N; ++i )
{
create_from_double( in[i], out[i] );
}
}
// --------------------------------------------------------
template<class T>
inline void edge_edge_collision_function(const Vec3d &d_x0, const Vec3d &d_x1, const Vec3d &d_x2, const Vec3d &d_x3,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool b_t, bool b_u, bool b_v,
Vec<3,T>& out )
{
Vec<3,T> x0, x1, x2, x3;
make_vector( d_x0, x0 );
make_vector( d_x1, x1 );
make_vector( d_x2, x2 );
make_vector( d_x3, x3 );
out = x0;
out -= x2;
if ( b_t )
{
Vec<3,T> x0new, x1new, x2new, x3new;
make_vector( d_x0new, x0new );
make_vector( d_x1new, x1new );
make_vector( d_x2new, x2new );
make_vector( d_x3new, x3new );
out += x0new;
out -= x0;
out -= x2new;
out += x2;
if ( b_u )
{
out += x0;
out -= x0new;
out += x1new;
out -= x1;
}
if ( b_v )
{
out += x2new;
out -= x2;
out -= x3new;
out += x3;
}
}
if ( b_u )
{
out += x1;
out -= x0;
}
if ( b_v )
{
out += x2;
out -= x3;
}
}
/// --------------------------------------------------------
inline void add( Vec3d& a, const Vec3d& b )
{
a[0] += b[0];
a[1] += b[1];
a[2] += b[2];
}
/// --------------------------------------------------------
inline void sub( Vec3d& a, const Vec3d& b )
{
a[0] -= b[0];
a[1] -= b[1];
a[2] -= b[2];
}
// --------------------------------------------------------
inline void edge_edge_collision_function(const Vec3d &d_x0, const Vec3d &d_x1, const Vec3d &d_x2, const Vec3d &d_x3,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool b_t, bool b_u, bool b_v,
Vec<3,IntervalType>& out )
{
Vec3d out_lower( -d_x0[0], -d_x0[1], -d_x0[2] );
add( out_lower, d_x2 );
Vec3d out_upper( d_x0[0], d_x0[1], d_x0[2] );
sub( out_upper, d_x2 );
if ( b_t )
{
sub( out_lower, d_x0new );
add( out_lower, d_x0 );
add( out_lower, d_x2new );
sub( out_lower, d_x2 );
add( out_upper, d_x0new );
sub( out_upper, d_x0 );
sub( out_upper, d_x2new );
add( out_upper, d_x2 );
if ( b_u )
{
sub( out_lower, d_x0 );
add( out_lower, d_x0new );
sub( out_lower, d_x1new );
add( out_lower, d_x1 );
add( out_upper, d_x0 );
sub( out_upper, d_x0new );
add( out_upper, d_x1new );
sub( out_upper, d_x1 );
}
if ( b_v )
{
sub( out_lower, d_x2new );
add( out_lower, d_x2 );
add( out_lower, d_x3new );
sub( out_lower, d_x3 );
add( out_upper, d_x2new );
sub( out_upper, d_x2 );
sub( out_upper, d_x3new );
add( out_upper, d_x3 );
}
}
if ( b_u )
{
sub( out_lower, d_x1 );
add( out_lower, d_x0 );
add( out_upper, d_x1 );
sub( out_upper, d_x0 );
}
if ( b_v )
{
sub( out_lower, d_x2 );
add( out_lower, d_x3 );
add( out_upper, d_x2 );
sub( out_upper, d_x3 );
}
out[0].v[0] = out_lower[0];
out[1].v[0] = out_lower[1];
out[2].v[0] = out_lower[2];
out[0].v[1] = out_upper[0];
out[1].v[1] = out_upper[1];
out[2].v[1] = out_upper[2];
}
/// --------------------------------------------------------
template<class T>
void point_triangle_collision_function(const Vec3d &d_x0, const Vec3d &d_x1, const Vec3d &d_x2, const Vec3d &d_x3,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool b_t, bool b_u, bool b_v,
Vec<3,T>& out )
{
Vec<3,T> x0, x1, x2, x3;
make_vector( d_x0, x0 );
make_vector( d_x1, x1 );
make_vector( d_x2, x2 );
make_vector( d_x3, x3 );
out = x0;
out -= x1;
if ( b_t )
{
Vec<3,T> x0new, x1new, x2new, x3new;
make_vector( d_x0new, x0new );
make_vector( d_x1new, x1new );
make_vector( d_x2new, x2new );
make_vector( d_x3new, x3new );
out += x0new;
out -= x0;
out -= x1new;
out += x1;
if ( b_u )
{
out += x1new;
out -= x1;
out -= x2new;
out += x2;
}
if ( b_v )
{
out += x1new;
out -= x1;
out -= x3new;
out += x3;
}
}
if ( b_u )
{
out += x1;
out -= x2;
}
if ( b_v )
{
out += x1;
out -= x3;
}
}
/// --------------------------------------------------------
void point_triangle_collision_function(const Vec3d &d_x0, const Vec3d &d_x1, const Vec3d &d_x2, const Vec3d &d_x3,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool b_t, bool b_u, bool b_v,
Vec<3,IntervalType>& out )
{
Vec3d out_lower( -d_x0[0], -d_x0[1], -d_x0[2] );
Vec3d out_upper( d_x0[0], d_x0[1], d_x0[2] );
add( out_lower, d_x1 );
sub( out_upper, d_x1 );
if ( b_t )
{
sub( out_lower, d_x0new );
add( out_lower, d_x0 );
add( out_lower, d_x1new );
sub( out_lower, d_x1 );
add( out_upper, d_x0new );
sub( out_upper, d_x0 );
sub( out_upper, d_x1new );
add( out_upper, d_x1 );
if ( b_u )
{
sub( out_lower, d_x1new );
add( out_lower, d_x1 );
add( out_lower, d_x2new );
sub( out_lower, d_x2 );
add( out_upper, d_x1new );
sub( out_upper, d_x1 );
sub( out_upper, d_x2new );
add( out_upper, d_x2 );
}
if ( b_v )
{
sub( out_lower, d_x1new );
add( out_lower, d_x1 );
add( out_lower, d_x3new );
sub( out_lower, d_x3 );
add( out_upper, d_x1new );
sub( out_upper, d_x1 );
sub( out_upper, d_x3new );
add( out_upper, d_x3 );
}
}
if ( b_u )
{
sub( out_lower, d_x1 );
add( out_lower, d_x2 );
add( out_upper, d_x1 );
sub( out_upper, d_x2 );
}
if ( b_v )
{
sub( out_lower, d_x1 );
add( out_lower, d_x3 );
add( out_upper, d_x1 );
sub( out_upper, d_x3 );
}
out[0].v[0] = out_lower[0];
out[1].v[0] = out_lower[1];
out[2].v[0] = out_lower[2];
out[0].v[1] = out_upper[0];
out[1].v[1] = out_upper[1];
out[2].v[1] = out_upper[2];
}
// --------------------------------------------------------
template<class T>
void get_quad_vertices(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec4b& ts, const Vec4b& us, const Vec4b& vs,
Vec<3,T>& q0, Vec<3,T>& q1, Vec<3,T>& q2, Vec<3,T>& q3 )
{
T::begin_special_arithmetic();
if ( is_edge_edge )
{
edge_edge_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[0], us[0], vs[0], q0 );
edge_edge_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[1], us[1], vs[1], q1 );
edge_edge_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[2], us[2], vs[2], q2 );
edge_edge_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[3], us[3], vs[3], q3 );
}
else
{
point_triangle_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[0], us[0], vs[0], q0 );
point_triangle_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[1], us[1], vs[1], q1 );
point_triangle_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[2], us[2], vs[2], q2 );
point_triangle_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[3], us[3], vs[3], q3 );
}
T::end_special_arithmetic();
}
// --------------------------------------------------------
template<class T>
void get_triangle_vertices(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec3b& ts, const Vec3b& us, const Vec3b& vs,
Vec<3,T>& q0, Vec<3,T>& q1, Vec<3,T>& q2 )
{
T::begin_special_arithmetic();
if ( is_edge_edge )
{
edge_edge_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[0], us[0], vs[0], q0 );
edge_edge_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[1], us[1], vs[1], q1 );
edge_edge_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[2], us[2], vs[2], q2 );
}
else
{
point_triangle_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[0], us[0], vs[0], q0 );
point_triangle_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[1], us[1], vs[1], q1 );
point_triangle_collision_function( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, ts[2], us[2], vs[2], q2 );
}
T::end_special_arithmetic();
}
// ----------------------------------------
template<class T>
static T orientation2d(const Vec<2,T>& x0,
const Vec<2,T>& x1,
const Vec<2,T>& x2)
{
return x0[0]*x1[1] + -(x0[0])*x2[1]
+ x1[0]*x2[1] + -(x1[0])*x0[1]
+ x2[0]*x0[1] + -(x2[0])*x1[1];
}
// ----------------------------------------
inline expansion orientation2d(const Vec<2,expansion>& x0,
const Vec<2,expansion>& x1,
const Vec<2,expansion>& x2)
{
expansion prod;
expansion result;
multiply( x0[0], x1[1], prod );
add( result, prod, result );
multiply( x0[0], x2[1], prod );
subtract( result, prod, result );
multiply( x1[0], x2[1], prod );
add( result, prod, result );
multiply( x1[0], x0[1], prod );
subtract( result, prod, result );
multiply( x2[0], x0[1], prod );
add( result, prod, result );
multiply( x2[0], x1[1], prod );
subtract( result, prod, result );
return result;
}
// --------------------------------------------------------
template<class T>
void orientation3d(const Vec<3,T>& x0,
const Vec<3,T>& x1,
const Vec<3,T>& x2,
const Vec<3,T>& x3,
T& result )
{
// avoid recomputing common factors
T x00x11 = x0[0] * x1[1];
T x00x21 = x0[0] * x2[1];
T x00x31 = x0[0] * x3[1];
T x10x01 = x1[0] * x0[1];
T x10x21 = x1[0] * x2[1];
T x10x31 = x1[0] * x3[1];
T x20x01 = x2[0] * x0[1];
T x20x11 = x2[0] * x1[1];
T x20x31 = x2[0] * x3[1];
T x30x01 = x3[0] * x0[1];
T x30x11 = x3[0] * x1[1];
T x30x21 = x3[0] * x2[1];
result = ( x00x11 * ( x2[2] - x3[2] ) )
+ ( x00x21 * ( x3[2] - x1[2] ) )
+ ( x00x31 * ( x1[2] - x2[2] ) )
+ ( x10x01 * ( x3[2] - x2[2] ) )
+ ( x10x21 * ( x0[2] - x3[2] ) )
+ ( x10x31 * ( x2[2] - x0[2] ) )
+ ( x20x01 * ( x1[2]- x3[2] ) )
+ ( x20x11 * ( x3[2] - x0[2] ) )
+ ( x20x31 * ( x0[2] - x1[2] ) )
+ ( x30x01 * ( x2[2] - x1[2] ) )
+ ( x30x11 * ( x0[2] - x2[2] ) )
+ ( x30x21 * ( x1[2] - x0[2] ) );
}
// --------------------------------------------------------
inline void orientation3d(const Vec<3,expansion>& x0,
const Vec<3,expansion>& x1,
const Vec<3,expansion>& x2,
const Vec<3,expansion>& x3,
expansion& result )
{
// avoid recomputing common factors
expansion x00x11;
multiply( x0[0], x1[1], x00x11 );
expansion x00x21;
multiply( x0[0], x2[1], x00x21 );
expansion x00x31;
multiply( x0[0], x3[1], x00x31 );
expansion x10x01;
multiply( x1[0], x0[1], x10x01 );
expansion x10x21;
multiply( x1[0], x2[1], x10x21 );
expansion x10x31;
multiply( x1[0], x3[1], x10x31 );
expansion x20x01;
multiply( x2[0], x0[1], x20x01 );
expansion x20x11;
multiply( x2[0], x1[1], x20x11 );
expansion x20x31;
multiply( x2[0], x3[1], x20x31 );
expansion x30x01;
multiply( x3[0], x0[1], x30x01 );
expansion x30x11;
multiply( x3[0], x1[1], x30x11 );
expansion x30x21;
multiply( x3[0], x2[1], x30x21 );
expansion diff;
subtract( x2[2], x3[2], diff );
multiply( x00x11, diff, result );
expansion prod;
subtract( x3[2], x1[2], diff );
multiply( x00x21, diff, prod );
add( result, prod, result );
subtract( x1[2], x2[2], diff );
multiply( x00x31, diff, prod );
add( result, prod, result );
subtract( x3[2], x2[2], diff );
multiply( x10x01, diff, prod );
add( result, prod, result );
subtract( x0[2], x3[2], diff );
multiply( x10x21, diff, prod );
add( result, prod, result );
subtract( x2[2], x0[2], diff );
multiply( x10x31, diff, prod );
add( result, prod, result );
subtract( x1[2], x3[2], diff );
multiply( x20x01, diff, prod );
add( result, prod, result );
subtract( x3[2], x0[2], diff );
multiply( x20x11, diff, prod );
add( result, prod, result );
subtract( x0[2], x1[2], diff );
multiply( x20x31, diff, prod );
add( result, prod, result );
subtract( x2[2], x1[2], diff );
multiply( x30x01, diff, prod );
add( result, prod, result );
subtract( x0[2], x2[2], diff );
multiply( x30x11, diff, prod );
add( result, prod, result );
subtract( x1[2], x0[2], diff );
multiply( x30x21, diff, prod );
add( result, prod, result );
}
// ----------------------------------------
int expansion_simplex_intersection1d(int k,
const expansion& x0,
const expansion& x1,
const expansion& x2,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2)
{
assert( k == 1 );
assert(out_alpha0 && out_alpha1 && out_alpha2);
assert(fegetround()== FE_TONEAREST);
if( sign(x1-x2) < 0 )
{
if( sign(x0-x1) < 0) return 0;
else if ( sign(x0-x2) > 0 ) return 0;
*out_alpha0=1;
*out_alpha1=(x2.estimate()-x0.estimate())/(x2.estimate()-x1.estimate());
*out_alpha2=(x0.estimate()-x1.estimate())/(x2.estimate()-x1.estimate());
return 1;
}
else if( sign(x1-x2) > 0 )
{
if( sign(x0-x2) < 0 ) return 0;
else if( sign(x0-x1) > 0) return 0;
*out_alpha0=1;
*out_alpha1=(x2.estimate()-x0.estimate())/(x2.estimate()-x1.estimate());
*out_alpha2=(x0.estimate()-x1.estimate())/(x2.estimate()-x1.estimate());
return 1;
}
else
{
// x1 == x2
if( sign(x0-x1) != 0 ) return 0;
*out_alpha0=1;
*out_alpha1=0.5;
*out_alpha2=0.5;
return 1;
}
}
// ----------------------------------------
int
expansion_simplex_intersection2d(int k,
const Vec2e& x0,
const Vec2e& x1,
const Vec2e& x2,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2 )
{
assert(k==1);
assert(fegetround()== FE_TONEAREST);
// try projecting each coordinate out in turn
double ax0, ax1, ax2;
if(!expansion_simplex_intersection1d(1, x0[1], x1[1], x2[1], &ax0, &ax1, &ax2)) return 0;
double ay0, ay1, ay2;
if(!expansion_simplex_intersection1d(1, x0[0], x1[0], x2[0], &ay0, &ay1, &ay2)) return 0;
// decide which solution is more accurate for barycentric coordinates
double checkx=std::fabs(-ax0*x0[0].estimate()+ax1*x1[0].estimate()+ax2*x2[0].estimate())
+std::fabs(-ax0*x0[1].estimate()+ax1*x1[1].estimate()+ax2*x2[1].estimate());
double checky=std::fabs(-ay0*x0[0].estimate()+ay1*x1[0].estimate()+ay2*x2[0].estimate())
+std::fabs(-ay0*x0[1].estimate()+ay1*x1[1].estimate()+ay2*x2[1].estimate());
if(checkx<=checky)
{
*out_alpha0=ax0;
*out_alpha1=ax1;
*out_alpha2=ax2;
}
else
{
*out_alpha0=ay0;
*out_alpha1=ay1;
*out_alpha2=ay2;
}
return 1;
}
// ----------------------------------------
int expansion_simplex_intersection2d(int k,
const Vec2e& x0,
const Vec2e& x1,
const Vec2e& x2,
const Vec2e& x3,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3)
{
assert(1<=k && k<=3);
assert(fegetround()== FE_TONEAREST);
switch(k)
{
case 1: // point vs. triangle
{
expansion alpha1 = -orientation2d(x0, x2, x3);
expansion alpha2 = orientation2d(x0, x1, x3);
if(certainly_opposite_sign(alpha1, alpha2)) return 0;
expansion alpha3 = -orientation2d(x0, x1, x2);
if(certainly_opposite_sign(alpha1, alpha3)) return 0;
if(certainly_opposite_sign(alpha2, alpha3)) return 0;
double sum2 = alpha1.estimate() + alpha2.estimate() + alpha3.estimate();
if(sum2)
{
*out_alpha0=1;
*out_alpha1 = alpha1.estimate() / sum2;
*out_alpha2 = alpha2.estimate() / sum2;
*out_alpha3 = alpha3.estimate() / sum2;
return 1;
}
else
{
// triangle is degenerate and point lies on same line
if(expansion_simplex_intersection2d(1, x0, x1, x2, out_alpha0, out_alpha1, out_alpha2))
{
*out_alpha3=0;
return 1;
}
if(expansion_simplex_intersection2d(1, x0, x1, x3, out_alpha0, out_alpha1, out_alpha3))
{
*out_alpha2=0;
return 1;
}
if(expansion_simplex_intersection2d(1, x0, x2, x3, out_alpha0, out_alpha2, out_alpha3))
{
*out_alpha1=0;
return 1;
}
return 0;
}
}
case 2: // segment vs. segment
{
expansion alpha0 = orientation2d(x1, x2, x3);
expansion alpha1 = -orientation2d(x0, x2, x3);
if( certainly_opposite_sign(alpha0, alpha1) ) return 0;
expansion alpha2 = orientation2d(x0, x1, x3);
expansion alpha3 = -orientation2d(x0, x1, x2);
if( certainly_opposite_sign(alpha2, alpha3) ) return 0;
double sum1, sum2;
sum1= alpha0.estimate() + alpha1.estimate();
sum2= alpha2.estimate() + alpha3.estimate();
if(sum1 && sum2){
*out_alpha0 = alpha0.estimate() / sum1;
*out_alpha1 = alpha1.estimate() / sum1;
*out_alpha2 = alpha2.estimate() / sum2;
*out_alpha3 = alpha3.estimate() / sum2;
return 1;
}
else
{
// degenerate: segments lie on the same line
if(expansion_simplex_intersection2d(1, x0, x2, x3, out_alpha0, out_alpha2, out_alpha3)){
*out_alpha1=0;
return 1;
}
if(expansion_simplex_intersection2d(1, x1, x2, x3, out_alpha1, out_alpha2, out_alpha3)){
*out_alpha0=0;
return 1;
}
if(expansion_simplex_intersection2d(1, x2, x0, x1, out_alpha2, out_alpha0, out_alpha1)){
*out_alpha3=0;
return 1;
}
if(expansion_simplex_intersection2d(1, x3, x0, x1, out_alpha3, out_alpha0, out_alpha1)){
*out_alpha2=0;
return 1;
}
return 0;
}
}
break;
default:
assert(0);
return -1; // should never get here
}
}
// --------------------------------------------------------
// degenerate test in 3d - assumes four points lie on the same plane
bool expansion_simplex_intersection3d(int k,
const Vec3e& x0,
const Vec3e& x1,
const Vec3e& x2,
const Vec3e& x3,
double* alpha0,
double* alpha1,
double* alpha2,
double* )
{
assert(k<=2);
assert(fegetround()== FE_TONEAREST);
// try projecting each coordinate out in turn
double ax0, ax1, ax2, ax3;
if( !expansion_simplex_intersection2d(k, Vec2e(x0[1],x0[2]), Vec2e(x1[1],x1[2]), Vec2e(x2[1],x2[2]), Vec2e(x3[1],x3[2]), &ax0, &ax1, &ax2,&ax3) )
{
return 0;
}
double ay0, ay1, ay2, ay3;
Vec2e p0( x0[0], x0[2] );
Vec2e p1( x1[0], x1[2] );
Vec2e p2( x2[0], x2[2] );
Vec2e p3( x3[0], x3[2] );
if ( !expansion_simplex_intersection2d(k, p0, p1, p2, p3, &ay0, &ay1, &ay2, &ay3) )
{
return 0;
}
double az0, az1, az2, az3;
if( !expansion_simplex_intersection2d(k, Vec2e(x0[0],x0[1]), Vec2e(x1[0],x1[1]), Vec2e(x2[0],x2[1]), Vec2e(x3[0],x3[1]), &az0, &az1, &az2, &az3) )
{
return 0;
}
// decide which solution is more accurate for barycentric coordinates
double checkx, checky, checkz;
Vec3d estx0( x0[0].estimate(), x0[1].estimate(), x0[2].estimate() );
Vec3d estx1( x1[0].estimate(), x1[1].estimate(), x1[2].estimate() );
Vec3d estx2( x2[0].estimate(), x2[1].estimate(), x2[2].estimate() );
Vec3d estx3( x3[0].estimate(), x3[1].estimate(), x3[2].estimate() );
if(k==1)
{
checkx=std::fabs(-ax0*estx0[0]+ax1*estx1[0]+ax2*estx2[0]+ax3*estx3[0])
+std::fabs(-ax0*estx0[1]+ax1*estx1[1]+ax2*estx2[1]+ax3*estx3[1])
+std::fabs(-ax0*estx0[2]+ax1*estx1[2]+ax2*estx2[2]+ax3*estx3[2]);
checky=std::fabs(-ay0*estx0[0]+ay1*estx1[0]+ay2*estx2[0]+ay3*estx3[0])
+std::fabs(-ay0*estx0[1]+ay1*estx1[1]+ay2*estx2[1]+ay3*estx3[1])
+std::fabs(-ay0*estx0[2]+ay1*estx1[2]+ay2*estx2[2]+ay3*estx3[2]);
checkz=std::fabs(-az0*estx0[0]+az1*estx1[0]+az2*estx2[0]+az3*estx3[0])
+std::fabs(-az0*estx0[1]+az1*estx1[1]+az2*estx2[1]+az3*estx3[1])
+std::fabs(-az0*estx0[2]+az1*estx1[2]+az2*estx2[2]+az3*estx3[2]);
}
else
{
checkx=std::fabs(-ax0*estx0[0]-ax1*estx1[0]+ax2*estx2[0]+ax3*estx3[0])
+std::fabs(-ax0*estx0[1]-ax1*estx1[1]+ax2*estx2[1]+ax3*estx3[1])
+std::fabs(-ax0*estx0[2]-ax1*estx1[2]+ax2*estx2[2]+ax3*estx3[2]);
checky=std::fabs(-ay0*estx0[0]-ay1*estx1[0]+ay2*estx2[0]+ay3*estx3[0])
+std::fabs(-ay0*estx0[1]-ay1*estx1[1]+ay2*estx2[1]+ay3*estx3[1])
+std::fabs(-ay0*estx0[2]-ay1*estx1[2]+ay2*estx2[2]+ay3*estx3[2]);
checkz=std::fabs(-az0*estx0[0]-az1*estx1[0]+az2*estx2[0]+az3*estx3[0])
+std::fabs(-az0*estx0[1]-az1*estx1[1]+az2*estx2[1]+az3*estx3[1])
+std::fabs(-az0*estx0[2]-az1*estx1[2]+az2*estx2[2]+az3*estx3[2]);
}
if(checkx<=checky && checkx<=checkz)
{
*alpha0=ax0;
*alpha1=ax1;
*alpha2=ax2;
*alpha2=ax3;
}
else if(checky<=checkz)
{
*alpha0=ay0;
*alpha1=ay1;
*alpha2=ay2;
*alpha2=ay3;
}
else
{
*alpha0=az0;
*alpha1=az1;
*alpha2=az2;
*alpha2=az3;
}
return 1;
}
// --------------------------------------------------------
int degenerate_point_tetrahedron_intersection(const Vec3e& x0,
const Vec3e& x1,
const Vec3e& x2,
const Vec3e& x3,
const Vec3e& x4,
double* alpha0,
double* alpha1,
double* alpha2,
double* alpha3,
double* alpha4)
{
assert(fegetround()== FE_TONEAREST);
// degenerate: point and tetrahedron in same plane
if (expansion_simplex_intersection3d(1, x0, x2, x3, x4, alpha0, alpha2, alpha3, alpha4))
{
*alpha1=0;
return 1;
}
if(expansion_simplex_intersection3d(1, x0, x1, x3, x4, alpha0, alpha1, alpha3, alpha4))
{
*alpha2=0;
return 1;
}
if(expansion_simplex_intersection3d(1, x0, x1, x2, x4, alpha0, alpha1, alpha2, alpha4))
{
*alpha3=0;
return 1;
}
if(expansion_simplex_intersection3d(1, x0, x1, x2, x3, alpha0, alpha1, alpha2, alpha3))
{
*alpha4=0;
return 1;
}
return 0;
}
// --------------------------------------------------------
int degenerate_point_tetrahedron_intersection(const Vec3Interval& ,
const Vec3Interval& ,
const Vec3Interval& ,
const Vec3Interval& ,
const Vec3Interval& ,
double* ,
double* ,
double* ,
double* ,
double* )
{
return -1;
}
// --------------------------------------------------------
template<class T>
int point_tetrahedron_intersection(const Vec<3,T>& x0,
const Vec<3,T>& x1,
const Vec<3,T>& x2,
const Vec<3,T>& x3,
const Vec<3,T>& x4,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3,
double* out_alpha4 )
{
T::begin_special_arithmetic();
T alpha1;
orientation3d(x0, x2, x3, x4, alpha1);
alpha1 = -alpha1;
T alpha2;
orientation3d(x0, x1, x3, x4, alpha2);
if( certainly_opposite_sign(alpha1, alpha2) )
{
T::end_special_arithmetic();
return 0;
}
T alpha3;
orientation3d(x0, x1, x2, x4, alpha3);
alpha3 = -alpha3;
if( certainly_opposite_sign(alpha1, alpha3) )
{
T::end_special_arithmetic();
return 0;
}
if( certainly_opposite_sign(alpha2, alpha3) )
{
T::end_special_arithmetic();
return 0;
}
T alpha4;
orientation3d(x0, x1, x2, x3, alpha4);
T::end_special_arithmetic();
if( certainly_opposite_sign(alpha1, alpha4) ) return 0;
if( certainly_opposite_sign(alpha2, alpha4) ) return 0;
if( certainly_opposite_sign(alpha3, alpha4) ) return 0;
if ( alpha1.indefinite_sign() || alpha2.indefinite_sign() || alpha3.indefinite_sign() || alpha4.indefinite_sign() )
{
// degenerate
return -1;
}
double sum2 = alpha1.estimate() + alpha2.estimate() + alpha3.estimate() + alpha4.estimate();
if(sum2)
{
*out_alpha0 = 1;
*out_alpha1 = alpha1.estimate() / sum2;
*out_alpha2 = alpha2.estimate() / sum2;
*out_alpha3 = alpha3.estimate() / sum2;
*out_alpha4 = alpha4.estimate() / sum2;
return 1;
}
else
{
// If T is IntervalType, returns -1
// If T is expansion, computes exact intersection by projecting to lower dimensions.
return degenerate_point_tetrahedron_intersection( x0, x1, x2, x3, x4, out_alpha0, out_alpha1, out_alpha2, out_alpha3, out_alpha4 );
}
}
// --------------------------------------------------------
int degenerate_edge_triangle_intersection(const Vec3e& x0,
const Vec3e& x1,
const Vec3e& x2,
const Vec3e& x3,
const Vec3e& x4,
double* alpha0,
double* alpha1,
double* alpha2,
double* alpha3,
double* alpha4)
{
// degenerate: segment and triangle in same plane
if(expansion_simplex_intersection3d(1, x1, x2, x3, x4, alpha1, alpha2, alpha3, alpha4)){
*alpha0=0;
return 1;
}
if(expansion_simplex_intersection3d(1, x0, x2, x3, x4, alpha0, alpha2, alpha3, alpha4)){
*alpha1=0;
return 1;
}
if(expansion_simplex_intersection3d(2, x0, x1, x3, x4, alpha0, alpha1, alpha3, alpha4)){
*alpha2=0;
return 1;
}
if(expansion_simplex_intersection3d(2, x0, x1, x2, x4, alpha0, alpha1, alpha2, alpha4)){
*alpha3=0;
return 1;
}
if(expansion_simplex_intersection3d(2, x0, x1, x2, x3, alpha0, alpha1, alpha2, alpha3)){
*alpha4=0;
return 1;
}
return 0;
}
// --------------------------------------------------------
int degenerate_edge_triangle_intersection(const Vec3Interval& ,
const Vec3Interval& ,
const Vec3Interval& ,
const Vec3Interval& ,
const Vec3Interval& ,
double* ,
double* ,
double* ,
double* ,
double* )
{
return -1;
}
// --------------------------------------------------------
template<class T>
int edge_triangle_intersection(const Vec<3,T>& x0,
const Vec<3,T>& x1,
const Vec<3,T>& x2,
const Vec<3,T>& x3,
const Vec<3,T>& x4,
double* out_alpha0,
double* out_alpha1,
double* out_alpha2,
double* out_alpha3,
double* out_alpha4 )
{
T::begin_special_arithmetic();
T alpha0;
orientation3d(x1, x2, x3, x4,alpha0);
T alpha1;
orientation3d(x0, x2, x3, x4,alpha1);
alpha1 = -alpha1;
if( certainly_opposite_sign(alpha0, alpha1) )
{
T::end_special_arithmetic();
return 0;
}
T alpha2;
orientation3d(x0, x1, x3, x4, alpha2);
T alpha3;
orientation3d(x0, x1, x2, x4, alpha3);
alpha3 = -alpha3;
if( certainly_opposite_sign(alpha2, alpha3) )
{
T::end_special_arithmetic();
return 0;
}
T alpha4;
orientation3d(x0, x1, x2, x3, alpha4);
if( certainly_opposite_sign(alpha2, alpha4) )
{
T::end_special_arithmetic();
return 0;
}
if( certainly_opposite_sign(alpha3, alpha4) )
{
T::end_special_arithmetic();
return 0;
}
T::end_special_arithmetic();
if ( alpha0.indefinite_sign() || alpha1.indefinite_sign() || alpha2.indefinite_sign() || alpha3.indefinite_sign() || alpha4.indefinite_sign() )
{
// degenerate
return -1;
}
double sum1 = alpha0.estimate() + alpha1.estimate();
double sum2 = alpha2.estimate() + alpha3.estimate() + alpha4.estimate();
if(sum1 && sum2)
{
*out_alpha0 = alpha0.estimate() / sum1;
*out_alpha1 = alpha1.estimate() / sum1;
*out_alpha2 = alpha2.estimate() / sum2;
*out_alpha3 = alpha3.estimate() / sum2;
*out_alpha4 = alpha4.estimate() / sum2;
return 1;
}
else
{
// If T is IntervalType, returns -1
// If T is expansion, computes exact intersection by projecting to lower dimensions.
return degenerate_edge_triangle_intersection( x0, x1, x2, x3, x4, out_alpha0, out_alpha1, out_alpha2, out_alpha3, out_alpha4 );
}
}
// ----------------------------------------
bool edge_triangle_intersection(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec3b& ts, const Vec3b& us, const Vec3b& vs,
const Vec3d& d_s0, const Vec3d& d_s1,
double* alphas )
{
// TODO: These should be cached already
Vec3Interval t0, t1, t2;
get_triangle_vertices( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, ts, us, vs, t0, t1, t2 );
Vec3Interval s0, s1;
make_vector( d_s0, s0 );
make_vector( d_s1, s1 );
int result = edge_triangle_intersection( s0, s1, t0, t1, t2, &alphas[0], &alphas[1], &alphas[2], &alphas[3], &alphas[4] );
// If degenerate, repeat test using expansion arithmetric
if ( result < 0 )
{
// TODO: These might be cached already
Vec3e exp_t0, exp_t1, exp_t2;
get_triangle_vertices( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, ts, us, vs, exp_t0, exp_t1, exp_t2 );
Vec3e exp_s0, exp_s1;
make_vector( d_s0, exp_s0 );
make_vector( d_s1, exp_s1 );
result = edge_triangle_intersection( exp_s0, exp_s1, exp_t0, exp_t1, exp_t2, &alphas[0], &alphas[1], &alphas[2], &alphas[3], &alphas[4] );
}
if ( result == 0 )
{
return false;
}
return true;
}
// ----------------------------------------
template<class T>
T plane_dist( const Vec<3,T>& x, const Vec<3,T>& q, const Vec<3,T>& r, const Vec<3,T>& p )
{
return dot( x-p, cross( q-p, r-p ) );
}
// ----------------------------------------
template<class T>
void implicit_surface_function( const Vec<3,T>& x, const Vec<3,T>& q0, const Vec<3,T>& q1, const Vec<3,T>& q2, const Vec<3,T>& q3, T& out )
{
T::begin_special_arithmetic();
T g012 = plane_dist( x, q0, q1, q2 );
T g132 = plane_dist( x, q1, q3, q2 );
T h12 = g012 * g132;
T g013 = plane_dist( x, q0, q1, q3 );
T g032 = plane_dist( x, q0, q3, q2 );
T h03 = g013 * g032;
out = h12 - h03;
T::end_special_arithmetic();
}
// ----------------------------------------
bool test_with_triangles(const Vec3d &d_x0old, const Vec3d &d_x1old, const Vec3d &d_x2old, const Vec3d &d_x3old,
const Vec3d &d_x0new, const Vec3d &d_x1new, const Vec3d &d_x2new, const Vec3d &d_x3new,
bool is_edge_edge,
const Vec4b& ts, const Vec4b& us, const Vec4b& vs,
const Vec3d& s0, const Vec3d& s1,
bool use_positive_triangles, bool& edge_hit )
{
// determine which two triangles are on the positive side of f
// first try evaluating sign using interval arithmetic
int sign_h12 = -2;
{
Vec3Interval q0, q1, q2, q3;
// TODO: These should be cached already
get_quad_vertices( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, ts, us, vs, q0, q1, q2, q3 );
IntervalType::begin_special_arithmetic();
Vec3Interval x = IntervalType(0.5) * ( q0 + q3 );
IntervalType g012 = plane_dist( x, q0, q1, q2 );
IntervalType g132 = plane_dist( x, q1, q3, q2 );
IntervalType h12 = g012 * g132;
IntervalType::end_special_arithmetic();
if ( h12.is_certainly_negative() ) { sign_h12 = -1; }
if ( h12.is_certainly_zero() ) { sign_h12 = 0; }
if ( h12.is_certainly_positive() ) { sign_h12 = 1; }
}
if ( sign_h12 == -2 )
{
// not sure about sign - compute using expansion
Vec3e eq0, eq1, eq2, eq3;
// TODO: These might be cached already
get_quad_vertices( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, ts, us, vs, eq0, eq1, eq2, eq3 );
Vec3e x = expansion( 0.5 ) * ( eq0 + eq3 );
expansion g012 = plane_dist( x, eq0, eq1, eq2 );
expansion g132 = plane_dist( x, eq1, eq3, eq2 );
expansion h12 = g012 * g132;
sign_h12 = sign( h12 );
}
if ( sign_h12 == 0 )
{
// use either pair of triangles
sign_h12 = 1;
}
if ( sign_h12 > 0 )
{
// positive side: 013, 032
// negative side: 012, 132
if ( use_positive_triangles )
{
double b[5] = { 0.5, 0.5, 0.5, 0.5, 0.5 };
Vec3b t013( ts[0], ts[1], ts[3] );
Vec3b u013( us[0], us[1], us[3] );
Vec3b v013( vs[0], vs[1], vs[3] );
bool hit_a = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t013, u013, v013, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
Vec3b t032( ts[0], ts[3], ts[2] );
Vec3b u032( us[0], us[3], us[2] );
Vec3b v032( vs[0], vs[3], vs[2] );
bool hit_b = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t032, u032, v032, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
return hit_a || hit_b;
}
else
{
double b[5] = { 0.5, 0.5, 0.5, 0.5, 0.5 };
Vec3b t012( ts[0], ts[1], ts[2] );
Vec3b u012( us[0], us[1], us[2] );
Vec3b v012( vs[0], vs[1], vs[2] );
bool hit_a = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t012, u012, v012, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
Vec3b t132( ts[1], ts[3], ts[2] );
Vec3b u132( us[1], us[3], us[2] );
Vec3b v132( vs[1], vs[3], vs[2] );
bool hit_b = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t132, u132, v132, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
return hit_a || hit_b;
}
}
else
{
// positive side: 012, 132
// negative side: 013, 032
if ( use_positive_triangles )
{
double b[5] = { 0.5, 0.5, 0.5, 0.5, 0.5 };
Vec3b t012( ts[0], ts[1], ts[2] );
Vec3b u012( us[0], us[1], us[2] );
Vec3b v012( vs[0], vs[1], vs[2] );
bool hit_a = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t012, u012, v012, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
Vec3b t132( ts[1], ts[3], ts[2] );
Vec3b u132( us[1], us[3], us[2] );
Vec3b v132( vs[1], vs[3], vs[2] );
bool hit_b = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t132, u132, v132, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
return hit_a || hit_b;
}
else
{
double b[5] = { 0.5, 0.5, 0.5, 0.5, 0.5 };
Vec3b t013( ts[0], ts[1], ts[3] );
Vec3b u013( us[0], us[1], us[3] );
Vec3b v013( vs[0], vs[1], vs[3] );
bool hit_a = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t013, u013, v013, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
Vec3b t032( ts[0], ts[3], ts[2] );
Vec3b u032( us[0], us[3], us[2] );
Vec3b v032( vs[0], vs[3], vs[2] );
bool hit_b = edge_triangle_intersection( d_x0old, d_x1old, d_x2old, d_x3old, d_x0new, d_x1new, d_x2new, d_x3new, is_edge_edge, t032, u032, v032, s0, s1, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
return hit_a || hit_b;
}
}
}
/// --------------------------------------------------------
inline int plane_sign( const Vec3d& n, const Vec3Interval& x )
{
IntervalType dist = IntervalType(n[0])*x[0] + IntervalType(n[1])*x[1] + IntervalType(n[2])*x[2];
if ( dist.is_certainly_negative() ) { return -1; }
else if ( dist.is_certainly_positive() ) { return 1; }
return 0;
}
/// --------------------------------------------------------
inline int quick_1_plane_sign( const Vec3i& normal, const Vec3Interval& x )
{
IntervalType dist( 0, 0 );
if ( normal[0] < 0 ) { dist -= x[0]; }
else if ( normal[0] > 0 ) { dist += x[0]; }
if ( normal[1] < 0 ) { dist -= x[1]; }
else if ( normal[1] > 0 ) { dist += x[1]; }
if ( normal[2] < 0 ) { dist -= x[2]; }
else if ( normal[2] > 0 ) { dist += x[2]; }
if ( dist.is_certainly_negative() ) { return -1; }
else if ( dist.is_certainly_positive() ) { return 1; }
return 0;
}
} // end unnamed namespace for local helper functions
//
// Member function definitions
//
// ----------------------------------------
///
/// Determine if the given point is inside the tetrahedron given by tet_indices
///
// ----------------------------------------
bool RootParityCollisionTest::point_tetrahedron_intersection( const Vec4ui& quad, const Vec4b& ts, const Vec4b& us, const Vec4b& vs, const Vec3d& d_x )
{
Vec3Interval x;
make_vector( d_x, x );
double s[5];
int result = rootparity::point_tetrahedron_intersection(x,
m_interval_hex_vertices[quad[0]],
m_interval_hex_vertices[quad[1]],
m_interval_hex_vertices[quad[2]],
m_interval_hex_vertices[quad[3]],
&s[0], &s[1], &s[2], &s[3], &s[4] );
// If degenerate, repeat test using expansion arithmetric
if ( result < 0 )
{
// Check if the expansion vertices have been computed already
for ( unsigned int i = 0; i < 4; ++i )
{
if ( !m_expansion_hex_vertices_computed[ quad[i] ] )
{
if ( m_is_edge_edge )
{
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, ts[i], us[i], vs[i], m_expansion_hex_vertices[ quad[i] ] );
}
else
{
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, ts[i], us[i], vs[i], m_expansion_hex_vertices[ quad[i] ] );
}
m_expansion_hex_vertices_computed[ quad[i] ] = true;
}
}
// now run the test in expansion arithmetic
Vec3e exp_x;
make_vector( d_x, exp_x );
result = rootparity::point_tetrahedron_intersection(exp_x,
m_expansion_hex_vertices[ quad[0] ],
m_expansion_hex_vertices[ quad[1] ],
m_expansion_hex_vertices[ quad[2] ],
m_expansion_hex_vertices[ quad[3] ],
&s[0], &s[1], &s[2], &s[3], &s[4] );
}
if ( result == 0 )
{
return false;
}
return true;
}
// ----------------------------------------
///
/// Determine if the given segment intersects the triangle
///
// ----------------------------------------
bool RootParityCollisionTest::edge_triangle_intersection( const Vec3ui& triangle,
const Vec3b& ts, const Vec3b& us, const Vec3b& vs,
const Vec3d& d_s0, const Vec3d& d_s1,
double* alphas )
{
const Vec3Interval& t0 = m_interval_hex_vertices[triangle[0]];
const Vec3Interval& t1 = m_interval_hex_vertices[triangle[1]];
const Vec3Interval& t2 = m_interval_hex_vertices[triangle[2]];
Vec3Interval s0, s1;
make_vector( d_s0, s0 );
make_vector( d_s1, s1 );
int result = rootparity::edge_triangle_intersection( s0, s1, t0, t1, t2, &alphas[0], &alphas[1], &alphas[2], &alphas[3], &alphas[4] );
// If degenerate, repeat test using expansion arithmetric
if ( result < 0 )
{
Vec3e exp_t0, exp_t1, exp_t2;
get_triangle_vertices( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, m_is_edge_edge, ts, us, vs, exp_t0, exp_t1, exp_t2 );
Vec3e exp_s0, exp_s1;
make_vector( d_s0, exp_s0 );
make_vector( d_s1, exp_s1 );
result = rootparity::edge_triangle_intersection( exp_s0, exp_s1, exp_t0, exp_t1, exp_t2, &alphas[0], &alphas[1], &alphas[2], &alphas[3], &alphas[4] );
}
if ( result == 0 )
{
return false;
}
return true;
}
// ----------------------------------------
///
/// Compute the sign of the implicit surface function which is zero on the bilinear patch defined by quad.
///
// ----------------------------------------
int RootParityCollisionTest::implicit_surface_function_sign( const Vec4ui& quad, const Vec4b& ts, const Vec4b& us, const Vec4b& vs, const Vec3d& d_x )
{
// first try evaluating sign using interval arithmetic
{
const Vec3Interval& q0 = m_interval_hex_vertices[quad[0]];
const Vec3Interval& q1 = m_interval_hex_vertices[quad[1]];
const Vec3Interval& q2 = m_interval_hex_vertices[quad[2]];
const Vec3Interval& q3 = m_interval_hex_vertices[quad[3]];
Vec3Interval x;
make_vector( d_x, x );
IntervalType f;
implicit_surface_function( x, q0, q1, q2, q3, f );
if ( f.is_certainly_negative() ) { return -1; }
if ( f.is_certainly_zero() ) { return 0; }
if ( f.is_certainly_positive() ) { return 1; }
}
// not sure about sign - compute using expansion
for ( unsigned int i = 0; i < 4; ++i )
{
if ( !m_expansion_hex_vertices_computed[ quad[i] ] )
{
if ( m_is_edge_edge )
{
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, ts[i], us[i], vs[i], m_expansion_hex_vertices[ quad[i] ] );
}
else
{
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, ts[i], us[i], vs[i], m_expansion_hex_vertices[ quad[i] ] );
}
m_expansion_hex_vertices_computed[ quad[i] ] = true;
}
}
Vec3e x;
make_vector( d_x, x );
expansion ef;
implicit_surface_function( x, m_expansion_hex_vertices[ quad[0] ], m_expansion_hex_vertices[ quad[1] ], m_expansion_hex_vertices[ quad[2] ], m_expansion_hex_vertices[ quad[3] ], ef );
return sign( ef );
}
// ----------------------------------------
///
/// Test the segment s0-s1 against the bilinear patch defined by quad to determine whether there is an even or odd number of
/// intersections. Returns true if odd.
///
// ----------------------------------------
bool RootParityCollisionTest::ray_quad_intersection_parity( const Vec4ui& quad,
const Vec4b& ts, const Vec4b& us, const Vec4b& vs,
const Vec3d& ray_origin, const Vec3d& ray_head,
bool& edge_hit, bool& origin_on_surface )
{
bool point_in_tet0 = point_tetrahedron_intersection( quad, ts, us, vs, ray_origin );
if ( point_in_tet0 )
{
int sign0 = implicit_surface_function_sign( quad, ts, us, vs, ray_origin );
if ( sign0 == 0 )
{
origin_on_surface = true;
return false;
}
// s0 is inside the tet, s1 is outside
if ( sign0 > 0 )
{
// use the triangles on the negative side of f()
return test_with_triangles( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, m_is_edge_edge, ts, us, vs, ray_origin, ray_head, false, edge_hit );
}
else
{
// use the triangles on the positive side of f()
return test_with_triangles( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, m_is_edge_edge, ts, us, vs, ray_origin, ray_head, true, edge_hit );
}
}
else
{
// s0 is outside the tet
// both outside
// use either set of triangles
bool hit_a, hit_b;
{
double b[5] = { 0.5, 0.5, 0.5, 0.5, 0.5 };
Vec3ui triangle013( quad[0], quad[1], quad[3] );
Vec3b t013( ts[0], ts[1], ts[3] );
Vec3b u013( us[0], us[1], us[3] );
Vec3b v013( vs[0], vs[1], vs[3] );
hit_a = edge_triangle_intersection( triangle013, t013, u013, v013, ray_origin, ray_head, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
}
{
double b[5] = { 0.5, 0.5, 0.5, 0.5, 0.5 };
Vec3ui triangle032( quad[0], quad[3], quad[2] );
Vec3b t032( ts[0], ts[3], ts[2] );
Vec3b u032( us[0], us[3], us[2] );
Vec3b v032( vs[0], vs[3], vs[2] );
hit_b = edge_triangle_intersection( triangle032, t032, u032, v032, ray_origin, ray_head, b );
if ( b[2] == 0.0 || b[3] == 0.0 || b[4] == 0.0 ) { edge_hit = true; }
if ( b[2] == 1.0 || b[3] == 1.0 || b[4] == 1.0 ) { edge_hit = true; }
}
return hit_a ^ hit_b;
}
assert( !"Should not get here" );
return true;
}
// --------------------------------------------------------
///
/// Determine the parity of the number of intersections between a ray from the origin and the generalized prism made up
/// of f(G) where G = the vertices of the domain boundary.
///
// --------------------------------------------------------
bool RootParityCollisionTest::ray_prism_parity_test()
{
Vec3d test_ray( m_ray );
double ray_len = mag( test_ray );
std::vector<Vec3ui> tris(2);
tris[0] = Vec3ui( 0, 1, 2 );
tris[1] = Vec3ui( 3, 4, 5 );
std::vector<Vec4ui> quads(3);
quads[0] = Vec4ui( 0, 1, 3, 4 );
quads[1] = Vec4ui( 1, 2, 4, 5 );
quads[2] = Vec4ui( 0, 2, 3, 5 );
const bool vertex_ts[6] = { 0, 0, 0, 1, 1, 1 };
const bool vertex_us[6] = { 0, 1, 0, 0, 1, 0 };
const bool vertex_vs[6] = { 0, 0, 1, 0, 0, 1 };
// for debugging purposes, store the result of each hit test
std::vector<bool> tri_hits( 2, false );
std::vector<bool> quad_hits( 3, false );
bool good_hit = false;
unsigned int num_tries = 0;
while (!good_hit && num_tries++ < 10 )
{
good_hit = true;
// ray-cast against each tri and each quad
for ( unsigned int i = 0; i < tris.size(); ++i )
{
const Vec3ui& t = tris[i];
double bary[5] = { 0.5, 0.5, 0.5, 0.5, 0.5 };
Vec3b ts( vertex_ts[t[0]], vertex_ts[t[1]], vertex_ts[t[2]] );
Vec3b us( vertex_us[t[0]], vertex_us[t[1]], vertex_us[t[2]] );
Vec3b vs( vertex_vs[t[0]], vertex_vs[t[1]], vertex_vs[t[2]] );
tri_hits[i] = rootparity::edge_triangle_intersection( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new, m_is_edge_edge, ts, us, vs, Vec3d(0,0,0), test_ray, bary );
if ( tri_hits[i] )
{
if ( bary[2] == 0.0 || bary[3] == 0.0 || bary[4] == 0.0 ) { good_hit = false; }
if ( bary[2] == 1.0 || bary[3] == 1.0 || bary[4] == 1.0 ) { good_hit = false; }
if ( bary[0] == 1.0 )
{
// origin is on surface
return true;
}
}
}
for ( unsigned int i = 0; i < quads.size(); ++i )
{
const Vec4ui& q = quads[i];
bool edge_hit = false;
bool origin_on_surface = false;
Vec4b ts( vertex_ts[q[0]], vertex_ts[q[1]], vertex_ts[q[2]], vertex_ts[q[3]] );
Vec4b us( vertex_us[q[0]], vertex_us[q[1]], vertex_us[q[2]], vertex_us[q[3]] );
Vec4b vs( vertex_vs[q[0]], vertex_vs[q[1]], vertex_vs[q[2]], vertex_vs[q[3]] );
quad_hits[i] = ray_quad_intersection_parity( q, ts, us, vs, Vec3d(0,0,0), test_ray, edge_hit, origin_on_surface );
if ( edge_hit )
{
good_hit = false;
break;
}
if ( origin_on_surface )
{
return true;
}
}
// check if any hit was not good
if ( !good_hit )
{
double r = rand() / (double)RAND_MAX * 2.0 * M_PI;
test_ray[0] = cos(r) * ray_len;
test_ray[1] = -sin(r) * ray_len;
}
}
if ( !good_hit ) { return true; }
unsigned int num_hits = 0;
for ( unsigned int i = 0; i < tri_hits.size(); ++i )
{
if ( tri_hits[i] ) { ++num_hits; }
}
for ( unsigned int i = 0; i < quad_hits.size(); ++i )
{
if ( quad_hits[i] ) { ++num_hits; }
}
return ( num_hits % 2 == 1 );
}
// --------------------------------------------------------
///
/// Determine the parity of the number of intersections between a ray from the origin and the generalized hexahedron made
/// up of f(G) where G = the vertices of the domain boundary (corners of the unit cube).
///
// --------------------------------------------------------
bool RootParityCollisionTest::ray_hex_parity_test( )
{
const bool vertex_ts[8] = { 0, 1, 1, 0, 0, 1, 1, 0 };
const bool vertex_us[8] = { 0, 0, 1, 1, 0, 0, 1, 1 };
const bool vertex_vs[8] = { 0, 0, 0, 0, 1, 1, 1, 1 };
Vec3d test_ray( m_ray );
double ray_len = mag( test_ray );
bool good_hit = false;
unsigned int num_tries = 0;
// bilinear patch faces of the mapped hex
std::vector<Vec4ui> quads(6);
quads[0] = Vec4ui( 0, 1, 3, 2 );
quads[1] = Vec4ui( 0, 1, 4, 5 );
quads[2] = Vec4ui( 1, 2, 5, 6 );
quads[3] = Vec4ui( 2, 3, 6, 7 );
quads[4] = Vec4ui( 3, 0, 7, 4 );
quads[5] = Vec4ui( 4, 5, 7, 6 );
// for debugging purposes, store the result of each hit test
std::vector<bool> hits( 6, false );
// (t,u,v) coordinates of each vertex on the hexahedron
while (!good_hit && num_tries++ < 10 )
{
good_hit = true;
bool any_origin_on_surface = false;
// ray-cast against each quad
for ( int i = 0; i < 6; ++i )
{
const Vec4ui& quad = quads[i];
Vec4b ts( vertex_ts[quad[0]], vertex_ts[quad[1]], vertex_ts[quad[2]], vertex_ts[quad[3]] );
Vec4b us( vertex_us[quad[0]], vertex_us[quad[1]], vertex_us[quad[2]], vertex_us[quad[3]] );
Vec4b vs( vertex_vs[quad[0]], vertex_vs[quad[1]], vertex_vs[quad[2]], vertex_vs[quad[3]] );
bool edge_hit = false;
bool origin_on_surface = false;
hits[i] = ray_quad_intersection_parity( quad, ts, us, vs, Vec3d(0,0,0), test_ray, edge_hit, origin_on_surface );
if ( edge_hit )
{
good_hit = false;
}
if ( origin_on_surface )
{
any_origin_on_surface = true;
}
}
if ( any_origin_on_surface )
{
return true;
}
// check if any hit was not okay
if ( !good_hit )
{
double r = rand() / (double)RAND_MAX * 2.0 * M_PI;
test_ray[0] = cos(r) * ray_len;
test_ray[1] = -sin(r) * ray_len;
}
}
if ( !good_hit ) { return true; }
unsigned int num_hits = 0;
for ( unsigned int i = 0; i < hits.size(); ++i )
{
if ( hits[i] ) { ++num_hits; }
}
return ( num_hits % 2 == 1 );
}
/// --------------------------------------------------------
///
/// For each triangle, form the plane it lies on, and determine if all m_interval_hex_vertices are on one side of the plane.
///
/// --------------------------------------------------------
bool RootParityCollisionTest::plane_culling( const std::vector<Vec3ui>& triangles, const std::vector<Vec3d>& boundary_vertices )
{
std::size_t num_triangles = triangles.size();
std::size_t num_boundary_vertices = boundary_vertices.size();
for ( unsigned int i = 0; i < num_triangles; ++i )
{
const Vec3ui& t = triangles[i];
Vec3d normal = cross( boundary_vertices[t[1]] - boundary_vertices[t[0]], boundary_vertices[t[2]] - boundary_vertices[t[0]] );
if ( mag(normal) == 0.0 ) { continue; }
normal = normalized(normal);
IntervalType::begin_special_arithmetic();
const int sgn = plane_sign( normal, m_interval_hex_vertices[0] );
IntervalType::end_special_arithmetic();
if ( sgn == 0 )
{
continue;
}
bool all_same_side = true;
for ( unsigned int v = 1; v < num_boundary_vertices; ++v )
{
IntervalType::begin_special_arithmetic();
const int this_plane_sign = plane_sign( normal, m_interval_hex_vertices[v] );
IntervalType::end_special_arithmetic();
if ( this_plane_sign == 0 || this_plane_sign != sgn )
{
all_same_side = false;
break;
}
}
if ( all_same_side )
{
return true;
}
}
IntervalType::end_special_arithmetic();
return false;
}
/// --------------------------------------------------------
///
/// Take a set of planes defined by the mapped domain boundary, and determine if all m_interval_hex_vertices on one side of
/// any plane.
///
/// --------------------------------------------------------
bool RootParityCollisionTest::edge_edge_interval_plane_culling()
{
std::vector<Vec3d> hex_vertices(8);
for ( unsigned int i = 0; i < 8; ++i )
{
hex_vertices[i][0] = m_interval_hex_vertices[i][0].estimate();
hex_vertices[i][1] = m_interval_hex_vertices[i][1].estimate();
hex_vertices[i][2] = m_interval_hex_vertices[i][2].estimate();
}
std::vector<Vec3ui> triangles(12);
triangles[0] = Vec3ui(0,1,3);
triangles[1] = Vec3ui(0,3,2);
triangles[2] = Vec3ui(0,1,4);
triangles[3] = Vec3ui(0,4,5);
triangles[4] = Vec3ui(1,2,5);
triangles[5] = Vec3ui(1,5,6);
triangles[6] = Vec3ui(2,3,6);
triangles[7] = Vec3ui(2,6,7);
triangles[8] = Vec3ui(3,0,7);
triangles[9] = Vec3ui(3,7,4);
triangles[10] = Vec3ui(4,5,7);
triangles[11] = Vec3ui(4,7,6);
return plane_culling( triangles, hex_vertices );
}
/// --------------------------------------------------------
///
/// Take a set of planes defined by the mapped domain boundary, and determine if all m_interval_hex_vertices lie on one side of
/// any plane.
///
/// --------------------------------------------------------
bool RootParityCollisionTest::point_triangle_interval_plane_culling()
{
std::vector<Vec3d> hex_vertices(6);
for ( unsigned int i = 0; i < 6; ++i )
{
hex_vertices[i][0] = m_interval_hex_vertices[i][0].estimate();
hex_vertices[i][1] = m_interval_hex_vertices[i][1].estimate();
hex_vertices[i][2] = m_interval_hex_vertices[i][2].estimate();
}
std::vector<Vec3ui> triangles(8);
triangles[0] = Vec3ui(0,1,2);
triangles[1] = Vec3ui(3,4,5);
triangles[2] = Vec3ui(0,1,3);
triangles[3] = Vec3ui(0,3,4);
triangles[4] = Vec3ui(1,2,4);
triangles[5] = Vec3ui(1,4,5);
triangles[6] = Vec3ui(0,2,3);
triangles[7] = Vec3ui(0,2,5);
return plane_culling( triangles, hex_vertices );
}
/// --------------------------------------------------------
///
/// Take a fixed set of planes and determine if all m_interval_hex_vertices lie on one side of any plane.
///
/// --------------------------------------------------------
bool RootParityCollisionTest::fixed_plane_culling( unsigned int num_hex_vertices )
{
for ( int i = -1; i < 1; ++i )
{
for ( int j = -1; j < 1; ++j )
{
for ( int k = -1; k < 1; ++k )
{
Vec3i normal( i, k, j );
const int plane_sign = quick_1_plane_sign( normal, m_interval_hex_vertices[0] );
if ( plane_sign == 0 )
{
continue;
}
bool all_same_side = true;
for ( unsigned int v = 1; v < num_hex_vertices; ++v )
{
const int this_plane_sign = quick_1_plane_sign( normal, m_interval_hex_vertices[v] );
if ( this_plane_sign == 0 || this_plane_sign != plane_sign )
{
all_same_side = false;
break;
}
}
if ( all_same_side )
{
return true;
}
}
}
}
return false;
}
/// --------------------------------------------------------
///
/// Run edge-edge continuous collision detection
///
/// --------------------------------------------------------
bool RootParityCollisionTest::edge_edge_collision( )
{
static const bool vertex_ts[8] = { 0, 1, 1, 0, 0, 1, 1, 0 };
static const bool vertex_us[8] = { 0, 0, 1, 1, 0, 0, 1, 1 };
static const bool vertex_vs[8] = { 0, 0, 0, 0, 1, 1, 1, 1 };
// Get the transformed corners of the domain boundary in interval representation
IntervalType::begin_special_arithmetic();
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[0], vertex_us[0], vertex_vs[0], m_interval_hex_vertices[0] );
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[1], vertex_us[1], vertex_vs[1], m_interval_hex_vertices[1] );
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[2], vertex_us[2], vertex_vs[2], m_interval_hex_vertices[2] );
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[3], vertex_us[3], vertex_vs[3], m_interval_hex_vertices[3] );
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[4], vertex_us[4], vertex_vs[4], m_interval_hex_vertices[4] );
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[5], vertex_us[5], vertex_vs[5], m_interval_hex_vertices[5] );
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[6], vertex_us[6], vertex_vs[6], m_interval_hex_vertices[6] );
edge_edge_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[7], vertex_us[7], vertex_vs[7], m_interval_hex_vertices[7] );
// Plane culling: check if all corners are on one side of the plane passing through the origin
bool plane_culled = fixed_plane_culling(8);
IntervalType::end_special_arithmetic();
bool hex_plane_culled = false;
if ( !plane_culled )
{
hex_plane_culled = edge_edge_interval_plane_culling();
}
bool safe_parity = false;
if ( !plane_culled && !hex_plane_culled )
{
// Cast ray from origin against boundary image (6 quads)
Vec3d xmin( 1e30 ), xmax( -1e30 );
for ( unsigned int i = 0; i < 8; ++i )
{
Vec2d interval0 = m_interval_hex_vertices[i][0].get_actual_interval();
Vec2d interval1 = m_interval_hex_vertices[i][1].get_actual_interval();
Vec2d interval2 = m_interval_hex_vertices[i][2].get_actual_interval();
xmin[0] = std::min( xmin[0], interval0[0] );
xmin[1] = std::min( xmin[1], interval1[0] );
xmin[2] = std::min( xmin[2], interval2[0] );
xmax[0] = std::max( xmax[0], interval0[1] );
xmax[1] = std::max( xmax[1], interval1[1] );
xmax[2] = std::max( xmax[2], interval2[1] );
}
const double ray_len = std::max( mag(xmin), mag(xmax) ) + 10.0;
m_ray = Vec3d( ray_len, ray_len, 0 );
safe_parity = ray_hex_parity_test( );
}
return safe_parity;
}
/// ----------------------------------------
///
/// Run point-triangle continuous collision detection
///
/// ----------------------------------------
bool RootParityCollisionTest::point_triangle_collision( )
{
static const bool vertex_ts[6] = { 0, 0, 0, 1, 1, 1 };
static const bool vertex_us[6] = { 0, 1, 0, 0, 1, 0 };
static const bool vertex_vs[6] = { 0, 0, 1, 0, 0, 1 };
// Get the transformed corners of the domain boundary in interval representation
IntervalType::begin_special_arithmetic();
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[0], vertex_us[0], vertex_vs[0], m_interval_hex_vertices[0] );
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[1], vertex_us[1], vertex_vs[1], m_interval_hex_vertices[1] );
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[2], vertex_us[2], vertex_vs[2], m_interval_hex_vertices[2] );
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[3], vertex_us[3], vertex_vs[3], m_interval_hex_vertices[3] );
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[4], vertex_us[4], vertex_vs[4], m_interval_hex_vertices[4] );
point_triangle_collision_function( m_x0old, m_x1old, m_x2old, m_x3old, m_x0new, m_x1new, m_x2new, m_x3new,
vertex_ts[5], vertex_us[5], vertex_vs[5], m_interval_hex_vertices[5] );
// Plane culling: check if all corners are on one side of the plane passing through the origin
bool plane_culled = fixed_plane_culling(6);
IntervalType::end_special_arithmetic();
if ( plane_culled )
{
return false;
}
bool prism_plane_culled = point_triangle_interval_plane_culling();
if ( prism_plane_culled )
{
return false;
}
// Cast ray from origin against boundary image (3 quads + 2 triangles)
Vec3d xmin( 1e30 ), xmax( -1e30 );
for ( unsigned int i = 0; i < 6; ++i )
{
Vec2d interval0 = m_interval_hex_vertices[i][0].get_actual_interval();
Vec2d interval1 = m_interval_hex_vertices[i][1].get_actual_interval();
Vec2d interval2 = m_interval_hex_vertices[i][2].get_actual_interval();
xmin[0] = std::min( xmin[0], interval0[0] );
xmin[1] = std::min( xmin[1], interval1[0] );
xmin[2] = std::min( xmin[2], interval2[0] );
xmax[0] = std::max( xmax[0], interval0[1] );
xmax[1] = std::max( xmax[1], interval1[1] );
xmax[2] = std::max( xmax[2], interval2[1] );
}
const double ray_len = std::max( mag(xmin), mag(xmax) ) + 10.0;
m_ray = Vec3d( ray_len, ray_len, 0 );
bool safe_parity = ray_prism_parity_test( );
return safe_parity;
}
} // namespace rootparity
}