summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp
diff options
context:
space:
mode:
authorsanine <sanine.not@pm.me>2022-10-01 20:59:36 -0500
committersanine <sanine.not@pm.me>2022-10-01 20:59:36 -0500
commitc5fc66ee58f2c60f2d226868bb1cf5b91badaf53 (patch)
tree277dd280daf10bf77013236b8edfa5f88708c7e0 /libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp
parent1cf9cc3408af7008451f9133fb95af66a9697d15 (diff)
add ode
Diffstat (limited to 'libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp')
-rw-r--r--libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp2071
1 files changed, 2071 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp
new file mode 100644
index 0000000..23d04a1
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/collision_trimesh_trimesh_old.cpp
@@ -0,0 +1,2071 @@
+/*************************************************************************
+ * *
+ * 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 <ode/collision.h>
+#include <ode/rotation.h>
+#include "config.h"
+#include "matrix.h"
+#include "odemath.h"
+
+
+#if dTRIMESH_ENABLED
+
+#include "collision_util.h"
+#include "collision_trimesh_internal.h"
+
+
+#if dTRIMESH_OPCODE
+
+// Classic Implementation
+#if dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
+
+#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); -- not used
+static void GenerateContact(int, dContactGeom*, int, dxTriMesh*, dxTriMesh*,
+ int TriIndex1, int TriIndex2,
+ 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 IS_ZERO(v) (!(v)[0] && !(v)[1] && !(v)[2])
+
+#define CROSS(dest,v1,v2) dCalcVectorCross3(dest, v1, v2)
+
+#define DOT(v1,v2) dCalcVectorDot3(v1, v2)
+
+#define SUB(dest,v1,v2) dSubtractVectors3(dest, v1, v2)
+
+#define ADD(dest,v1,v2) dAddVectors3(dest, v1, v2)
+
+#define MULT(dest,v,factor) dCopyScaledVector3(dest, v, factor)
+
+#define SET(dest,src) dCopyVector3(dest, src)
+
+#define SMULT(p,q,s) dCopyScaledVector3(p, q, s)
+
+#define LENGTH(x) dCalcVectorLength3(x)
+
+#define DEPTH(d, p, q, n) d = dCalcPointDepth3(q, p, n)
+
+
+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;
+
+ const dReal* TriNormals1 = TriMesh1->retrieveMeshNormals();
+ const dReal* TriNormals2 = TriMesh2->retrieveMeshNormals();
+
+ 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);
+
+ const unsigned uiTLSKind = TriMesh1->getParentSpaceTLSKind();
+ dIASSERT(uiTLSKind == TriMesh2->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
+ TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
+ AABBTreeCollider& Collider = pccColliderCache->m_AABBTreeCollider;
+ BVTCache &ColCache = pccColliderCache->ColCache;
+
+ ColCache.Model0 = &TriMesh1->retrieveMeshBVTreeRef();
+ ColCache.Model1 = &TriMesh2->retrieveMeshBVTreeRef();
+
+ // Collision query
+ Matrix4x4 amatrix, bmatrix;
+ dVector3 TLOffsetPosition1 = { REAL(0.0), };
+ dVector3 TLOffsetPosition2;
+ dSubtractVectors3(TLOffsetPosition2, TLPosition2, TLPosition1);
+ MakeMatrix(TLOffsetPosition1, TLRotation1, amatrix);
+ MakeMatrix(TLOffsetPosition2, TLRotation2, bmatrix);
+ BOOL IsOk = Collider.Collide(ColCache, &amatrix, &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
+ static_cast<dxTriMesh *>(g1)->fetchMeshTriangle(v1, id1, TLPosition1, TLRotation1);
+ static_cast<dxTriMesh *>(g2)->fetchMeshTriangle(v2, id2, TLPosition2, TLRotation2);
+
+ // Since we'll be doing matrix transformations, 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(dCalcVectorDot3(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, id1, id2,
+ 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)->m_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)->m_last_trans, orig_pos, 4, 4, 1);
+ SUB(elt2, CoplanarPt, old_pos2);
+
+ SUB(elt_sum, elt1, elt2); // net motion of the coplanar point
+ dReal elt_sum_len = LENGTH(elt_sum); // Could be calculated on demand but there is no good place...
+
+
+ // 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)->m_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( dCalcVectorDot3(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)->m_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( dCalcVectorDot3(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, id1, id2,
+ 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, id1, id2,
+ 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] = dCalcVectorDot3(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] = dCalcVectorDot3(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] = dCalcVectorDot3(tmp1, SolidPlanes[2]);
+
+
+ FindTriSolidIntrsection(pen_v, SolidPlanes, 3, firstClippedTri);
+
+ for (int j=0; j<firstClippedTri.Count; j++) {
+ firstClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults
+
+ DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], n);
+
+ // 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
+ if (pen_v == v1) {
+ dMultiply1(orig_pos, InvMatrix1, firstClippedTri.Points[j], 4, 4, 1);
+ dMultiply1(old_pos1, ((dxTriMesh*)g1)->m_last_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)->m_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(dCalcVectorDot3(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, id1, id2,
+ 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] = dCalcVectorDot3(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] = dCalcVectorDot3(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] = dCalcVectorDot3(tmp1, SolidPlanes[2]);
+
+ FindTriSolidIntrsection(pen_v, SolidPlanes, 3, secondClippedTri);
+
+ for (int j=0; j<secondClippedTri.Count; j++) {
+ secondClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults
+
+ DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], n);
+
+ if (pen_v == v1) {
+ dMultiply1(orig_pos, InvMatrix1, secondClippedTri.Points[j], 4, 4, 1);
+ dMultiply1(old_pos1, ((dxTriMesh*)g1)->m_last_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)->m_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(dCalcVectorDot3(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, id1, id2,
+ 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(dCalcVectorDot3(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 (elt_sum_len < DISTANCE_EPSILON) {
+ depth = dFabs(dCalcVectorDot3(n, elt_sum));
+
+ if (depth > REAL(1e-12)) {
+ dNormalize3(n);
+ GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
+ 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 = dCalcVectorDot3(dvTmp, n);
+ if ( dFabs(rTmp) < dFabs(depth) ) {
+ depth = rTmp;
+ SET( ContactPt, pen_v[j] );
+ contact_elt_length = dFabs(dCalcVectorDot3(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, id1, id2,
+ ContactPt, n, depth, OutTriCount);
+ badPen = false;
+ }
+
+ }
+ }
+ }
+
+
+ if (badPen && elt_sum_len != 0.0) {
+ // 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(dCalcVectorDot3(pen_elt[j], esn));
+ }
+ }
+ }
+
+ if ((depth > 0.0) && (depth <= contact_elt_length)) {
+ GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
+ ContactPt, esn, depth, OutTriCount);
+ badPen = false;
+ }
+ }
+
+
+ if (badPen && elt_sum_len != 0.0) {
+ // If the direction of motion is perpindicular to both normals
+ if ( (dFabs(dCalcVectorDot3(n1, elt_sum)) < REAL(0.01)) && (dFabs(dCalcVectorDot3(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<firstClippedTri.Count; j++) {
+ DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], esn);
+
+ if (dp >= 0.0) {
+ contact_elt_length = dFabs(dCalcVectorDot3(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, id1, id2,
+ 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<secondClippedTri.Count; j++) {
+ DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], esn);
+
+ if (dp >= 0.0) {
+ contact_elt_length = dFabs(dCalcVectorDot3(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, id1, id2,
+ 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 (elt_sum_len < 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, id1, id2,
+ 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, id1, id2,
+ 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; j<OutTriCount; j++) {
+ Contact = SAFECONTACT(Flags, Contacts, j, Stride);
+
+ SUB(pos_diff, Contact->pos, CoplanarPt);
+
+ dist = dCalcVectorDot3(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, id1, id2,
+ CoplanarPt, ContactNormal, depth, OutTriCount);
+ badPen = false;
+ }
+ }
+
+
+ if (badPen) {
+ // Add a tiny contact at the coplanar point
+ if (-dCalcVectorDot3(elt_sum, n1) > -dCalcVectorDot3(elt_sum, n2)) {
+ SET(ContactNormal, n1);
+ }
+ else {
+ SET(ContactNormal, n2);
+ }
+
+ GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
+ 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;
+}
+
+
+/* -- not used
+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|<EPSILON then dv=0.0;
+ else no check is done (which is less robust)
+*/
+#define USE_EPSILON_TEST TRUE
+#define EPSILON REAL(0.000001)
+
+
+/* sort so that a<=b */
+#define SORT(a,b) \
+ if(a>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]={0,0}, isect2[2]={0,0};
+ dReal isectpointA1[3],isectpointA2[3];
+ dReal isectpointB1[3]={0,0,0},isectpointB2[3]={0,0,0};
+ 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);
+
+ // Even though all triangles might be initially valid,
+ // a triangle may degenerate into a segment after applying
+ // space transformation.
+ //
+ // Oleh_Derevenko:
+ // I'm not quite sure if this routine will fail/assert for zero normal
+ // (it's too large and complex to be fully analyzed).
+ // However in such a large code block three extra float comparisons
+ // will not have any noticeable influence on performance.
+ if (IS_ZERO(N1))
+ return 0;
+
+ 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)<EPSILON) du0=0.0;
+ if(dFabs(du1)<EPSILON) du1=0.0;
+ if(dFabs(du2)<EPSILON) du2=0.0;
+#endif
+ du0du1=du0*du1;
+ du0du2=du0*du2;
+
+ if(du0du1>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);
+
+ // Even though all triangles might be initially valid,
+ // a triangle may degenerate into a segment after applying
+ // space transformation.
+ //
+ // Oleh_Derevenko:
+ // I'm not quite sure if this routine will fail/assert for zero normal
+ // (it's too large and complex to be fully analyzed).
+ // However in such a large code block three extra float comparisons
+ // will not have any noticeable influence on performance.
+ if (IS_ZERO(N2))
+ return 0;
+
+ 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)<EPSILON) dv0=0.0;
+ if(dFabs(dv1)<EPSILON) dv1=0.0;
+ if(dFabs(dv2)<EPSILON) dv2=0.0;
+#endif
+
+ dv0dv1=dv0*dv1;
+ dv0dv2=dv0*dv2;
+
+ if(dv0dv1>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]<isect2[0] || isect2[1]<isect1[0]) return 0;
+
+ /* at this point, we know that the triangles intersect */
+
+ if(isect2[0]<isect1[0])
+ {
+ if(smallest1==0) { SET(isectpt1,isectpointA1); }
+ else { SET(isectpt1,isectpointA2); }
+
+ if(isect2[1]<isect1[1])
+ {
+ if(smallest2==0) { SET(isectpt2,isectpointB2); }
+ else { SET(isectpt2,isectpointB1); }
+ }
+ else
+ {
+ if(smallest1==0) { SET(isectpt2,isectpointA2); }
+ else { SET(isectpt2,isectpointA1); }
+ }
+ }
+ else
+ {
+ if(smallest2==0) { SET(isectpt1,isectpointB1); }
+ else { SET(isectpt1,isectpointB2); }
+
+ if(isect2[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)
+#if 0 // not used anywhere
+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 = dCalcVectorDot3(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 (dCalcVectorDot3(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 (dCalcVectorDot3(a,b) < 0.0)
+ return 1;
+ else
+ return 0;
+}
+#endif
+
+// 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] = dCalcVectorDot3(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(dCalcVectorDot3(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,
+ int TriIndex1, int TriIndex2,
+ 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; i<OutTriCount; i++)
+ {
+ Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
+
+ // same position?
+ SUB(diff, in_ContactPos, Contact->pos);
+ if (dCalcVectorDot3(diff, diff) < dEpsilon)
+ {
+ // same normal?
+ if (REAL(1.0) - dFabs(dCalcVectorDot3(in_Normal, Contact->normal)) < 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;
+
+ Contact->side1 = TriIndex1;
+ Contact->side2 = TriIndex2;
+
+ OutTriCount++;
+ }
+ while (false);
+}
+
+
+#endif // dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
+
+
+#endif // dTRIMESH_OPCODE
+
+
+#endif // dTRIMESH_ENABLED
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-02 01:27:45 +0000