// very robust triangle intersection test
// uses no divisions
// works on coplanar triangles
int
TriContact(PQP_REAL *P1, PQP_REAL *P2, PQP_REAL *P3,
PQP_REAL *Q1, PQP_REAL *Q2, PQP_REAL *Q3)
{
// One triangle is (p1,p2,p3). Other is (q1,q2,q3).
// Edges are (e1,e2,e3) and (f1,f2,f3).
// Normals are n1 and m1
// Outwards are (g1,g2,g3) and (h1,h2,h3).
//
// We assume that the triangle vertices are in the same coordinate system.
//
// First thing we do is establish a new c.s. so that p1 is at (0,0,0).
PQP_REAL p1[3], p2[3], p3[3];
PQP_REAL q1[3], q2[3], q3[3];
PQP_REAL e1[3], e2[3], e3[3];
PQP_REAL f1[3], f2[3], f3[3];
PQP_REAL g1[3], g2[3], g3[3];
PQP_REAL h1[3], h2[3], h3[3];
PQP_REAL n1[3], m1[3];
PQP_REAL ef11[3], ef12[3], ef13[3];
PQP_REAL ef21[3], ef22[3], ef23[3];
PQP_REAL ef31[3], ef32[3], ef33[3];
p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2];
p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2];
p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2];
q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2];
q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2];
q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2];
e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2];
e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2];
e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2];
f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2];
f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2];
f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2];
VcrossV(n1, e1, e2);
VcrossV(m1, f1, f2);
VcrossV(g1, e1, n1);
VcrossV(g2, e2, n1);
VcrossV(g3, e3, n1);
VcrossV(h1, f1, m1);
VcrossV(h2, f2, m1);
VcrossV(h3, f3, m1);
VcrossV(ef11, e1, f1);
VcrossV(ef12, e1, f2);
VcrossV(ef13, e1, f3);
VcrossV(ef21, e2, f1);
VcrossV(ef22, e2, f2);
VcrossV(ef23, e2, f3);
VcrossV(ef31, e3, f1);
VcrossV(ef32, e3, f2);
VcrossV(ef33, e3, f3);
// now begin the series of tests
if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
return 1;
}
void tests() {
cout << "Running tests..." << endl;
TEST("1", 1);
TEST("1 + 2", 3);
TEST("1 - 2", -1);
TEST("2 * 3", 6);
TEST("10 / 5", 2);
TEST("2 == 2", 1);
TEST("2 == 3", 0);
TEST("2 != 3", 1);
TEST("2 != 2", 0);
TEST("1 < 2", 1);
TEST("2 < 1", 0);
TEST("3 > 1", 1);
TEST("3 > 10", 0);
TEST("c(1,2)", 1, 2);
TEST("c(1, 2, 3)", 1, 2, 3);
TEST("c(1, 2) + c(3, 4)", 4, 6);
TEST("c(1, 2) - c(2, 1)", -1, 1);
TEST("c(2, 3) * c(3, 4)", 6, 12);
TEST("c(10, 9) / c(2, 3)", 5, 3);
TEST("c(1,2) == c(3, 4)", 0, 0);
TEST("c(1,2) == c(1, 4)", 1, 0);
TEST("c(1,2) == c(3, 2)", 0, 1);
TEST("c(1,2) == c(1, 2)", 1, 1);
TEST("c(1,2) != c(3, 4)", 1, 1);
TEST("c(1,2) != c(1, 4)", 0, 1);
TEST("c(1,2) != c(3, 2)", 1, 0);
TEST("c(1,2) != c(1, 2)", 0, 0);
TEST("c(1,2) < c(3,4)", 1, 1);
TEST("c(1,2) < c(2, 1)", 1, 0);
TEST("c(3,4) > c(5, 6)", 0, 0);
TEST("c(1,2,3) < 2", 1, 0, 0);
TEST("c(1,2,3) + c(1,2)", 2, 4, 4);
TEST("\"a\"", "a");
TEST("\"foo\" + \"bar\"", "foobar");
TEST("\"aba\" == \"aca\"", 1, 0, 1);
TEST("\"aba\" == c(1,2)", 0);
TEST("\"aba\" != c(1,2)", 1);
TEST("a = 1 a", 1);
TEST("a = 1 a = a + 2 a", 3);
TEST("a = 1 a = a - a a", 0);
TEST("a = 2 a = a * 3 a", 6);
TEST("a = 20 a / 4", 5);
TEST("a = 1 b = 2 a < b", 1);
TEST("a = 1 a = c(a, a)", 1, 1);
TEST("type(1)", "double");
TEST("type(\"a\")", "character");
TEST("type(function() { 1 })", "function");
TEST("length(1)", 1);
TEST("length(\"aba\")", 3);
TEST("length(\"\")", 0);
TEST("a = 1 if (a) { 1 } else { 2 }", 1);
TEST("a = 1 b = 1 if (a) { b = 2 } b", 2);
TEST("a = 0 b = 1 if (a) { b = 2 } b", 1);
TEST("a = 10 b = 0 while (a > 0) { b = b + 1 a = a - 1 } c(a, b)", 0, 10);
TEST("f = function() { 1 } f()", 1);
TEST("f = function(a, b) { a + b } f(1, 2)", 3);
TEST("f = function() { a + b } a = 1 b = 2 f()", 3);
TEST("f = function() { a = 1 a } a = 2 c(f(), a)", 1, 2);
TEST("a = c(1, 2, 3) a[1]", 2);
TEST("a = \"aba\" a[c(0,2)]", "aa");
TEST("a = c(1,2,3) a[c(0,1)] = 56 a", 56, 56, 3);
project2();
project3();
project4();
project5();
project6();
}
bool Intersect::intersect_Triangle(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
Vec3f* contact_points,
unsigned int* num_contact_points,
FCL_REAL* penetration_depth,
Vec3f* normal)
{
Vec3f p1 = P1 - P1;
Vec3f p2 = P2 - P1;
Vec3f p3 = P3 - P1;
Vec3f q1 = Q1 - P1;
Vec3f q2 = Q2 - P1;
Vec3f q3 = Q3 - P1;
Vec3f e1 = p2 - p1;
Vec3f e2 = p3 - p2;
Vec3f n1 = e1.cross(e2);
if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false;
Vec3f f1 = q2 - q1;
Vec3f f2 = q3 - q2;
Vec3f m1 = f1.cross(f2);
if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false;
Vec3f ef11 = e1.cross(f1);
if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false;
Vec3f ef12 = e1.cross(f2);
if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false;
Vec3f f3 = q1 - q3;
Vec3f ef13 = e1.cross(f3);
if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false;
Vec3f ef21 = e2.cross(f1);
if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false;
Vec3f ef22 = e2.cross(f2);
if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false;
Vec3f ef23 = e2.cross(f3);
if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false;
Vec3f e3 = p1 - p3;
Vec3f ef31 = e3.cross(f1);
if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false;
Vec3f ef32 = e3.cross(f2);
if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false;
Vec3f ef33 = e3.cross(f3);
if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false;
Vec3f g1 = e1.cross(n1);
if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false;
Vec3f g2 = e2.cross(n1);
if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false;
Vec3f g3 = e3.cross(n1);
if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false;
Vec3f h1 = f1.cross(m1);
if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false;
Vec3f h2 = f2.cross(m1);
if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false;
Vec3f h3 = f3.cross(m1);
if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false;
if(contact_points && num_contact_points && penetration_depth && normal)
{
Vec3f n1, n2;
FCL_REAL t1, t2;
buildTrianglePlane(P1, P2, P3, &n1, &t1);
buildTrianglePlane(Q1, Q2, Q3, &n2, &t2);
Vec3f deepest_points1[3];
unsigned int num_deepest_points1 = 0;
Vec3f deepest_points2[3];
unsigned int num_deepest_points2 = 0;
FCL_REAL penetration_depth1, penetration_depth2;
Vec3f P[3] = {P1, P2, P3};
Vec3f Q[3] = {Q1, Q2, Q3};
computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2);
computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1);
if(penetration_depth1 > penetration_depth2)
{
*num_contact_points = std::min(num_deepest_points2, (unsigned int)2);
for(unsigned int i = 0; i < num_deepest_points2; ++i)
{
contact_points[i] = deepest_points2[i];
}
*normal = -n1;
*penetration_depth = penetration_depth2;
}
else
{
*num_contact_points = std::min(num_deepest_points1, (unsigned int)2);
for(unsigned int i = 0; i < num_deepest_points1; ++i)
{
contact_points[i] = deepest_points1[i];
}
*normal = n2;
*penetration_depth = penetration_depth1;
}
}
return true;
}
int rapidCollide::tri_contact (const float *P1, const float *P2, const float *P3,
const float *Q1, const float *Q2, const float *Q3)
{
/*
One triangle is (p1,p2,p3). Other is (q1,q2,q3).
Edges are (e1,e2,e3) and (f1,f2,f3).
Normals are n1 and m1
Outwards are (g1,g2,g3) and (h1,h2,h3).
We assume that the triangle vertices are in the same coordinate system.
First thing we do is establish a new c.s. so that p1 is at (0,0,0).
*/
float p1[3], p2[3], p3[3];
float q1[3], q2[3], q3[3];
float e1[3], e2[3], e3[3];
float f1[3], f2[3], f3[3];
float g1[3], g2[3], g3[3];
float h1[3], h2[3], h3[3];
float n1[3], m1[3];
float z[3];
float ef11[3], ef12[3], ef13[3];
float ef21[3], ef22[3], ef23[3];
float ef31[3], ef32[3], ef33[3];
z[0] = 0.0; z[1] = 0.0; z[2] = 0.0;
p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2];
p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2];
p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2];
q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2];
q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2];
q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2];
e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2];
e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2];
e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2];
f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2];
f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2];
f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2];
VcrossV(n1, e1, e2);
VcrossV(m1, f1, f2);
VcrossV(g1, e1, n1);
VcrossV(g2, e2, n1);
VcrossV(g3, e3, n1);
VcrossV(h1, f1, m1);
VcrossV(h2, f2, m1);
VcrossV(h3, f3, m1);
VcrossV(ef11, e1, f1);
VcrossV(ef12, e1, f2);
VcrossV(ef13, e1, f3);
VcrossV(ef21, e2, f1);
VcrossV(ef22, e2, f2);
VcrossV(ef23, e2, f3);
VcrossV(ef31, e3, f1);
VcrossV(ef32, e3, f2);
VcrossV(ef33, e3, f3);
// now begin the series of tests
if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
return 1;
}
