/************************************************************************* * * * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * * All rights reserved. Email: russ@q12.org Web: www.q12.org * * * * This library is free software; you can redistribute it and/or * * modify it under the terms of EITHER: * * (1) The GNU Lesser General Public License as published by the Free * * Software Foundation; either version 2.1 of the License, or (at * * your option) any later version. The text of the GNU Lesser * * General Public License is included with this library in the * * file LICENSE.TXT. * * (2) The BSD-style license that is included with this library in * * the file LICENSE-BSD.TXT. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * * LICENSE.TXT and LICENSE-BSD.TXT for more details. * * * *************************************************************************/ // OPCODE TriMesh/TriMesh collision code by Jeff Smith (c) 2004 #ifdef _MSC_VER #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints #endif #include #include #include #include // Classic Implementation #if !dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER #if dTRIMESH_ENABLED #include "collision_util.h" #define TRIMESH_INTERNAL #include "collision_trimesh_internal.h" #if dTRIMESH_OPCODE #define SMALL_ELT REAL(2.5e-4) #define EXPANDED_ELT_THRESH REAL(1.0e-3) #define DISTANCE_EPSILON REAL(1.0e-8) #define VELOCITY_EPSILON REAL(1.0e-5) #define TINY_PENETRATION REAL(5.0e-6) struct LineContactSet { enum { MAX_POINTS = 8 }; dVector3 Points[MAX_POINTS]; int Count; }; static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); static void GenerateContact(int, dContactGeom*, int, dxTriMesh*, dxTriMesh*, const dVector3, const dVector3, dReal, int&); static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, dReal isectpt1[3],dReal isectpt2[3]); inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B); static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ); static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3); static bool FindTriSolidIntrsection(const dVector3 Tri[3], const dVector4 Planes[6], int numSides, LineContactSet& ClippedPolygon ); static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& ); static bool SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, dVector3 in_n, dVector3* in_col_v, dReal &out_depth); static int ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point); static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir, const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, dReal *t,dReal *u,dReal *v); /* some math macros */ #define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \ dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \ dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; } #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2]) #define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; } #define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; } #define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; } #define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; } #define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; } #define COMBO(combo,p,t,q) { combo[0]=p[0]+t*q[0]; combo[1]=p[1]+t*q[1]; combo[2]=p[2]+t*q[2]; } #define LENGTH(x) ((dReal) dSqrt(dDOT(x, x))) #define DEPTH(d, p, q, n) d = (p[0] - q[0])*n[0] + (p[1] - q[1])*n[1] + (p[2] - q[2])*n[2]; inline const dReal dMin(const dReal x, const dReal y) { return x < y ? x : y; } inline void SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2, dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2, dVector3 n, dVector3 n1, dVector3 n2) { if (pen_v == v1) { pen_v = v2; pen_elt = elt_f2; col_v = v1; SET(n, n1); } else { pen_v = v1; pen_elt = elt_f1; col_v = v2; SET(n, n2); } } int dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride) { dIASSERT (Stride >= (int)sizeof(dContactGeom)); dIASSERT (g1->type == dTriMeshClass); dIASSERT (g2->type == dTriMeshClass); dIASSERT ((Flags & NUMC_MASK) >= 1); dxTriMesh* TriMesh1 = (dxTriMesh*) g1; dxTriMesh* TriMesh2 = (dxTriMesh*) g2; dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals; dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals; const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1); // TLRotation1 = column-major order const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1); const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2); // TLRotation2 = column-major order const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2); AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider; static BVTCache ColCache; ColCache.Model0 = &TriMesh1->Data->BVTree; ColCache.Model1 = &TriMesh2->Data->BVTree; // Collision query Matrix4x4 amatrix, bmatrix; BOOL IsOk = Collider.Collide(ColCache, &MakeMatrix(TLPosition1, TLRotation1, amatrix), &MakeMatrix(TLPosition2, TLRotation2, bmatrix) ); // Make "double" versions of these matrices, if appropriate dMatrix4 A, B; dMakeMatrix4(TLPosition1, TLRotation1, A); dMakeMatrix4(TLPosition2, TLRotation2, B); if (IsOk) { // Get collision status => if true, objects overlap if ( Collider.GetContactStatus() ) { // Number of colliding pairs and list of pairs int TriCount = Collider.GetNbPairs(); const Pair* CollidingPairs = Collider.GetPairs(); if (TriCount > 0) { // step through the pairs, adding contacts int id1, id2; int OutTriCount = 0; dVector3 v1[3], v2[3], CoplanarPt; dVector3 e1, e2, e3, n1, n2, n, ContactNormal; dReal depth; dVector3 orig_pos, old_pos1, old_pos2, elt1, elt2, elt_sum; dVector3 elt_f1[3], elt_f2[3]; dReal contact_elt_length = SMALL_ELT; LineContactSet firstClippedTri, secondClippedTri; dVector3 *firstClippedElt = new dVector3[LineContactSet::MAX_POINTS]; dVector3 *secondClippedElt = new dVector3[LineContactSet::MAX_POINTS]; // only do these expensive inversions once dMatrix4 InvMatrix1, InvMatrix2; dInvertMatrix4(A, InvMatrix1); dInvertMatrix4(B, InvMatrix2); for (int i = 0; i < TriCount; i++) { id1 = CollidingPairs[i].id0; id2 = CollidingPairs[i].id1; // grab the colliding triangles FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1); FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2); // Since we'll be doing matrix transfomrations, we need to // make sure that all vertices have four elements for (int j=0; j<3; j++) { v1[j][3] = 1.0; v2[j][3] = 1.0; } int IsCoplanar = 0; dReal IsectPt1[3], IsectPt2[3]; // Sometimes OPCODE makes mistakes, so we look at the return // value for TriTriIntersectWithIsectLine. A retcode of "0" // means no intersection took place if ( TriTriIntersectWithIsectLine( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2], &IsCoplanar, IsectPt1, IsectPt2) ) { // Compute the normals of the colliding faces // if (TriNormals1 == NULL) { SUB( e1, v1[1], v1[0] ); SUB( e2, v1[2], v1[0] ); CROSS( n1, e1, e2 ); dNormalize3(n1); } else { // If we were passed normals, we need to adjust them to take into // account the objects' current rotations e1[0] = TriNormals1[id1*3]; e1[1] = TriNormals1[id1*3 + 1]; e1[2] = TriNormals1[id1*3 + 2]; e1[3] = 0.0; //dMultiply1(n1, TLRotation1, e1, 3, 3, 1); dMultiply0(n1, TLRotation1, e1, 3, 3, 1); n1[3] = 1.0; } if (TriNormals2 == NULL) { SUB( e1, v2[1], v2[0] ); SUB( e2, v2[2], v2[0] ); CROSS( n2, e1, e2); dNormalize3(n2); } else { // If we were passed normals, we need to adjust them to take into // account the objects' current rotations e2[0] = TriNormals2[id2*3]; e2[1] = TriNormals2[id2*3 + 1]; e2[2] = TriNormals2[id2*3 + 2]; e2[3] = 0.0; //dMultiply1(n2, TLRotation2, e2, 3, 3, 1); dMultiply0(n2, TLRotation2, e2, 3, 3, 1); n2[3] = 1.0; } if (IsCoplanar) { // We can reach this case if the faces are coplanar, OR // if they don't actually intersect. (OPCODE can make // mistakes) if (dFabs(dDOT(n1, n2)) > REAL(0.999)) { // If the faces are coplanar, we declare that the point of // contact is at the average location of the vertices of // both faces dVector3 ContactPt; for (int j=0; j<3; j++) { ContactPt[j] = 0.0; for (int k=0; k<3; k++) ContactPt[j] += v1[k][j] + v2[k][j]; ContactPt[j] /= 6.0; } ContactPt[3] = 1.0; // and the contact normal is the normal of face 2 // (could be face 1, because they are the same) SET(n, n2); // and the penetration depth is the co-normal // distance between any two vertices A and B, // i.e. d = DOT(n, (A-B)) DEPTH(depth, v1[1], v2[1], n); if (depth < 0) depth *= -1.0; GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, ContactPt, n, depth, OutTriCount); } } else { // Otherwise (in non-co-planar cases), we create a coplanar // point -- the middle of the line of intersection -- that // will be used for various computations down the road for (int j=0; j<3; j++) CoplanarPt[j] = ( (IsectPt1[j] + IsectPt2[j]) / REAL(2.0) ); CoplanarPt[3] = 1.0; // Find the ELT of the coplanar point // dMultiply1(orig_pos, InvMatrix1, CoplanarPt, 4, 4, 1); dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1); SUB(elt1, CoplanarPt, old_pos1); dMultiply1(orig_pos, InvMatrix2, CoplanarPt, 4, 4, 1); dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); SUB(elt2, CoplanarPt, old_pos2); SUB(elt_sum, elt1, elt2); // net motion of the coplanar point // Calculate how much the vertices of each face moved in the // direction of the opposite face's normal // dReal total_dp1, total_dp2; total_dp1 = 0.0; total_dp2 = 0.0; for (int ii=0; ii<3; ii++) { // find the estimated linear translation (ELT) of the vertices // on face 1, wrt to the center of face 2. // un-transform this vertex by the current transform dMultiply1(orig_pos, InvMatrix1, v1[ii], 4, 4, 1 ); // re-transform this vertex by last_trans (to get its old // position) dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1); // Then subtract this position from our current one to find // the elapsed linear translation (ELT) for (int k=0; k<3; k++) { elt_f1[ii][k] = (v1[ii][k] - old_pos1[k]) - elt2[k]; } // Take the dot product of the ELT for each vertex (wrt the // center of face2) total_dp1 += dFabs( dDOT(elt_f1[ii], n2) ); } for (int ii=0; ii<3; ii++) { // find the estimated linear translation (ELT) of the vertices // on face 2, wrt to the center of face 1. dMultiply1(orig_pos, InvMatrix2, v2[ii], 4, 4, 1); dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); for (int k=0; k<3; k++) { elt_f2[ii][k] = (v2[ii][k] - old_pos2[k]) - elt1[k]; } // Take the dot product of the ELT for each vertex (wrt the // center of face2) and add them total_dp2 += dFabs( dDOT(elt_f2[ii], n1) ); } //////// // Estimate the penetration depth. // dReal dp; BOOL badPen = true; dVector3 *pen_v; // the "penetrating vertices" dVector3 *pen_elt; // the elt_f of the penetrating face dVector3 *col_v; // the "collision vertices" (the penetrated face) SMULT(n2, n2, -1.0); // SF PATCH #1335183 depth = 0.0; if ((total_dp1 > DISTANCE_EPSILON) || (total_dp2 > DISTANCE_EPSILON)) { //////// // Find the collision normal, by finding the face // that is pointed "most" in the direction of travel // of the two triangles // if (total_dp2 > total_dp1) { pen_v = v2; pen_elt = elt_f2; col_v = v1; SET(n, n1); } else { pen_v = v1; pen_elt = elt_f1; col_v = v2; SET(n, n2); } } else { // the total_dp is very small, so let's fall back // to a different test if (LENGTH(elt2) > LENGTH(elt1)) { pen_v = v2; pen_elt = elt_f2; col_v = v1; SET(n, n1); } else { pen_v = v1; pen_elt = elt_f1; col_v = v2; SET(n, n2); } } for (int j=0; j<3; j++) if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, pen_v[j], n, depth, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } if (badPen) { // try the other normal SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); for (int j=0; j<3; j++) if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) { GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, pen_v[j], n, depth, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } //////////////////////////////////////// // // If we haven't found a good penetration, then we're probably straddling // the edge of one of the objects, or the penetraing face is big // enough that all of its vertices are outside the bounds of the // penetrated face. // In these cases, we do a more expensive test. We clip the penetrating // triangle with a solid defined by the penetrated triangle, and repeat // the tests above on this new polygon if (badPen) { // Switch pen_v and n back again SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); // Find the three sides (no top or bottom) of the solid defined by // the edges of the penetrated triangle. // The dVector4 "plane" structures contain the following information: // [0]-[2]: The normal of the face, pointing INWARDS (i.e. // the inverse normal // [3]: The distance between the face and the center of the // solid, along the normal dVector4 SolidPlanes[3]; dVector3 tmp1; dVector3 sn; for (int j=0; j<3; j++) { e1[j] = col_v[1][j] - col_v[0][j]; e2[j] = col_v[0][j] - col_v[2][j]; e3[j] = col_v[2][j] - col_v[1][j]; } // side 1 CROSS(sn, e1, n); dNormalize3(sn); SMULT( SolidPlanes[0], sn, -1.0 ); ADD(tmp1, col_v[0], col_v[1]); SMULT(tmp1, tmp1, 0.5); // center of edge // distance from center to edge along normal SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]); // side 2 CROSS(sn, e2, n); dNormalize3(sn); SMULT( SolidPlanes[1], sn, -1.0 ); ADD(tmp1, col_v[0], col_v[2]); SMULT(tmp1, tmp1, 0.5); // center of edge // distance from center to edge along normal SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]); // side 3 CROSS(sn, e3, n); dNormalize3(sn); SMULT( SolidPlanes[2], sn, -1.0 ); ADD(tmp1, col_v[2], col_v[1]); SMULT(tmp1, tmp1, 0.5); // center of edge // distance from center to edge along normal SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]); FindTriSolidIntrsection(pen_v, SolidPlanes, 3, firstClippedTri); for (int j=0; jlast_trans, orig_pos, 4, 4, 1); for (int k=0; k<3; k++) { firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; } } else { dMultiply1(orig_pos, InvMatrix2, firstClippedTri.Points[j], 4, 4, 1); dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); for (int k=0; k<3; k++) { firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; } } if (dp >= 0.0) { contact_elt_length = dFabs(dDOT(firstClippedElt[j], n)); depth = dp; if (depth == 0.0) depth = dMin(DISTANCE_EPSILON, contact_elt_length); if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) depth = contact_elt_length; if (depth <= contact_elt_length) { // Add a contact GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, firstClippedTri.Points[j], n, depth, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } } } if (badPen) { // Switch pen_v and n (again!) SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); // Find the three sides (no top or bottom) of the solid created by // the penetrated triangle. // The dVector4 "plane" structures contain the following information: // [0]-[2]: The normal of the face, pointing INWARDS (i.e. // the inverse normal // [3]: The distance between the face and the center of the // solid, along the normal dVector4 SolidPlanes[3]; dVector3 tmp1; dVector3 sn; for (int j=0; j<3; j++) { e1[j] = col_v[1][j] - col_v[0][j]; e2[j] = col_v[0][j] - col_v[2][j]; e3[j] = col_v[2][j] - col_v[1][j]; } // side 1 CROSS(sn, e1, n); dNormalize3(sn); SMULT( SolidPlanes[0], sn, -1.0 ); ADD(tmp1, col_v[0], col_v[1]); SMULT(tmp1, tmp1, 0.5); // center of edge // distance from center to edge along normal SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]); // side 2 CROSS(sn, e2, n); dNormalize3(sn); SMULT( SolidPlanes[1], sn, -1.0 ); ADD(tmp1, col_v[0], col_v[2]); SMULT(tmp1, tmp1, 0.5); // center of edge // distance from center to edge along normal SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]); // side 3 CROSS(sn, e3, n); dNormalize3(sn); SMULT( SolidPlanes[2], sn, -1.0 ); ADD(tmp1, col_v[2], col_v[1]); SMULT(tmp1, tmp1, 0.5); // center of edge // distance from center to edge along normal SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]); FindTriSolidIntrsection(pen_v, SolidPlanes, 3, secondClippedTri); for (int j=0; jlast_trans, orig_pos, 4, 4, 1); for (int k=0; k<3; k++) { secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos1[k]) - elt2[k]; } } else { dMultiply1(orig_pos, InvMatrix2, secondClippedTri.Points[j], 4, 4, 1); dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1); for (int k=0; k<3; k++) { secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos2[k]) - elt1[k]; } } if (dp >= 0.0) { contact_elt_length = dFabs(dDOT(secondClippedElt[j],n)); depth = dp; if (depth == 0.0) depth = dMin(DISTANCE_EPSILON, contact_elt_length); if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) depth = contact_elt_length; if (depth <= contact_elt_length) { // Add a contact GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, secondClippedTri.Points[j], n, depth, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } } } ///////////////// // All conventional tests have failed at this point, so now we deal with // cases on a more "heuristic" basis // if (badPen) { // Switch pen_v and n (for the fourth time, so they're // what my original guess said they were) SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); if (dFabs(dDOT(n1, n2)) < REAL(0.01)) { // If we reach this point, we have (close to) perpindicular // faces, either resting on each other or sliding in a // direction orthogonal to both surface normals. if (LENGTH(elt_sum) < DISTANCE_EPSILON) { depth = dFabs(dDOT(n, elt_sum)); if (depth > REAL(1e-12)) { dNormalize3(n); GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, CoplanarPt, n, depth, OutTriCount); badPen = false; } else { // If the two faces are (nearly) perfectly at rest with // respect to each other, then we ignore the contact, // allowing the objects to slip a little in the hopes // that next frame, they'll give us something to work // with. badPen = false; } } else { // The faces are perpindicular, but moving significantly // This can be sliding, or an unusual edge-straddling // penetration. dVector3 cn; CROSS(cn, n1, n2); dNormalize3(cn); SET(n, cn); // The shallowest ineterpenetration of the faces // is the depth dVector3 ContactPt; dVector3 dvTmp; dReal rTmp; depth = dInfinity; for (int j=0; j<3; j++) { for (int k=0; k<3; k++) { SUB(dvTmp, col_v[k], pen_v[j]); rTmp = dDOT(dvTmp, n); if ( dFabs(rTmp) < dFabs(depth) ) { depth = rTmp; SET( ContactPt, pen_v[j] ); contact_elt_length = dFabs(dDOT(pen_elt[j], n)); } } } if (depth < 0.0) { SMULT(n, n, -1.0); depth *= -1.0; } if ((depth > 0.0) && (depth <= contact_elt_length)) { GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, ContactPt, n, depth, OutTriCount); badPen = false; } } } } if (badPen) { // Use as the normal the direction of travel, rather than any particular // face normal // dVector3 esn; if (pen_v == v1) { SMULT(esn, elt_sum, -1.0); } else { SET(esn, elt_sum); } dNormalize3(esn); // The shallowest ineterpenetration of the faces // is the depth dVector3 ContactPt; depth = dInfinity; for (int j=0; j<3; j++) { for (int k=0; k<3; k++) { DEPTH(dp, col_v[k], pen_v[j], esn); if ( (ExamineContactPoint(col_v, esn, pen_v[j])) && ( dFabs(dp) < dFabs(depth)) ) { depth = dp; SET( ContactPt, pen_v[j] ); contact_elt_length = dFabs(dDOT(pen_elt[j], esn)); } } } if ((depth > 0.0) && (depth <= contact_elt_length)) { GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, ContactPt, esn, depth, OutTriCount); badPen = false; } } if (badPen) { // If the direction of motion is perpindicular to both normals if ( (dFabs(dDOT(n1, elt_sum)) < REAL(0.01)) && (dFabs(dDOT(n2, elt_sum)) < REAL(0.01)) ) { dVector3 esn; if (pen_v == v1) { SMULT(esn, elt_sum, -1.0); } else { SET(esn, elt_sum); } dNormalize3(esn); // Look at the clipped points again, checking them against this // new normal for (int j=0; j= 0.0) { contact_elt_length = dFabs(dDOT(firstClippedElt[j], esn)); depth = dp; //if (depth == 0.0) //depth = dMin(DISTANCE_EPSILON, contact_elt_length); if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) depth = contact_elt_length; if (depth <= contact_elt_length) { // Add a contact GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, firstClippedTri.Points[j], esn, depth, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } } if (badPen) { // If this test failed, try it with the second set of clipped faces for (int j=0; j= 0.0) { contact_elt_length = dFabs(dDOT(secondClippedElt[j], esn)); depth = dp; //if (depth == 0.0) //depth = dMin(DISTANCE_EPSILON, contact_elt_length); if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH)) depth = contact_elt_length; if (depth <= contact_elt_length) { // Add a contact GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, secondClippedTri.Points[j], esn, depth, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } } } } } if (badPen) { // if we have very little motion, we're dealing with resting contact // and shouldn't reference the ELTs at all // if (LENGTH(elt_sum) < VELOCITY_EPSILON) { // instead of a "contact_elt_length" threshhold, we'll use an // arbitrary, small one for (int j=0; j<3; j++) { DEPTH(dp, CoplanarPt, pen_v[j], n); if (dp == 0.0) dp = TINY_PENETRATION; if ( (dp > 0.0) && (dp <= SMALL_ELT)) { // Add a contact GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, pen_v[j], n, dp, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } if (badPen) { // try the other normal SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2); for (int j=0; j<3; j++) { DEPTH(dp, CoplanarPt, pen_v[j], n); if (dp == 0.0) dp = TINY_PENETRATION; if ( (dp > 0.0) && (dp <= SMALL_ELT)) { GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, pen_v[j], n, dp, OutTriCount); badPen = false; if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } } } } } if (badPen) { // find the nearest existing contact, and replicate it's // normal and depth // dContactGeom* Contact; dVector3 pos_diff; dReal min_dist, dist; min_dist = dInfinity; depth = 0.0; for (int j=0; jpos, CoplanarPt); dist = dDOT(pos_diff, pos_diff); if (dist < min_dist) { min_dist = dist; depth = Contact->depth; SMULT(ContactNormal, Contact->normal, -1.0); } } if (depth > 0.0) { // Add a tiny contact at the coplanar point GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, CoplanarPt, ContactNormal, depth, OutTriCount); badPen = false; } } if (badPen) { // Add a tiny contact at the coplanar point if (-dDOT(elt_sum, n1) > -dDOT(elt_sum, n2)) { SET(ContactNormal, n1); } else { SET(ContactNormal, n2); } GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, CoplanarPt, ContactNormal, TINY_PENETRATION, OutTriCount); badPen = false; } } // not coplanar (main loop) } // TriTriIntersectWithIsectLine if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) { break; } } // Free memory delete[] firstClippedElt; delete[] secondClippedElt; // Return the number of contacts return OutTriCount; } } } // There was some kind of failure during the Collide call or // there are no faces overlapping return 0; } static void GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data) { dVector3 Out[3]; FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out); for (int i = 0; i < 3; i++) triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]); } // // // #define B11 B[0] #define B12 B[1] #define B13 B[2] #define B14 B[3] #define B21 B[4] #define B22 B[5] #define B23 B[6] #define B24 B[7] #define B31 B[8] #define B32 B[9] #define B33 B[10] #define B34 B[11] #define B41 B[12] #define B42 B[13] #define B43 B[14] #define B44 B[15] #define Binv11 Binv[0] #define Binv12 Binv[1] #define Binv13 Binv[2] #define Binv14 Binv[3] #define Binv21 Binv[4] #define Binv22 Binv[5] #define Binv23 Binv[6] #define Binv24 Binv[7] #define Binv31 Binv[8] #define Binv32 Binv[9] #define Binv33 Binv[10] #define Binv34 Binv[11] #define Binv41 Binv[12] #define Binv42 Binv[13] #define Binv43 Binv[14] #define Binv44 Binv[15] inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B) { B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0]; B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1]; B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2]; B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0; } static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv ) { dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43) -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42) +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42) +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41) -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41) +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41); dAASSERT (det != 0.0); det = 1.0 / det; Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32))); Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12))); Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22))); Binv14 = 0.0f; Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33))); Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13))); Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23))); Binv24 = 0.0f; Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31))); Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11))); Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21))); Binv34 = 0.0f; Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42))); Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42))); Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22))); Binv44 = 1.0f; } ///////////////////////////////////////////////// // // Triangle/Triangle intersection utilities // // From the article "A Fast Triangle-Triangle Intersection Test", // Journal of Graphics Tools, 2(2), 1997 // // Some of this functionality is duplicated in OPCODE (see // OPC_TriTriOverlap.h) but we have replicated it here so we don't // have to mess with the internals of OPCODE, as well as so we can // further optimize some of the functions. // // This version computes the line of intersection as well (if they // are not coplanar): // int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], // dReal U0[3],dReal U1[3],dReal U2[3], // int *coplanar, // dReal isectpt1[3],dReal isectpt2[3]); // // parameters: vertices of triangle 1: V0,V1,V2 // vertices of triangle 2: U0,U1,U2 // // result : returns 1 if the triangles intersect, otherwise 0 // "coplanar" returns whether the tris are coplanar // isectpt1, isectpt2 are the endpoints of the line of // intersection // /* if USE_EPSILON_TEST is true then we do a check: if |dv|b) \ { \ dReal c; \ c=a; \ a=b; \ b=c; \ } #define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \ isect0=VV0+(VV1-VV0)*D0/(D0-D1); \ isect1=VV0+(VV2-VV0)*D0/(D0-D2); #define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \ if(D0D1>0.0f) \ { \ /* here we know that D0D2<=0.0 */ \ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ } \ else if(D0D2>0.0f) \ { \ /* here we know that d0d1<=0.0 */ \ ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ } \ else if(D1*D2>0.0f || D0!=0.0f) \ { \ /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \ } \ else if(D1!=0.0f) \ { \ ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \ } \ else if(D2!=0.0f) \ { \ ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \ } \ else \ { \ /* triangles are coplanar */ \ return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ } /* this edge to edge test is based on Franlin Antonio's gem: "Faster Line Segment Intersection", in Graphics Gems III, pp. 199-202 */ #define EDGE_EDGE_TEST(V0,U0,U1) \ Bx=U0[i0]-U1[i0]; \ By=U0[i1]-U1[i1]; \ Cx=V0[i0]-U0[i0]; \ Cy=V0[i1]-U0[i1]; \ f=Ay*Bx-Ax*By; \ d=By*Cx-Bx*Cy; \ if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \ { \ e=Ax*Cy-Ay*Cx; \ if(f>0) \ { \ if(e>=0 && e<=f) return 1; \ } \ else \ { \ if(e<=0 && e>=f) return 1; \ } \ } #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \ { \ dReal Ax,Ay,Bx,By,Cx,Cy,e,d,f; \ Ax=V1[i0]-V0[i0]; \ Ay=V1[i1]-V0[i1]; \ /* test edge U0,U1 against V0,V1 */ \ EDGE_EDGE_TEST(V0,U0,U1); \ /* test edge U1,U2 against V0,V1 */ \ EDGE_EDGE_TEST(V0,U1,U2); \ /* test edge U2,U1 against V0,V1 */ \ EDGE_EDGE_TEST(V0,U2,U0); \ } #define POINT_IN_TRI(V0,U0,U1,U2) \ { \ dReal a,b,c,d0,d1,d2; \ /* is T1 completly inside T2? */ \ /* check if V0 is inside tri(U0,U1,U2) */ \ a=U1[i1]-U0[i1]; \ b=-(U1[i0]-U0[i0]); \ c=-a*U0[i0]-b*U0[i1]; \ d0=a*V0[i0]+b*V0[i1]+c; \ \ a=U2[i1]-U1[i1]; \ b=-(U2[i0]-U1[i0]); \ c=-a*U1[i0]-b*U1[i1]; \ d1=a*V0[i0]+b*V0[i1]+c; \ \ a=U0[i1]-U2[i1]; \ b=-(U0[i0]-U2[i0]); \ c=-a*U2[i0]-b*U2[i1]; \ d2=a*V0[i0]+b*V0[i1]+c; \ if(d0*d1>0.0) \ { \ if(d0*d2>0.0) return 1; \ } \ } int coplanar_tri_tri(dReal N[3],dReal V0[3],dReal V1[3],dReal V2[3], dReal U0[3],dReal U1[3],dReal U2[3]) { dReal A[3]; short i0,i1; /* first project onto an axis-aligned plane, that maximizes the area */ /* of the triangles, compute indices: i0,i1. */ A[0]= dFabs(N[0]); A[1]= dFabs(N[1]); A[2]= dFabs(N[2]); if(A[0]>A[1]) { if(A[0]>A[2]) { i0=1; /* A[0] is greatest */ i1=2; } else { i0=0; /* A[2] is greatest */ i1=1; } } else /* A[0]<=A[1] */ { if(A[2]>A[1]) { i0=0; /* A[2] is greatest */ i1=1; } else { i0=0; /* A[1] is greatest */ i1=2; } } /* test all edges of triangle 1 against the edges of triangle 2 */ EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2); EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2); EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2); /* finally, test if tri1 is totally contained in tri2 or vice versa */ POINT_IN_TRI(V0,U0,U1,U2); POINT_IN_TRI(U0,V0,V1,V2); return 0; } #define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \ { \ if(D0D1>0.0f) \ { \ /* here we know that D0D2<=0.0 */ \ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \ } \ else if(D0D2>0.0f)\ { \ /* here we know that d0d1<=0.0 */ \ A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \ } \ else if(D1*D2>0.0f || D0!=0.0f) \ { \ /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \ } \ else if(D1!=0.0f) \ { \ A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \ } \ else if(D2!=0.0f) \ { \ A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \ } \ else \ { \ /* triangles are coplanar */ \ return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ } \ } /* sort so that a<=b */ #define SORT2(a,b,smallest) \ if(a>b) \ { \ dReal c; \ c=a; \ a=b; \ b=c; \ smallest=1; \ } \ else smallest=0; inline void isect2(dReal VTX0[3],dReal VTX1[3],dReal VTX2[3],dReal VV0,dReal VV1,dReal VV2, dReal D0,dReal D1,dReal D2,dReal *isect0,dReal *isect1,dReal isectpoint0[3],dReal isectpoint1[3]) { dReal tmp=D0/(D0-D1); dReal diff[3]; *isect0=VV0+(VV1-VV0)*tmp; SUB(diff,VTX1,VTX0); MULT(diff,diff,tmp); ADD(isectpoint0,diff,VTX0); tmp=D0/(D0-D2); *isect1=VV0+(VV2-VV0)*tmp; SUB(diff,VTX2,VTX0); MULT(diff,diff,tmp); ADD(isectpoint1,VTX0,diff); } #if 0 #define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \ tmp=D0/(D0-D1); \ isect0=VV0+(VV1-VV0)*tmp; \ SUB(diff,VTX1,VTX0); \ MULT(diff,diff,tmp); \ ADD(isectpoint0,diff,VTX0); \ tmp=D0/(D0-D2); /* isect1=VV0+(VV2-VV0)*tmp; \ */ /* SUB(diff,VTX2,VTX0); \ */ /* MULT(diff,diff,tmp); \ */ /* ADD(isectpoint1,VTX0,diff); */ #endif inline int compute_intervals_isectline(dReal VERT0[3],dReal VERT1[3],dReal VERT2[3], dReal VV0,dReal VV1,dReal VV2,dReal D0,dReal D1,dReal D2, dReal D0D1,dReal D0D2,dReal *isect0,dReal *isect1, dReal isectpoint0[3],dReal isectpoint1[3]) { if(D0D1>0.0f) { /* here we know that D0D2<=0.0 */ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); } else if(D0D2>0.0f) { /* here we know that d0d1<=0.0 */ isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); } else if(D1*D2>0.0f || D0!=0.0f) { /* here we know that d0d1<=0.0 or that D0!=0.0 */ isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1); } else if(D1!=0.0f) { isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1); } else if(D2!=0.0f) { isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1); } else { /* triangles are coplanar */ return 1; } return 0; } #define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \ if(D0D1>0.0f) \ { \ /* here we know that D0D2<=0.0 */ \ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \ isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ } #if 0 else if(D0D2>0.0f) \ { \ /* here we know that d0d1<=0.0 */ \ isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ } \ else if(D1*D2>0.0f || D0!=0.0f) \ { \ /* here we know that d0d1<=0.0 or that D0!=0.0 */ \ isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ } \ else if(D1!=0.0f) \ { \ isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \ } \ else if(D2!=0.0f) \ { \ isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \ } \ else \ { \ /* triangles are coplanar */ \ coplanar=1; \ return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \ } #endif static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3], dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar, dReal isectpt1[3],dReal isectpt2[3]) { dReal E1[3],E2[3]; dReal N1[3],N2[3],d1,d2; dReal du0,du1,du2,dv0,dv1,dv2; dReal D[3]; dReal isect1[2], isect2[2]; dReal isectpointA1[3],isectpointA2[3]; dReal isectpointB1[3],isectpointB2[3]; dReal du0du1,du0du2,dv0dv1,dv0dv2; short index; dReal vp0,vp1,vp2; dReal up0,up1,up2; dReal b,c,max; int smallest1,smallest2; /* compute plane equation of triangle(V0,V1,V2) */ SUB(E1,V1,V0); SUB(E2,V2,V0); CROSS(N1,E1,E2); d1=-DOT(N1,V0); /* plane equation 1: N1.X+d1=0 */ /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/ du0=DOT(N1,U0)+d1; du1=DOT(N1,U1)+d1; du2=DOT(N1,U2)+d1; /* coplanarity robustness check */ #if USE_EPSILON_TEST==TRUE if(dFabs(du0)0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */ return 0; /* no intersection occurs */ /* compute plane of triangle (U0,U1,U2) */ SUB(E1,U1,U0); SUB(E2,U2,U0); CROSS(N2,E1,E2); d2=-DOT(N2,U0); /* plane equation 2: N2.X+d2=0 */ /* put V0,V1,V2 into plane equation 2 */ dv0=DOT(N2,V0)+d2; dv1=DOT(N2,V1)+d2; dv2=DOT(N2,V2)+d2; #if USE_EPSILON_TEST==TRUE if(dFabs(dv0)0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */ return 0; /* no intersection occurs */ /* compute direction of intersection line */ CROSS(D,N1,N2); /* compute and index to the largest component of D */ max= dFabs(D[0]); index=0; b= dFabs(D[1]); c= dFabs(D[2]); if(b>max) max=b,index=1; if(c>max) max=c,index=2; /* this is the simplified projection onto L*/ vp0=V0[index]; vp1=V1[index]; vp2=V2[index]; up0=U0[index]; up1=U1[index]; up2=U2[index]; /* compute interval for triangle 1 */ *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2, dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2); if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); /* compute interval for triangle 2 */ compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2, du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2); SORT2(isect1[0],isect1[1],smallest1); SORT2(isect2[0],isect2[1],smallest2); if(isect1[1]isect1[1]) { if(smallest1==0) { SET(isectpt2,isectpointA2); } else { SET(isectpt2,isectpointA1); } } else { if(smallest2==0) { SET(isectpt2,isectpointB2); } else { SET(isectpt2,isectpointB1); } } } return 1; } // Find the intersectiojn point between a coplanar line segement, // defined by X1 and X2, and a ray defined by X3 and direction N. // // This forumla for this calculation is: // (c x b) . (a x b) // Q = x1 + a ------------------- // | a x b | ^2 // // where a = x2 - x1 // b = x4 - x3 // c = x3 - x1 // x1 and x2 are the edges of the triangle, and x3 is CoplanarPt // and x4 is (CoplanarPt - n) static int IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n, dVector3 out_pt) { dVector3 a, b, c, x4; ADD(x4, x3, n); // x4 = x3 + n SUB(a, x2, x1); // a = x2 - x1 SUB(b, x4, x3); SUB(c, x3, x1); dVector3 tmp1, tmp2; CROSS(tmp1, c, b); CROSS(tmp2, a, b); dReal num, denom; num = dDOT(tmp1, tmp2); denom = LENGTH( tmp2 ); dReal s; s = num /(denom*denom); for (int i=0; i<3; i++) out_pt[i] = x1[i] + a[i]*s; // Test if this intersection is "behind" x3, w.r.t. n SUB(a, x3, out_pt); if (dDOT(a, n) > 0.0) return 0; // Test if this intersection point is outside the edge limits, // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside // else outside SUB(a, out_pt, x1); SUB(b, out_pt, x2); if (dDOT(a,b) < 0.0) return 1; else return 0; } // FindTriSolidIntersection - Clips the input trinagle TRI with the // sides of a convex bounding solid, described by PLANES, returning // the (convex) clipped polygon in CLIPPEDPOLYGON // static bool FindTriSolidIntrsection(const dVector3 Tri[3], const dVector4 Planes[6], int numSides, LineContactSet& ClippedPolygon ) { // Set up the LineContactSet structure for (int k=0; k<3; k++) { SET(ClippedPolygon.Points[k], Tri[k]); } ClippedPolygon.Count = 3; // Clip wrt the sides for ( int i = 0; i < numSides; i++ ) ClipConvexPolygonAgainstPlane( Planes[i], Planes[i][3], ClippedPolygon ); return (ClippedPolygon.Count > 0); } // ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by // CONTACTS, with a plane (described by N and C). Note: the input // vertices are assumed to be in counterclockwise order. // // This code is taken from The Nebula Device: // http://nebuladevice.sourceforge.net/cgi-bin/twiki/view/Nebula/WebHome // and is licensed under the following license: // http://nebuladevice.sourceforge.net/doc/source/license.txt // static void ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C, LineContactSet& Contacts ) { // test on which side of line are the vertices int Positive = 0, Negative = 0, PIndex = -1; int Quantity = Contacts.Count; dReal Test[8]; for ( int i = 0; i < Contacts.Count; i++ ) { // An epsilon is used here because it is possible for the dot product // and C to be exactly equal to each other (in theory), but differ // slightly because of floating point problems. Thus, add a little // to the test number to push actually equal numbers over the edge // towards the positive. This should probably be somehow a relative // tolerance, and I don't think multiplying by the constant is the best // way to do this. Test[i] = dDOT(N, Contacts.Points[i]) - C + dFabs(C)*REAL(1e-08); if (Test[i] >= REAL(0.0)) { Positive++; if (PIndex < 0) { PIndex = i; } } else Negative++; } if (Positive > 0) { if (Negative > 0) { // plane transversely intersects polygon dVector3 CV[8]; int CQuantity = 0, Cur, Prv; dReal T; if (PIndex > 0) { // first clip vertex on line Cur = PIndex; Prv = Cur - 1; T = Test[Cur] / (Test[Cur] - Test[Prv]); CV[CQuantity][0] = Contacts.Points[Cur][0] + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); CV[CQuantity][1] = Contacts.Points[Cur][1] + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); CV[CQuantity][2] = Contacts.Points[Cur][2] + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); CV[CQuantity][3] = Contacts.Points[Cur][3] + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); CQuantity++; // vertices on positive side of line while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { CV[CQuantity][0] = Contacts.Points[Cur][0]; CV[CQuantity][1] = Contacts.Points[Cur][1]; CV[CQuantity][2] = Contacts.Points[Cur][2]; CV[CQuantity][3] = Contacts.Points[Cur][3]; CQuantity++; Cur++; } // last clip vertex on line if (Cur < Quantity) { Prv = Cur - 1; } else { Cur = 0; Prv = Quantity - 1; } T = Test[Cur] / (Test[Cur] - Test[Prv]); CV[CQuantity][0] = Contacts.Points[Cur][0] + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); CV[CQuantity][1] = Contacts.Points[Cur][1] + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); CV[CQuantity][2] = Contacts.Points[Cur][2] + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); CV[CQuantity][3] = Contacts.Points[Cur][3] + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); CQuantity++; } else { // iPIndex is 0 // vertices on positive side of line Cur = 0; while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { CV[CQuantity][0] = Contacts.Points[Cur][0]; CV[CQuantity][1] = Contacts.Points[Cur][1]; CV[CQuantity][2] = Contacts.Points[Cur][2]; CV[CQuantity][3] = Contacts.Points[Cur][3]; CQuantity++; Cur++; } // last clip vertex on line Prv = Cur - 1; T = Test[Cur] / (Test[Cur] - Test[Prv]); CV[CQuantity][0] = Contacts.Points[Cur][0] + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); CV[CQuantity][1] = Contacts.Points[Cur][1] + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); CV[CQuantity][2] = Contacts.Points[Cur][2] + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); CV[CQuantity][3] = Contacts.Points[Cur][3] + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); CQuantity++; // skip vertices on negative side while (Cur < Quantity && Test[Cur] < REAL(0.0)) { Cur++; } // first clip vertex on line if (Cur < Quantity) { Prv = Cur - 1; T = Test[Cur] / (Test[Cur] - Test[Prv]); CV[CQuantity][0] = Contacts.Points[Cur][0] + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]); CV[CQuantity][1] = Contacts.Points[Cur][1] + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]); CV[CQuantity][2] = Contacts.Points[Cur][2] + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]); CV[CQuantity][3] = Contacts.Points[Cur][3] + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]); CQuantity++; // vertices on positive side of line while (Cur < Quantity && Test[Cur] >= REAL(0.0)) { CV[CQuantity][0] = Contacts.Points[Cur][0]; CV[CQuantity][1] = Contacts.Points[Cur][1]; CV[CQuantity][2] = Contacts.Points[Cur][2]; CV[CQuantity][3] = Contacts.Points[Cur][3]; CQuantity++; Cur++; } } else { // iCur = 0 Prv = Quantity - 1; T = Test[0] / (Test[0] - Test[Prv]); CV[CQuantity][0] = Contacts.Points[0][0] + T * (Contacts.Points[Prv][0] - Contacts.Points[0][0]); CV[CQuantity][1] = Contacts.Points[0][1] + T * (Contacts.Points[Prv][1] - Contacts.Points[0][1]); CV[CQuantity][2] = Contacts.Points[0][2] + T * (Contacts.Points[Prv][2] - Contacts.Points[0][2]); CV[CQuantity][3] = Contacts.Points[0][3] + T * (Contacts.Points[Prv][3] - Contacts.Points[0][3]); CQuantity++; } } Quantity = CQuantity; memcpy( Contacts.Points, CV, CQuantity * sizeof(dVector3) ); } // else polygon fully on positive side of plane, nothing to do Contacts.Count = Quantity; } else { Contacts.Count = 0; // This should not happen, but for safety } } // Determine if a potential collision point is // // static int ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point) { // Cast a ray from in_point, along the collison normal. Does it intersect the // collision face. dReal t, u, v; if (!RayTriangleIntersect(in_point, in_n, v_col[0], v_col[1], v_col[2], &t, &u, &v)) return 0; else return 1; } // RayTriangleIntersect - If an intersection is found, t contains the // distance along the ray (dir) and u/v contain u/v coordinates into // the triangle. Returns 0 if no hit is found // From "Real-Time Rendering," page 305 // static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir, const dVector3 vert0, const dVector3 vert1,const dVector3 vert2, dReal *t,dReal *u,dReal *v) { dReal edge1[3], edge2[3], tvec[3], pvec[3], qvec[3]; dReal det,inv_det; // find vectors for two edges sharing vert0 SUB(edge1, vert1, vert0); SUB(edge2, vert2, vert0); // begin calculating determinant - also used to calculate U parameter CROSS(pvec, dir, edge2); // if determinant is near zero, ray lies in plane of triangle det = DOT(edge1, pvec); if ((det > REAL(-0.001)) && (det < REAL(0.001))) return 0; inv_det = 1.0 / det; // calculate distance from vert0 to ray origin SUB(tvec, orig, vert0); // calculate U parameter and test bounds *u = DOT(tvec, pvec) * inv_det; if ((*u < 0.0) || (*u > 1.0)) return 0; // prepare to test V parameter CROSS(qvec, tvec, edge1); // calculate V parameter and test bounds *v = DOT(dir, qvec) * inv_det; if ((*v < 0.0) || ((*u + *v) > 1.0)) return 0; // calculate t, ray intersects triangle *t = DOT(edge2, qvec) * inv_det; return 1; } static bool SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt, dVector3 in_n, dVector3* in_col_v, dReal &out_depth) { dReal dp = 0.0; dReal contact_elt_length; DEPTH(dp, in_CoplanarPt, in_v, in_n); if (dp >= 0.0) { // if the penetration depth (calculated above) is more than // the contact point's ELT, then we've chosen the wrong face // and should switch faces contact_elt_length = dFabs(dDOT(in_elt, in_n)); if (dp == 0.0) dp = dMin(DISTANCE_EPSILON, contact_elt_length); if ((contact_elt_length < SMALL_ELT) && (dp < EXPANDED_ELT_THRESH)) dp = contact_elt_length; if ( (dp > 0.0) && (dp <= contact_elt_length)) { // Add a contact if ( ExamineContactPoint(in_col_v, in_n, in_v) ) { out_depth = dp; return true; } } } return false; } // Generate a "unique" contact. A unique contact has a unique // position or normal. If the potential contact has the same // position and normal as an existing contact, but a larger // penetration depth, this new depth is used instead // static void GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride, dxTriMesh* in_TriMesh1, dxTriMesh* in_TriMesh2, const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth, int& OutTriCount) { /* NOTE by Oleh_Derevenko: This function is called after maximal number of contacts has already been collected because it has a side effect of replacing penetration depth of existing contact with larger penetration depth of another matching normal contact. If this logic is not necessary any more, you can bail out on reach of contact number maximum immediately in dCollideTTL(). You will also need to correct conditional statements after invocations of GenerateContact() in dCollideTTL(). */ dIASSERT(in_Depth >= 0.0); //if (in_Depth < 0.0) -- the function is always called with depth >= 0 // return; do { dContactGeom* Contact; dVector3 diff; if (!(in_Flags & CONTACTS_UNIMPORTANT)) { bool duplicate = false; for (int i=0; ipos); if (dDOT(diff, diff) < dEpsilon) { // same normal? if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon)) { if (in_Depth > Contact->depth) { Contact->depth = in_Depth; SMULT( Contact->normal, in_Normal, -1.0); Contact->normal[3] = 0.0; } duplicate = true; /* NOTE by Oleh_Derevenko: There may be a case when two normals are close to each other but no duplicate while third normal is detected to be duplicate for both of them. This is the only reason I can think of, there is no "break" statement. Perhaps author considered it to be logical that the third normal would replace the depth in both of initial contacts. However, I consider it a questionable practice which should not be applied without deep understanding of underlaying physics. Even more, is this situation with close normal triplet acceptable at all? Should not be two initial contacts reduced to one (replaced with the latter)? If you know the answers for these questions, you may want to change this code. See the same statement in GenerateContact() of collision_trimesh_box.cpp */ } } } if (duplicate || OutTriCount == (in_Flags & NUMC_MASK)) { break; } } else { dIASSERT(OutTriCount < (in_Flags & NUMC_MASK)); } // Add a new contact Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride); SET( Contact->pos, in_ContactPos ); Contact->pos[3] = 0.0; SMULT( Contact->normal, in_Normal, -1.0); Contact->normal[3] = 0.0; Contact->depth = in_Depth; Contact->g1 = in_TriMesh1; Contact->g2 = in_TriMesh2; OutTriCount++; } while (false); } #endif // dTRIMESH_OPCODE #endif // !dTRIMESH_USE_NEW_TRIMESH_TRIMESH_COLLIDER #endif // dTRIMESH_ENABLED