2034 lines
89 KiB
C++
2034 lines
89 KiB
C++
/*************************************************************************
|
|
* *
|
|
* 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/matrix.h>
|
|
#include <ode/rotation.h>
|
|
#include <ode/odemath.h>
|
|
|
|
// 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; 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)->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)->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; 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)->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)->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<firstClippedTri.Count; j++) {
|
|
DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], esn);
|
|
|
|
if (dp >= 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<secondClippedTri.Count; j++) {
|
|
DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], esn);
|
|
|
|
if (dp >= 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; j<OutTriCount; j++) {
|
|
Contact = SAFECONTACT(Flags, Contacts, j, Stride);
|
|
|
|
SUB(pos_diff, Contact->pos, 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|<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], 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)<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);
|
|
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)
|
|
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; i<OutTriCount; i++)
|
|
{
|
|
Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
|
|
|
|
// same position?
|
|
SUB(diff, in_ContactPos, Contact->pos);
|
|
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
|