From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- .../ode-0.9/ode/src/collision_trimesh_trimesh.cpp | 2033 -------------------- 1 file changed, 2033 deletions(-) delete mode 100644 libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp (limited to 'libraries/ode-0.9\/ode/src/collision_trimesh_trimesh.cpp') diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp deleted file mode 100644 index c9f209e..0000000 --- a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh.cpp +++ /dev/null @@ -1,2033 +0,0 @@ -/************************************************************************* - * * - * 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 -- cgit v1.1