1 /*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23 // OPCODE TriMesh/TriMesh collision code by Jeff Smith (c) 2004
24
25 #ifdef _MSC_VER
26 #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
27 #endif
28
29 #include <ode/collision.h>
30 #include <ode/matrix.h>
31 #include <ode/rotation.h>
32 #include <ode/odemath.h>
33 #include "config.h"
34 // Classic Implementation
35 #if dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
36
37 #if dTRIMESH_ENABLED
38
39 #include "collision_util.h"
40 #include "collision_trimesh_internal.h"
41
42
43 #if !dTLS_ENABLED
44 // Have collider cache instance unconditionally of OPCODE or GIMPACT selection
45 /*extern */TrimeshCollidersCache g_ccTrimeshCollidersCache;
46 #endif
47
48
49 #if dTRIMESH_OPCODE
50
51 #define SMALL_ELT REAL(2.5e-4)
52 #define EXPANDED_ELT_THRESH REAL(1.0e-3)
53 #define DISTANCE_EPSILON REAL(1.0e-8)
54 #define VELOCITY_EPSILON REAL(1.0e-5)
55 #define TINY_PENETRATION REAL(5.0e-6)
56
57 struct LineContactSet
58 {
59 enum
60 {
61 MAX_POINTS = 8
62 };
63
64 dVector3 Points[MAX_POINTS];
65 int Count;
66 };
67
68
69 // static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); -- not used
70 static void GenerateContact(int, dContactGeom*, int, dxTriMesh*, dxTriMesh*,
71 int TriIndex1, int TriIndex2,
72 const dVector3, const dVector3, dReal, int&);
73 static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3],
74 dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar,
75 dReal isectpt1[3],dReal isectpt2[3]);
76 inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
77 static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
78 //static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3);
79 static bool FindTriSolidIntrsection(const dVector3 Tri[3],
80 const dVector4 Planes[6], int numSides,
81 LineContactSet& ClippedPolygon );
82 static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );
83 static bool SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt,
84 dVector3 in_n, dVector3* in_col_v, dReal &out_depth);
85 static int ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point);
86 static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir,
87 const dVector3 vert0, const dVector3 vert1,const dVector3 vert2,
88 dReal *t,dReal *u,dReal *v);
89
90
91
92
93 /* some math macros */
94 #define IS_ZERO(v) (!(v)[0] && !(v)[1] && !(v)[2])
95
96 #define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
97 dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
98 dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; }
99
100 #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
101
102 #define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; }
103
104 #define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; }
105
106 #define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; }
107
108 #define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; }
109
110 #define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; }
111
112 #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]; }
113
114 #define LENGTH(x) ((dReal) dSqrt(dDOT(x, x)))
115
116 #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];
117
dMin(const dReal x,const dReal y)118 inline const dReal dMin(const dReal x, const dReal y)
119 {
120 return x < y ? x : y;
121 }
122
123
124 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)125 SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2,
126 dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2,
127 dVector3 n, dVector3 n1, dVector3 n2)
128 {
129 if (pen_v == v1) {
130 pen_v = v2;
131 pen_elt = elt_f2;
132 col_v = v1;
133 SET(n, n1);
134 }
135 else {
136 pen_v = v1;
137 pen_elt = elt_f1;
138 col_v = v2;
139 SET(n, n2);
140 }
141 }
142
143
144
145
146 int
dCollideTTL(dxGeom * g1,dxGeom * g2,int Flags,dContactGeom * Contacts,int Stride)147 dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
148 {
149 dIASSERT (Stride >= (int)sizeof(dContactGeom));
150 dIASSERT (g1->type == dTriMeshClass);
151 dIASSERT (g2->type == dTriMeshClass);
152 dIASSERT ((Flags & NUMC_MASK) >= 1);
153
154 dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
155 dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
156
157 dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals;
158 dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals;
159
160 const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1);
161 // TLRotation1 = column-major order
162 const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1);
163
164 const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2);
165 // TLRotation2 = column-major order
166 const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2);
167
168 const unsigned uiTLSKind = TriMesh1->getParentSpaceTLSKind();
169 dIASSERT(uiTLSKind == TriMesh2->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
170 TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
171 AABBTreeCollider& Collider = pccColliderCache->_AABBTreeCollider;
172 BVTCache &ColCache = pccColliderCache->ColCache;
173
174 ColCache.Model0 = &TriMesh1->Data->BVTree;
175 ColCache.Model1 = &TriMesh2->Data->BVTree;
176
177 // Collision query
178 Matrix4x4 amatrix, bmatrix;
179 BOOL IsOk = Collider.Collide(ColCache,
180 &MakeMatrix(TLPosition1, TLRotation1, amatrix),
181 &MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
182
183
184 // Make "double" versions of these matrices, if appropriate
185 dMatrix4 A, B;
186 dMakeMatrix4(TLPosition1, TLRotation1, A);
187 dMakeMatrix4(TLPosition2, TLRotation2, B);
188
189
190 if (IsOk) {
191 // Get collision status => if true, objects overlap
192 if ( Collider.GetContactStatus() ) {
193 // Number of colliding pairs and list of pairs
194 int TriCount = Collider.GetNbPairs();
195 const Pair* CollidingPairs = Collider.GetPairs();
196
197 if (TriCount > 0) {
198 // step through the pairs, adding contacts
199 int id1, id2;
200 int OutTriCount = 0;
201 dVector3 v1[3], v2[3], CoplanarPt;
202 dVector3 e1, e2, e3, n1, n2, n, ContactNormal;
203 dReal depth;
204 dVector3 orig_pos, old_pos1, old_pos2, elt1, elt2, elt_sum;
205 dVector3 elt_f1[3], elt_f2[3];
206 dReal contact_elt_length = SMALL_ELT;
207 LineContactSet firstClippedTri, secondClippedTri;
208 dVector3 *firstClippedElt = new dVector3[LineContactSet::MAX_POINTS];
209 dVector3 *secondClippedElt = new dVector3[LineContactSet::MAX_POINTS];
210
211
212 // only do these expensive inversions once
213 dMatrix4 InvMatrix1, InvMatrix2;
214 dInvertMatrix4(A, InvMatrix1);
215 dInvertMatrix4(B, InvMatrix2);
216
217
218 for (int i = 0; i < TriCount; i++) {
219
220 id1 = CollidingPairs[i].id0;
221 id2 = CollidingPairs[i].id1;
222
223 // grab the colliding triangles
224 FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1);
225 FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2);
226
227 // Since we'll be doing matrix transfomrations, we need to
228 // make sure that all vertices have four elements
229 for (int j=0; j<3; j++) {
230 v1[j][3] = 1.0;
231 v2[j][3] = 1.0;
232 }
233
234
235 int IsCoplanar = 0;
236 dReal IsectPt1[3], IsectPt2[3];
237
238 // Sometimes OPCODE makes mistakes, so we look at the return
239 // value for TriTriIntersectWithIsectLine. A retcode of "0"
240 // means no intersection took place
241 if ( TriTriIntersectWithIsectLine( v1[0], v1[1], v1[2], v2[0], v2[1], v2[2],
242 &IsCoplanar,
243 IsectPt1, IsectPt2) ) {
244
245 // Compute the normals of the colliding faces
246 //
247 if (TriNormals1 == NULL) {
248 SUB( e1, v1[1], v1[0] );
249 SUB( e2, v1[2], v1[0] );
250 CROSS( n1, e1, e2 );
251 dNormalize3(n1);
252 }
253 else {
254 // If we were passed normals, we need to adjust them to take into
255 // account the objects' current rotations
256 e1[0] = TriNormals1[id1*3];
257 e1[1] = TriNormals1[id1*3 + 1];
258 e1[2] = TriNormals1[id1*3 + 2];
259 e1[3] = 0.0;
260
261 //dMultiply1(n1, TLRotation1, e1, 3, 3, 1);
262 dMultiply0(n1, TLRotation1, e1, 3, 3, 1);
263 n1[3] = 1.0;
264 }
265
266 if (TriNormals2 == NULL) {
267 SUB( e1, v2[1], v2[0] );
268 SUB( e2, v2[2], v2[0] );
269 CROSS( n2, e1, e2);
270 dNormalize3(n2);
271 }
272 else {
273 // If we were passed normals, we need to adjust them to take into
274 // account the objects' current rotations
275 e2[0] = TriNormals2[id2*3];
276 e2[1] = TriNormals2[id2*3 + 1];
277 e2[2] = TriNormals2[id2*3 + 2];
278 e2[3] = 0.0;
279
280 //dMultiply1(n2, TLRotation2, e2, 3, 3, 1);
281 dMultiply0(n2, TLRotation2, e2, 3, 3, 1);
282 n2[3] = 1.0;
283 }
284
285
286 if (IsCoplanar) {
287 // We can reach this case if the faces are coplanar, OR
288 // if they don't actually intersect. (OPCODE can make
289 // mistakes)
290 if (dFabs(dDOT(n1, n2)) > REAL(0.999)) {
291 // If the faces are coplanar, we declare that the point of
292 // contact is at the average location of the vertices of
293 // both faces
294 dVector3 ContactPt;
295 for (int j=0; j<3; j++) {
296 ContactPt[j] = 0.0;
297 for (int k=0; k<3; k++)
298 ContactPt[j] += v1[k][j] + v2[k][j];
299 ContactPt[j] /= 6.0;
300 }
301 ContactPt[3] = 1.0;
302
303 // and the contact normal is the normal of face 2
304 // (could be face 1, because they are the same)
305 SET(n, n2);
306
307 // and the penetration depth is the co-normal
308 // distance between any two vertices A and B,
309 // i.e. d = DOT(n, (A-B))
310 DEPTH(depth, v1[1], v2[1], n);
311 if (depth < 0)
312 depth *= -1.0;
313
314 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
315 ContactPt, n, depth, OutTriCount);
316 }
317 }
318 else {
319 // Otherwise (in non-co-planar cases), we create a coplanar
320 // point -- the middle of the line of intersection -- that
321 // will be used for various computations down the road
322 for (int j=0; j<3; j++)
323 CoplanarPt[j] = ( (IsectPt1[j] + IsectPt2[j]) / REAL(2.0) );
324 CoplanarPt[3] = 1.0;
325
326 // Find the ELT of the coplanar point
327 //
328 dMultiply1(orig_pos, InvMatrix1, CoplanarPt, 4, 4, 1);
329 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
330 SUB(elt1, CoplanarPt, old_pos1);
331
332 dMultiply1(orig_pos, InvMatrix2, CoplanarPt, 4, 4, 1);
333 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
334 SUB(elt2, CoplanarPt, old_pos2);
335
336 SUB(elt_sum, elt1, elt2); // net motion of the coplanar point
337 dReal elt_sum_len = LENGTH(elt_sum); // Could be calculated on demand but there is no good place...
338
339
340 // Calculate how much the vertices of each face moved in the
341 // direction of the opposite face's normal
342 //
343 dReal total_dp1, total_dp2;
344 total_dp1 = 0.0;
345 total_dp2 = 0.0;
346
347 for (int ii=0; ii<3; ii++) {
348 // find the estimated linear translation (ELT) of the vertices
349 // on face 1, wrt to the center of face 2.
350
351 // un-transform this vertex by the current transform
352 dMultiply1(orig_pos, InvMatrix1, v1[ii], 4, 4, 1 );
353
354 // re-transform this vertex by last_trans (to get its old
355 // position)
356 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
357
358 // Then subtract this position from our current one to find
359 // the elapsed linear translation (ELT)
360 for (int k=0; k<3; k++) {
361 elt_f1[ii][k] = (v1[ii][k] - old_pos1[k]) - elt2[k];
362 }
363
364 // Take the dot product of the ELT for each vertex (wrt the
365 // center of face2)
366 total_dp1 += dFabs( dDOT(elt_f1[ii], n2) );
367 }
368
369 for (int ii=0; ii<3; ii++) {
370 // find the estimated linear translation (ELT) of the vertices
371 // on face 2, wrt to the center of face 1.
372 dMultiply1(orig_pos, InvMatrix2, v2[ii], 4, 4, 1);
373 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
374 for (int k=0; k<3; k++) {
375 elt_f2[ii][k] = (v2[ii][k] - old_pos2[k]) - elt1[k];
376 }
377
378 // Take the dot product of the ELT for each vertex (wrt the
379 // center of face2) and add them
380 total_dp2 += dFabs( dDOT(elt_f2[ii], n1) );
381 }
382
383
384 ////////
385 // Estimate the penetration depth.
386 //
387 dReal dp;
388 BOOL badPen = true;
389 dVector3 *pen_v; // the "penetrating vertices"
390 dVector3 *pen_elt; // the elt_f of the penetrating face
391 dVector3 *col_v; // the "collision vertices" (the penetrated face)
392
393 SMULT(n2, n2, -1.0); // SF PATCH #1335183
394 depth = 0.0;
395 if ((total_dp1 > DISTANCE_EPSILON) || (total_dp2 > DISTANCE_EPSILON)) {
396 ////////
397 // Find the collision normal, by finding the face
398 // that is pointed "most" in the direction of travel
399 // of the two triangles
400 //
401 if (total_dp2 > total_dp1) {
402 pen_v = v2;
403 pen_elt = elt_f2;
404 col_v = v1;
405 SET(n, n1);
406 }
407 else {
408 pen_v = v1;
409 pen_elt = elt_f1;
410 col_v = v2;
411 SET(n, n2);
412 }
413 }
414 else {
415 // the total_dp is very small, so let's fall back
416 // to a different test
417 if (LENGTH(elt2) > LENGTH(elt1)) {
418 pen_v = v2;
419 pen_elt = elt_f2;
420 col_v = v1;
421 SET(n, n1);
422 }
423 else {
424 pen_v = v1;
425 pen_elt = elt_f1;
426 col_v = v2;
427 SET(n, n2);
428 }
429 }
430
431
432 for (int j=0; j<3; j++) {
433 if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) {
434 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
435 pen_v[j], n, depth, OutTriCount);
436 badPen = false;
437
438 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
439 break;
440 }
441 }
442 }
443
444 if (badPen) {
445 // try the other normal
446 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
447
448 for (int j=0; j<3; j++)
449 if (SimpleUnclippedTest(CoplanarPt, pen_v[j], pen_elt[j], n, col_v, depth)) {
450 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
451 pen_v[j], n, depth, OutTriCount);
452 badPen = false;
453
454 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
455 break;
456 }
457 }
458 }
459
460
461
462 ////////////////////////////////////////
463 //
464 // If we haven't found a good penetration, then we're probably straddling
465 // the edge of one of the objects, or the penetraing face is big
466 // enough that all of its vertices are outside the bounds of the
467 // penetrated face.
468 // In these cases, we do a more expensive test. We clip the penetrating
469 // triangle with a solid defined by the penetrated triangle, and repeat
470 // the tests above on this new polygon
471 if (badPen) {
472
473 // Switch pen_v and n back again
474 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
475
476
477 // Find the three sides (no top or bottom) of the solid defined by
478 // the edges of the penetrated triangle.
479
480 // The dVector4 "plane" structures contain the following information:
481 // [0]-[2]: The normal of the face, pointing INWARDS (i.e.
482 // the inverse normal
483 // [3]: The distance between the face and the center of the
484 // solid, along the normal
485 dVector4 SolidPlanes[3];
486 dVector3 tmp1;
487 dVector3 sn;
488
489 for (int j=0; j<3; j++) {
490 e1[j] = col_v[1][j] - col_v[0][j];
491 e2[j] = col_v[0][j] - col_v[2][j];
492 e3[j] = col_v[2][j] - col_v[1][j];
493 }
494
495 // side 1
496 CROSS(sn, e1, n);
497 dNormalize3(sn);
498 SMULT( SolidPlanes[0], sn, -1.0 );
499
500 ADD(tmp1, col_v[0], col_v[1]);
501 SMULT(tmp1, tmp1, 0.5); // center of edge
502 // distance from center to edge along normal
503 SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]);
504
505
506 // side 2
507 CROSS(sn, e2, n);
508 dNormalize3(sn);
509 SMULT( SolidPlanes[1], sn, -1.0 );
510
511 ADD(tmp1, col_v[0], col_v[2]);
512 SMULT(tmp1, tmp1, 0.5); // center of edge
513 // distance from center to edge along normal
514 SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]);
515
516
517 // side 3
518 CROSS(sn, e3, n);
519 dNormalize3(sn);
520 SMULT( SolidPlanes[2], sn, -1.0 );
521
522 ADD(tmp1, col_v[2], col_v[1]);
523 SMULT(tmp1, tmp1, 0.5); // center of edge
524 // distance from center to edge along normal
525 SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]);
526
527
528 FindTriSolidIntrsection(pen_v, SolidPlanes, 3, firstClippedTri);
529
530 for (int j=0; j<firstClippedTri.Count; j++) {
531 firstClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults
532
533 DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], n);
534
535 // if the penetration depth (calculated above) is more than the contact
536 // point's ELT, then we've chosen the wrong face and should switch faces
537 if (pen_v == v1) {
538 dMultiply1(orig_pos, InvMatrix1, firstClippedTri.Points[j], 4, 4, 1);
539 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
540 for (int k=0; k<3; k++) {
541 firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos1[k]) - elt2[k];
542 }
543 }
544 else {
545 dMultiply1(orig_pos, InvMatrix2, firstClippedTri.Points[j], 4, 4, 1);
546 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
547 for (int k=0; k<3; k++) {
548 firstClippedElt[j][k] = (firstClippedTri.Points[j][k] - old_pos2[k]) - elt1[k];
549 }
550 }
551
552 if (dp >= 0.0) {
553 contact_elt_length = dFabs(dDOT(firstClippedElt[j], n));
554
555 depth = dp;
556 if (depth == 0.0)
557 depth = dMin(DISTANCE_EPSILON, contact_elt_length);
558
559 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
560 depth = contact_elt_length;
561
562 if (depth <= contact_elt_length) {
563 // Add a contact
564 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
565 firstClippedTri.Points[j], n, depth, OutTriCount);
566 badPen = false;
567
568 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
569 break;
570 }
571 }
572 }
573
574 }
575 }
576
577 if (badPen) {
578 // Switch pen_v and n (again!)
579 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
580
581
582 // Find the three sides (no top or bottom) of the solid created by
583 // the penetrated triangle.
584 // The dVector4 "plane" structures contain the following information:
585 // [0]-[2]: The normal of the face, pointing INWARDS (i.e.
586 // the inverse normal
587 // [3]: The distance between the face and the center of the
588 // solid, along the normal
589 dVector4 SolidPlanes[3];
590 dVector3 tmp1;
591
592 dVector3 sn;
593 for (int j=0; j<3; j++) {
594 e1[j] = col_v[1][j] - col_v[0][j];
595 e2[j] = col_v[0][j] - col_v[2][j];
596 e3[j] = col_v[2][j] - col_v[1][j];
597 }
598
599 // side 1
600 CROSS(sn, e1, n);
601 dNormalize3(sn);
602 SMULT( SolidPlanes[0], sn, -1.0 );
603
604 ADD(tmp1, col_v[0], col_v[1]);
605 SMULT(tmp1, tmp1, 0.5); // center of edge
606 // distance from center to edge along normal
607 SolidPlanes[0][3] = dDOT(tmp1, SolidPlanes[0]);
608
609
610 // side 2
611 CROSS(sn, e2, n);
612 dNormalize3(sn);
613 SMULT( SolidPlanes[1], sn, -1.0 );
614
615 ADD(tmp1, col_v[0], col_v[2]);
616 SMULT(tmp1, tmp1, 0.5); // center of edge
617 // distance from center to edge along normal
618 SolidPlanes[1][3] = dDOT(tmp1, SolidPlanes[1]);
619
620
621 // side 3
622 CROSS(sn, e3, n);
623 dNormalize3(sn);
624 SMULT( SolidPlanes[2], sn, -1.0 );
625
626 ADD(tmp1, col_v[2], col_v[1]);
627 SMULT(tmp1, tmp1, 0.5); // center of edge
628 // distance from center to edge along normal
629 SolidPlanes[2][3] = dDOT(tmp1, SolidPlanes[2]);
630
631 FindTriSolidIntrsection(pen_v, SolidPlanes, 3, secondClippedTri);
632
633 for (int j=0; j<secondClippedTri.Count; j++) {
634 secondClippedTri.Points[j][3] = 1.0; // because we will be doing matrix mults
635
636 DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], n);
637
638 if (pen_v == v1) {
639 dMultiply1(orig_pos, InvMatrix1, secondClippedTri.Points[j], 4, 4, 1);
640 dMultiply1(old_pos1, ((dxTriMesh*)g1)->last_trans, orig_pos, 4, 4, 1);
641 for (int k=0; k<3; k++) {
642 secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos1[k]) - elt2[k];
643 }
644 }
645 else {
646 dMultiply1(orig_pos, InvMatrix2, secondClippedTri.Points[j], 4, 4, 1);
647 dMultiply1(old_pos2, ((dxTriMesh*)g2)->last_trans, orig_pos, 4, 4, 1);
648 for (int k=0; k<3; k++) {
649 secondClippedElt[j][k] = (secondClippedTri.Points[j][k] - old_pos2[k]) - elt1[k];
650 }
651 }
652
653
654 if (dp >= 0.0) {
655 contact_elt_length = dFabs(dDOT(secondClippedElt[j],n));
656
657 depth = dp;
658 if (depth == 0.0)
659 depth = dMin(DISTANCE_EPSILON, contact_elt_length);
660
661 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
662 depth = contact_elt_length;
663
664 if (depth <= contact_elt_length) {
665 // Add a contact
666 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
667 secondClippedTri.Points[j], n, depth, OutTriCount);
668 badPen = false;
669
670 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
671 break;
672 }
673 }
674 }
675
676
677 }
678 }
679
680
681
682 /////////////////
683 // All conventional tests have failed at this point, so now we deal with
684 // cases on a more "heuristic" basis
685 //
686
687 if (badPen) {
688 // Switch pen_v and n (for the fourth time, so they're
689 // what my original guess said they were)
690 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
691
692 if (dFabs(dDOT(n1, n2)) < REAL(0.01)) {
693 // If we reach this point, we have (close to) perpindicular
694 // faces, either resting on each other or sliding in a
695 // direction orthogonal to both surface normals.
696 if (elt_sum_len < DISTANCE_EPSILON) {
697 depth = dFabs(dDOT(n, elt_sum));
698
699 if (depth > REAL(1e-12)) {
700 dNormalize3(n);
701 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
702 CoplanarPt, n, depth, OutTriCount);
703 badPen = false;
704 }
705 else {
706 // If the two faces are (nearly) perfectly at rest with
707 // respect to each other, then we ignore the contact,
708 // allowing the objects to slip a little in the hopes
709 // that next frame, they'll give us something to work
710 // with.
711 badPen = false;
712 }
713 }
714 else {
715 // The faces are perpindicular, but moving significantly
716 // This can be sliding, or an unusual edge-straddling
717 // penetration.
718 dVector3 cn;
719
720 CROSS(cn, n1, n2);
721 dNormalize3(cn);
722 SET(n, cn);
723
724 // The shallowest ineterpenetration of the faces
725 // is the depth
726 dVector3 ContactPt;
727 dVector3 dvTmp;
728 dReal rTmp;
729 depth = dInfinity;
730 for (int j=0; j<3; j++) {
731 for (int k=0; k<3; k++) {
732 SUB(dvTmp, col_v[k], pen_v[j]);
733
734 rTmp = dDOT(dvTmp, n);
735 if ( dFabs(rTmp) < dFabs(depth) ) {
736 depth = rTmp;
737 SET( ContactPt, pen_v[j] );
738 contact_elt_length = dFabs(dDOT(pen_elt[j], n));
739 }
740 }
741 }
742 if (depth < 0.0) {
743 SMULT(n, n, -1.0);
744 depth *= -1.0;
745 }
746
747 if ((depth > 0.0) && (depth <= contact_elt_length)) {
748 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
749 ContactPt, n, depth, OutTriCount);
750 badPen = false;
751 }
752
753 }
754 }
755 }
756
757
758 if (badPen && elt_sum_len != 0.0) {
759 // Use as the normal the direction of travel, rather than any particular
760 // face normal
761 //
762 dVector3 esn;
763
764 if (pen_v == v1) {
765 SMULT(esn, elt_sum, -1.0);
766 }
767 else {
768 SET(esn, elt_sum);
769 }
770 dNormalize3(esn);
771
772
773 // The shallowest ineterpenetration of the faces
774 // is the depth
775 dVector3 ContactPt;
776 depth = dInfinity;
777 for (int j=0; j<3; j++) {
778 for (int k=0; k<3; k++) {
779 DEPTH(dp, col_v[k], pen_v[j], esn);
780 if ( (ExamineContactPoint(col_v, esn, pen_v[j])) &&
781 ( dFabs(dp) < dFabs(depth)) ) {
782 depth = dp;
783 SET( ContactPt, pen_v[j] );
784 contact_elt_length = dFabs(dDOT(pen_elt[j], esn));
785 }
786 }
787 }
788
789 if ((depth > 0.0) && (depth <= contact_elt_length)) {
790 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
791 ContactPt, esn, depth, OutTriCount);
792 badPen = false;
793 }
794 }
795
796
797 if (badPen && elt_sum_len != 0.0) {
798 // If the direction of motion is perpindicular to both normals
799 if ( (dFabs(dDOT(n1, elt_sum)) < REAL(0.01)) && (dFabs(dDOT(n2, elt_sum)) < REAL(0.01)) ) {
800 dVector3 esn;
801 if (pen_v == v1) {
802 SMULT(esn, elt_sum, -1.0);
803 }
804 else {
805 SET(esn, elt_sum);
806 }
807
808 dNormalize3(esn);
809
810
811 // Look at the clipped points again, checking them against this
812 // new normal
813 for (int j=0; j<firstClippedTri.Count; j++) {
814 DEPTH(dp, CoplanarPt, firstClippedTri.Points[j], esn);
815
816 if (dp >= 0.0) {
817 contact_elt_length = dFabs(dDOT(firstClippedElt[j], esn));
818
819 depth = dp;
820 //if (depth == 0.0)
821 //depth = dMin(DISTANCE_EPSILON, contact_elt_length);
822
823 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
824 depth = contact_elt_length;
825
826 if (depth <= contact_elt_length) {
827 // Add a contact
828 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
829 firstClippedTri.Points[j], esn, depth, OutTriCount);
830 badPen = false;
831
832 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
833 break;
834 }
835 }
836 }
837 }
838
839 if (badPen) {
840 // If this test failed, try it with the second set of clipped faces
841 for (int j=0; j<secondClippedTri.Count; j++) {
842 DEPTH(dp, CoplanarPt, secondClippedTri.Points[j], esn);
843
844 if (dp >= 0.0) {
845 contact_elt_length = dFabs(dDOT(secondClippedElt[j], esn));
846
847 depth = dp;
848 //if (depth == 0.0)
849 //depth = dMin(DISTANCE_EPSILON, contact_elt_length);
850
851 if ((contact_elt_length < SMALL_ELT) && (depth < EXPANDED_ELT_THRESH))
852 depth = contact_elt_length;
853
854 if (depth <= contact_elt_length) {
855 // Add a contact
856 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
857 secondClippedTri.Points[j], esn, depth, OutTriCount);
858 badPen = false;
859
860 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
861 break;
862 }
863 }
864 }
865 }
866 }
867 }
868 }
869
870
871
872 if (badPen) {
873 // if we have very little motion, we're dealing with resting contact
874 // and shouldn't reference the ELTs at all
875 //
876 if (elt_sum_len < VELOCITY_EPSILON) {
877
878 // instead of a "contact_elt_length" threshhold, we'll use an
879 // arbitrary, small one
880 for (int j=0; j<3; j++) {
881 DEPTH(dp, CoplanarPt, pen_v[j], n);
882
883 if (dp == 0.0)
884 dp = TINY_PENETRATION;
885
886 if ( (dp > 0.0) && (dp <= SMALL_ELT)) {
887 // Add a contact
888 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
889 pen_v[j], n, dp, OutTriCount);
890 badPen = false;
891
892 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
893 break;
894 }
895 }
896 }
897
898
899 if (badPen) {
900 // try the other normal
901 SwapNormals(pen_v, col_v, v1, v2, pen_elt, elt_f1, elt_f2, n, n1, n2);
902
903 for (int j=0; j<3; j++) {
904 DEPTH(dp, CoplanarPt, pen_v[j], n);
905
906 if (dp == 0.0)
907 dp = TINY_PENETRATION;
908
909 if ( (dp > 0.0) && (dp <= SMALL_ELT)) {
910 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
911 pen_v[j], n, dp, OutTriCount);
912 badPen = false;
913
914 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
915 break;
916 }
917 }
918 }
919 }
920
921
922
923 }
924 }
925
926 if (badPen) {
927 // find the nearest existing contact, and replicate it's
928 // normal and depth
929 //
930 dContactGeom* Contact;
931 dVector3 pos_diff;
932 dReal min_dist, dist;
933
934 min_dist = dInfinity;
935 depth = 0.0;
936 for (int j=0; j<OutTriCount; j++) {
937 Contact = SAFECONTACT(Flags, Contacts, j, Stride);
938
939 SUB(pos_diff, Contact->pos, CoplanarPt);
940
941 dist = dDOT(pos_diff, pos_diff);
942 if (dist < min_dist) {
943 min_dist = dist;
944 depth = Contact->depth;
945 SMULT(ContactNormal, Contact->normal, -1.0);
946 }
947 }
948
949 if (depth > 0.0) {
950 // Add a tiny contact at the coplanar point
951 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
952 CoplanarPt, ContactNormal, depth, OutTriCount);
953 badPen = false;
954 }
955 }
956
957
958 if (badPen) {
959 // Add a tiny contact at the coplanar point
960 if (-dDOT(elt_sum, n1) > -dDOT(elt_sum, n2)) {
961 SET(ContactNormal, n1);
962 }
963 else {
964 SET(ContactNormal, n2);
965 }
966
967 GenerateContact(Flags, Contacts, Stride, TriMesh1, TriMesh2, id1, id2,
968 CoplanarPt, ContactNormal, TINY_PENETRATION, OutTriCount);
969 badPen = false;
970 }
971
972
973 } // not coplanar (main loop)
974 } // TriTriIntersectWithIsectLine
975
976 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
977 break;
978 }
979 }
980
981 // Free memory
982 delete[] firstClippedElt;
983 delete[] secondClippedElt;
984
985 // Return the number of contacts
986 return OutTriCount;
987 }
988 }
989 }
990
991
992 // There was some kind of failure during the Collide call or
993 // there are no faces overlapping
994 return 0;
995 }
996
997
998 /* -- not used
999 static void
1000 GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data)
1001 {
1002 dVector3 Out[3];
1003
1004 FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out);
1005
1006 for (int i = 0; i < 3; i++)
1007 triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]);
1008 }
1009 */
1010
1011 //
1012 //
1013 //
1014 #define B11 B[0]
1015 #define B12 B[1]
1016 #define B13 B[2]
1017 #define B14 B[3]
1018 #define B21 B[4]
1019 #define B22 B[5]
1020 #define B23 B[6]
1021 #define B24 B[7]
1022 #define B31 B[8]
1023 #define B32 B[9]
1024 #define B33 B[10]
1025 #define B34 B[11]
1026 #define B41 B[12]
1027 #define B42 B[13]
1028 #define B43 B[14]
1029 #define B44 B[15]
1030
1031 #define Binv11 Binv[0]
1032 #define Binv12 Binv[1]
1033 #define Binv13 Binv[2]
1034 #define Binv14 Binv[3]
1035 #define Binv21 Binv[4]
1036 #define Binv22 Binv[5]
1037 #define Binv23 Binv[6]
1038 #define Binv24 Binv[7]
1039 #define Binv31 Binv[8]
1040 #define Binv32 Binv[9]
1041 #define Binv33 Binv[10]
1042 #define Binv34 Binv[11]
1043 #define Binv41 Binv[12]
1044 #define Binv42 Binv[13]
1045 #define Binv43 Binv[14]
1046 #define Binv44 Binv[15]
1047
1048 inline void
dMakeMatrix4(const dVector3 Position,const dMatrix3 Rotation,dMatrix4 & B)1049 dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B)
1050 {
1051 B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0];
1052 B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1];
1053 B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2];
1054
1055 B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0;
1056 }
1057
1058
1059 static void
dInvertMatrix4(dMatrix4 & B,dMatrix4 & Binv)1060 dInvertMatrix4( dMatrix4& B, dMatrix4& Binv )
1061 {
1062 dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43)
1063 -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42)
1064 +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42)
1065 +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41)
1066 -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41)
1067 +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41);
1068
1069 dAASSERT (det != 0.0);
1070
1071 det = 1.0 / det;
1072
1073 Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32)));
1074 Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12)));
1075 Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22)));
1076 Binv14 = 0.0f;
1077 Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33)));
1078 Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13)));
1079 Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23)));
1080 Binv24 = 0.0f;
1081 Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31)));
1082 Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11)));
1083 Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21)));
1084 Binv34 = 0.0f;
1085 Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42)));
1086 Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42)));
1087 Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22)));
1088 Binv44 = 1.0f;
1089 }
1090
1091
1092
1093 /////////////////////////////////////////////////
1094 //
1095 // Triangle/Triangle intersection utilities
1096 //
1097 // From the article "A Fast Triangle-Triangle Intersection Test",
1098 // Journal of Graphics Tools, 2(2), 1997
1099 //
1100 // Some of this functionality is duplicated in OPCODE (see
1101 // OPC_TriTriOverlap.h) but we have replicated it here so we don't
1102 // have to mess with the internals of OPCODE, as well as so we can
1103 // further optimize some of the functions.
1104 //
1105 // This version computes the line of intersection as well (if they
1106 // are not coplanar):
1107 // int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3],
1108 // dReal U0[3],dReal U1[3],dReal U2[3],
1109 // int *coplanar,
1110 // dReal isectpt1[3],dReal isectpt2[3]);
1111 //
1112 // parameters: vertices of triangle 1: V0,V1,V2
1113 // vertices of triangle 2: U0,U1,U2
1114 //
1115 // result : returns 1 if the triangles intersect, otherwise 0
1116 // "coplanar" returns whether the tris are coplanar
1117 // isectpt1, isectpt2 are the endpoints of the line of
1118 // intersection
1119 //
1120
1121
1122
1123 /* if USE_EPSILON_TEST is true then we do a check:
1124 if |dv|<EPSILON then dv=0.0;
1125 else no check is done (which is less robust)
1126 */
1127 #define USE_EPSILON_TEST TRUE
1128 #define EPSILON REAL(0.000001)
1129
1130
1131 /* sort so that a<=b */
1132 #define SORT(a,b) \
1133 if(a>b) \
1134 { \
1135 dReal c; \
1136 c=a; \
1137 a=b; \
1138 b=c; \
1139 }
1140
1141 #define ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1) \
1142 isect0=VV0+(VV1-VV0)*D0/(D0-D1); \
1143 isect1=VV0+(VV2-VV0)*D0/(D0-D2);
1144
1145
1146 #define COMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1) \
1147 if(D0D1>0.0f) \
1148 { \
1149 /* here we know that D0D2<=0.0 */ \
1150 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
1151 ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
1152 } \
1153 else if(D0D2>0.0f) \
1154 { \
1155 /* here we know that d0d1<=0.0 */ \
1156 ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
1157 } \
1158 else if(D1*D2>0.0f || D0!=0.0f) \
1159 { \
1160 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
1161 ISECT(VV0,VV1,VV2,D0,D1,D2,isect0,isect1); \
1162 } \
1163 else if(D1!=0.0f) \
1164 { \
1165 ISECT(VV1,VV0,VV2,D1,D0,D2,isect0,isect1); \
1166 } \
1167 else if(D2!=0.0f) \
1168 { \
1169 ISECT(VV2,VV0,VV1,D2,D0,D1,isect0,isect1); \
1170 } \
1171 else \
1172 { \
1173 /* triangles are coplanar */ \
1174 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
1175 }
1176
1177
1178
1179 /* this edge to edge test is based on Franlin Antonio's gem:
1180 "Faster Line Segment Intersection", in Graphics Gems III,
1181 pp. 199-202 */
1182 #define EDGE_EDGE_TEST(V0,U0,U1) \
1183 Bx=U0[i0]-U1[i0]; \
1184 By=U0[i1]-U1[i1]; \
1185 Cx=V0[i0]-U0[i0]; \
1186 Cy=V0[i1]-U0[i1]; \
1187 f=Ay*Bx-Ax*By; \
1188 d=By*Cx-Bx*Cy; \
1189 if((f>0 && d>=0 && d<=f) || (f<0 && d<=0 && d>=f)) \
1190 { \
1191 e=Ax*Cy-Ay*Cx; \
1192 if(f>0) \
1193 { \
1194 if(e>=0 && e<=f) return 1; \
1195 } \
1196 else \
1197 { \
1198 if(e<=0 && e>=f) return 1; \
1199 } \
1200 }
1201
1202 #define EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2) \
1203 { \
1204 dReal Ax,Ay,Bx,By,Cx,Cy,e,d,f; \
1205 Ax=V1[i0]-V0[i0]; \
1206 Ay=V1[i1]-V0[i1]; \
1207 /* test edge U0,U1 against V0,V1 */ \
1208 EDGE_EDGE_TEST(V0,U0,U1); \
1209 /* test edge U1,U2 against V0,V1 */ \
1210 EDGE_EDGE_TEST(V0,U1,U2); \
1211 /* test edge U2,U1 against V0,V1 */ \
1212 EDGE_EDGE_TEST(V0,U2,U0); \
1213 }
1214
1215 #define POINT_IN_TRI(V0,U0,U1,U2) \
1216 { \
1217 dReal a,b,c,d0,d1,d2; \
1218 /* is T1 completly inside T2? */ \
1219 /* check if V0 is inside tri(U0,U1,U2) */ \
1220 a=U1[i1]-U0[i1]; \
1221 b=-(U1[i0]-U0[i0]); \
1222 c=-a*U0[i0]-b*U0[i1]; \
1223 d0=a*V0[i0]+b*V0[i1]+c; \
1224 \
1225 a=U2[i1]-U1[i1]; \
1226 b=-(U2[i0]-U1[i0]); \
1227 c=-a*U1[i0]-b*U1[i1]; \
1228 d1=a*V0[i0]+b*V0[i1]+c; \
1229 \
1230 a=U0[i1]-U2[i1]; \
1231 b=-(U0[i0]-U2[i0]); \
1232 c=-a*U2[i0]-b*U2[i1]; \
1233 d2=a*V0[i0]+b*V0[i1]+c; \
1234 if(d0*d1>0.0) \
1235 { \
1236 if(d0*d2>0.0) return 1; \
1237 } \
1238 }
1239
coplanar_tri_tri(dReal N[3],dReal V0[3],dReal V1[3],dReal V2[3],dReal U0[3],dReal U1[3],dReal U2[3])1240 int coplanar_tri_tri(dReal N[3],dReal V0[3],dReal V1[3],dReal V2[3],
1241 dReal U0[3],dReal U1[3],dReal U2[3])
1242 {
1243 dReal A[3];
1244 short i0,i1;
1245 /* first project onto an axis-aligned plane, that maximizes the area */
1246 /* of the triangles, compute indices: i0,i1. */
1247 A[0]= dFabs(N[0]);
1248 A[1]= dFabs(N[1]);
1249 A[2]= dFabs(N[2]);
1250 if(A[0]>A[1])
1251 {
1252 if(A[0]>A[2])
1253 {
1254 i0=1; /* A[0] is greatest */
1255 i1=2;
1256 }
1257 else
1258 {
1259 i0=0; /* A[2] is greatest */
1260 i1=1;
1261 }
1262 }
1263 else /* A[0]<=A[1] */
1264 {
1265 if(A[2]>A[1])
1266 {
1267 i0=0; /* A[2] is greatest */
1268 i1=1;
1269 }
1270 else
1271 {
1272 i0=0; /* A[1] is greatest */
1273 i1=2;
1274 }
1275 }
1276
1277 /* test all edges of triangle 1 against the edges of triangle 2 */
1278 EDGE_AGAINST_TRI_EDGES(V0,V1,U0,U1,U2);
1279 EDGE_AGAINST_TRI_EDGES(V1,V2,U0,U1,U2);
1280 EDGE_AGAINST_TRI_EDGES(V2,V0,U0,U1,U2);
1281
1282 /* finally, test if tri1 is totally contained in tri2 or vice versa */
1283 POINT_IN_TRI(V0,U0,U1,U2);
1284 POINT_IN_TRI(U0,V0,V1,V2);
1285
1286 return 0;
1287 }
1288
1289
1290
1291 #define NEWCOMPUTE_INTERVALS(VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,A,B,C,X0,X1) \
1292 { \
1293 if(D0D1>0.0f) \
1294 { \
1295 /* here we know that D0D2<=0.0 */ \
1296 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
1297 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
1298 } \
1299 else if(D0D2>0.0f)\
1300 { \
1301 /* here we know that d0d1<=0.0 */ \
1302 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
1303 } \
1304 else if(D1*D2>0.0f || D0!=0.0f) \
1305 { \
1306 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
1307 A=VV0; B=(VV1-VV0)*D0; C=(VV2-VV0)*D0; X0=D0-D1; X1=D0-D2; \
1308 } \
1309 else if(D1!=0.0f) \
1310 { \
1311 A=VV1; B=(VV0-VV1)*D1; C=(VV2-VV1)*D1; X0=D1-D0; X1=D1-D2; \
1312 } \
1313 else if(D2!=0.0f) \
1314 { \
1315 A=VV2; B=(VV0-VV2)*D2; C=(VV1-VV2)*D2; X0=D2-D0; X1=D2-D1; \
1316 } \
1317 else \
1318 { \
1319 /* triangles are coplanar */ \
1320 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
1321 } \
1322 }
1323
1324
1325
1326
1327 /* sort so that a<=b */
1328 #define SORT2(a,b,smallest) \
1329 if(a>b) \
1330 { \
1331 dReal c; \
1332 c=a; \
1333 a=b; \
1334 b=c; \
1335 smallest=1; \
1336 } \
1337 else smallest=0;
1338
1339
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])1340 inline void isect2(dReal VTX0[3],dReal VTX1[3],dReal VTX2[3],dReal VV0,dReal VV1,dReal VV2,
1341 dReal D0,dReal D1,dReal D2,dReal *isect0,dReal *isect1,dReal isectpoint0[3],dReal isectpoint1[3])
1342 {
1343 dReal tmp=D0/(D0-D1);
1344 dReal diff[3];
1345 *isect0=VV0+(VV1-VV0)*tmp;
1346 SUB(diff,VTX1,VTX0);
1347 MULT(diff,diff,tmp);
1348 ADD(isectpoint0,diff,VTX0);
1349 tmp=D0/(D0-D2);
1350 *isect1=VV0+(VV2-VV0)*tmp;
1351 SUB(diff,VTX2,VTX0);
1352 MULT(diff,diff,tmp);
1353 ADD(isectpoint1,VTX0,diff);
1354 }
1355
1356
1357 #if 0
1358 #define ISECT2(VTX0,VTX1,VTX2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1) \
1359 tmp=D0/(D0-D1); \
1360 isect0=VV0+(VV1-VV0)*tmp; \
1361 SUB(diff,VTX1,VTX0); \
1362 MULT(diff,diff,tmp); \
1363 ADD(isectpoint0,diff,VTX0); \
1364 tmp=D0/(D0-D2);
1365 /* isect1=VV0+(VV2-VV0)*tmp; \ */
1366 /* SUB(diff,VTX2,VTX0); \ */
1367 /* MULT(diff,diff,tmp); \ */
1368 /* ADD(isectpoint1,VTX0,diff); */
1369 #endif
1370
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])1371 inline int compute_intervals_isectline(dReal VERT0[3],dReal VERT1[3],dReal VERT2[3],
1372 dReal VV0,dReal VV1,dReal VV2,dReal D0,dReal D1,dReal D2,
1373 dReal D0D1,dReal D0D2,dReal *isect0,dReal *isect1,
1374 dReal isectpoint0[3],dReal isectpoint1[3])
1375 {
1376 if(D0D1>0.0f)
1377 {
1378 /* here we know that D0D2<=0.0 */
1379 /* that is D0, D1 are on the same side, D2 on the other or on the plane */
1380 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
1381 }
1382 else if(D0D2>0.0f)
1383 {
1384 /* here we know that d0d1<=0.0 */
1385 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
1386 }
1387 else if(D1*D2>0.0f || D0!=0.0f)
1388 {
1389 /* here we know that d0d1<=0.0 or that D0!=0.0 */
1390 isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,isect0,isect1,isectpoint0,isectpoint1);
1391 }
1392 else if(D1!=0.0f)
1393 {
1394 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,isect0,isect1,isectpoint0,isectpoint1);
1395 }
1396 else if(D2!=0.0f)
1397 {
1398 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,isect0,isect1,isectpoint0,isectpoint1);
1399 }
1400 else
1401 {
1402 /* triangles are coplanar */
1403 return 1;
1404 }
1405 return 0;
1406 }
1407
1408 #define COMPUTE_INTERVALS_ISECTLINE(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,D0D1,D0D2,isect0,isect1,isectpoint0,isectpoint1) \
1409 if(D0D1>0.0f) \
1410 { \
1411 /* here we know that D0D2<=0.0 */ \
1412 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
1413 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
1414 }
1415 #if 0
1416 else if(D0D2>0.0f) \
1417 { \
1418 /* here we know that d0d1<=0.0 */ \
1419 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
1420 } \
1421 else if(D1*D2>0.0f || D0!=0.0f) \
1422 { \
1423 /* here we know that d0d1<=0.0 or that D0!=0.0 */ \
1424 isect2(VERT0,VERT1,VERT2,VV0,VV1,VV2,D0,D1,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
1425 } \
1426 else if(D1!=0.0f) \
1427 { \
1428 isect2(VERT1,VERT0,VERT2,VV1,VV0,VV2,D1,D0,D2,&isect0,&isect1,isectpoint0,isectpoint1); \
1429 } \
1430 else if(D2!=0.0f) \
1431 { \
1432 isect2(VERT2,VERT0,VERT1,VV2,VV0,VV1,D2,D0,D1,&isect0,&isect1,isectpoint0,isectpoint1); \
1433 } \
1434 else \
1435 { \
1436 /* triangles are coplanar */ \
1437 coplanar=1; \
1438 return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2); \
1439 }
1440 #endif
1441
1442
1443
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])1444 static int TriTriIntersectWithIsectLine(dReal V0[3],dReal V1[3],dReal V2[3],
1445 dReal U0[3],dReal U1[3],dReal U2[3],int *coplanar,
1446 dReal isectpt1[3],dReal isectpt2[3])
1447 {
1448 dReal E1[3],E2[3];
1449 dReal N1[3],N2[3],d1,d2;
1450 dReal du0,du1,du2,dv0,dv1,dv2;
1451 dReal D[3];
1452 dReal isect1[2]={0,0}, isect2[2]={0,0};
1453 dReal isectpointA1[3],isectpointA2[3];
1454 dReal isectpointB1[3]={0,0,0},isectpointB2[3]={0,0,0};
1455 dReal du0du1,du0du2,dv0dv1,dv0dv2;
1456 short index;
1457 dReal vp0,vp1,vp2;
1458 dReal up0,up1,up2;
1459 dReal b,c,max;
1460 int smallest1,smallest2;
1461
1462 /* compute plane equation of triangle(V0,V1,V2) */
1463 SUB(E1,V1,V0);
1464 SUB(E2,V2,V0);
1465 CROSS(N1,E1,E2);
1466
1467 // Even though all triangles might be initially valid,
1468 // a triangle may degenerate into a segment after applying
1469 // space transformation.
1470 //
1471 // Oleh_Derevenko:
1472 // I'm not quite sure if this routine will fail/assert for zero normal
1473 // (it's too large and complex to be fully analyzed).
1474 // However in such a large code block three extra float comparisons
1475 // will not have any noticeable influence on performance.
1476 if (IS_ZERO(N1))
1477 return 0;
1478
1479 d1=-DOT(N1,V0);
1480 /* plane equation 1: N1.X+d1=0 */
1481
1482 /* put U0,U1,U2 into plane equation 1 to compute signed distances to the plane*/
1483 du0=DOT(N1,U0)+d1;
1484 du1=DOT(N1,U1)+d1;
1485 du2=DOT(N1,U2)+d1;
1486
1487 /* coplanarity robustness check */
1488 #if USE_EPSILON_TEST==TRUE
1489 if(dFabs(du0)<EPSILON) du0=0.0;
1490 if(dFabs(du1)<EPSILON) du1=0.0;
1491 if(dFabs(du2)<EPSILON) du2=0.0;
1492 #endif
1493 du0du1=du0*du1;
1494 du0du2=du0*du2;
1495
1496 if(du0du1>0.0f && du0du2>0.0f) /* same sign on all of them + not equal 0 ? */
1497 return 0; /* no intersection occurs */
1498
1499 /* compute plane of triangle (U0,U1,U2) */
1500 SUB(E1,U1,U0);
1501 SUB(E2,U2,U0);
1502 CROSS(N2,E1,E2);
1503
1504 // Even though all triangles might be initially valid,
1505 // a triangle may degenerate into a segment after applying
1506 // space transformation.
1507 //
1508 // Oleh_Derevenko:
1509 // I'm not quite sure if this routine will fail/assert for zero normal
1510 // (it's too large and complex to be fully analyzed).
1511 // However in such a large code block three extra float comparisons
1512 // will not have any noticeable influence on performance.
1513 if (IS_ZERO(N2))
1514 return 0;
1515
1516 d2=-DOT(N2,U0);
1517 /* plane equation 2: N2.X+d2=0 */
1518
1519 /* put V0,V1,V2 into plane equation 2 */
1520 dv0=DOT(N2,V0)+d2;
1521 dv1=DOT(N2,V1)+d2;
1522 dv2=DOT(N2,V2)+d2;
1523
1524 #if USE_EPSILON_TEST==TRUE
1525 if(dFabs(dv0)<EPSILON) dv0=0.0;
1526 if(dFabs(dv1)<EPSILON) dv1=0.0;
1527 if(dFabs(dv2)<EPSILON) dv2=0.0;
1528 #endif
1529
1530 dv0dv1=dv0*dv1;
1531 dv0dv2=dv0*dv2;
1532
1533 if(dv0dv1>0.0f && dv0dv2>0.0f) /* same sign on all of them + not equal 0 ? */
1534 return 0; /* no intersection occurs */
1535
1536 /* compute direction of intersection line */
1537 CROSS(D,N1,N2);
1538
1539 /* compute and index to the largest component of D */
1540 max= dFabs(D[0]);
1541 index=0;
1542 b= dFabs(D[1]);
1543 c= dFabs(D[2]);
1544 if(b>max) max=b,index=1;
1545 if(c>max) max=c,index=2;
1546
1547 /* this is the simplified projection onto L*/
1548 vp0=V0[index];
1549 vp1=V1[index];
1550 vp2=V2[index];
1551
1552 up0=U0[index];
1553 up1=U1[index];
1554 up2=U2[index];
1555
1556 /* compute interval for triangle 1 */
1557 *coplanar=compute_intervals_isectline(V0,V1,V2,vp0,vp1,vp2,dv0,dv1,dv2,
1558 dv0dv1,dv0dv2,&isect1[0],&isect1[1],isectpointA1,isectpointA2);
1559 if(*coplanar) return coplanar_tri_tri(N1,V0,V1,V2,U0,U1,U2);
1560
1561
1562 /* compute interval for triangle 2 */
1563 compute_intervals_isectline(U0,U1,U2,up0,up1,up2,du0,du1,du2,
1564 du0du1,du0du2,&isect2[0],&isect2[1],isectpointB1,isectpointB2);
1565
1566 SORT2(isect1[0],isect1[1],smallest1);
1567 SORT2(isect2[0],isect2[1],smallest2);
1568
1569 if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return 0;
1570
1571 /* at this point, we know that the triangles intersect */
1572
1573 if(isect2[0]<isect1[0])
1574 {
1575 if(smallest1==0) { SET(isectpt1,isectpointA1); }
1576 else { SET(isectpt1,isectpointA2); }
1577
1578 if(isect2[1]<isect1[1])
1579 {
1580 if(smallest2==0) { SET(isectpt2,isectpointB2); }
1581 else { SET(isectpt2,isectpointB1); }
1582 }
1583 else
1584 {
1585 if(smallest1==0) { SET(isectpt2,isectpointA2); }
1586 else { SET(isectpt2,isectpointA1); }
1587 }
1588 }
1589 else
1590 {
1591 if(smallest2==0) { SET(isectpt1,isectpointB1); }
1592 else { SET(isectpt1,isectpointB2); }
1593
1594 if(isect2[1]>isect1[1])
1595 {
1596 if(smallest1==0) { SET(isectpt2,isectpointA2); }
1597 else { SET(isectpt2,isectpointA1); }
1598 }
1599 else
1600 {
1601 if(smallest2==0) { SET(isectpt2,isectpointB2); }
1602 else { SET(isectpt2,isectpointB1); }
1603 }
1604 }
1605 return 1;
1606 }
1607
1608
1609
1610
1611
1612 // Find the intersectiojn point between a coplanar line segement,
1613 // defined by X1 and X2, and a ray defined by X3 and direction N.
1614 //
1615 // This forumla for this calculation is:
1616 // (c x b) . (a x b)
1617 // Q = x1 + a -------------------
1618 // | a x b | ^2
1619 //
1620 // where a = x2 - x1
1621 // b = x4 - x3
1622 // c = x3 - x1
1623 // x1 and x2 are the edges of the triangle, and x3 is CoplanarPt
1624 // and x4 is (CoplanarPt - n)
1625 #if 0 // not used anywhere
1626 static int
1627 IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n,
1628 dVector3 out_pt)
1629 {
1630 dVector3 a, b, c, x4;
1631
1632 ADD(x4, x3, n); // x4 = x3 + n
1633
1634 SUB(a, x2, x1); // a = x2 - x1
1635 SUB(b, x4, x3);
1636 SUB(c, x3, x1);
1637
1638 dVector3 tmp1, tmp2;
1639 CROSS(tmp1, c, b);
1640 CROSS(tmp2, a, b);
1641
1642 dReal num, denom;
1643 num = dDOT(tmp1, tmp2);
1644 denom = LENGTH( tmp2 );
1645
1646 dReal s;
1647 s = num /(denom*denom);
1648
1649 for (int i=0; i<3; i++)
1650 out_pt[i] = x1[i] + a[i]*s;
1651
1652 // Test if this intersection is "behind" x3, w.r.t. n
1653 SUB(a, x3, out_pt);
1654 if (dDOT(a, n) > 0.0)
1655 return 0;
1656
1657 // Test if this intersection point is outside the edge limits,
1658 // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside
1659 // else outside
1660 SUB(a, out_pt, x1);
1661 SUB(b, out_pt, x2);
1662 if (dDOT(a,b) < 0.0)
1663 return 1;
1664 else
1665 return 0;
1666 }
1667 #endif
1668
1669 // FindTriSolidIntersection - Clips the input trinagle TRI with the
1670 // sides of a convex bounding solid, described by PLANES, returning
1671 // the (convex) clipped polygon in CLIPPEDPOLYGON
1672 //
1673 static bool
FindTriSolidIntrsection(const dVector3 Tri[3],const dVector4 Planes[6],int numSides,LineContactSet & ClippedPolygon)1674 FindTriSolidIntrsection(const dVector3 Tri[3],
1675 const dVector4 Planes[6], int numSides,
1676 LineContactSet& ClippedPolygon )
1677 {
1678 // Set up the LineContactSet structure
1679 for (int k=0; k<3; k++) {
1680 SET(ClippedPolygon.Points[k], Tri[k]);
1681 }
1682 ClippedPolygon.Count = 3;
1683
1684 // Clip wrt the sides
1685 for ( int i = 0; i < numSides; i++ )
1686 ClipConvexPolygonAgainstPlane( Planes[i], Planes[i][3], ClippedPolygon );
1687
1688 return (ClippedPolygon.Count > 0);
1689 }
1690
1691
1692
1693
1694 // ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by
1695 // CONTACTS, with a plane (described by N and C). Note: the input
1696 // vertices are assumed to be in counterclockwise order.
1697 //
1698 // This code is taken from The Nebula Device:
1699 // http://nebuladevice.sourceforge.net/cgi-bin/twiki/view/Nebula/WebHome
1700 // and is licensed under the following license:
1701 // http://nebuladevice.sourceforge.net/doc/source/license.txt
1702 //
1703 static void
ClipConvexPolygonAgainstPlane(const dVector3 N,dReal C,LineContactSet & Contacts)1704 ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C,
1705 LineContactSet& Contacts )
1706 {
1707 // test on which side of line are the vertices
1708 int Positive = 0, Negative = 0, PIndex = -1;
1709 int Quantity = Contacts.Count;
1710
1711 dReal Test[8];
1712 for ( int i = 0; i < Contacts.Count; i++ ) {
1713 // An epsilon is used here because it is possible for the dot product
1714 // and C to be exactly equal to each other (in theory), but differ
1715 // slightly because of floating point problems. Thus, add a little
1716 // to the test number to push actually equal numbers over the edge
1717 // towards the positive. This should probably be somehow a relative
1718 // tolerance, and I don't think multiplying by the constant is the best
1719 // way to do this.
1720 Test[i] = dDOT(N, Contacts.Points[i]) - C + dFabs(C)*REAL(1e-08);
1721
1722 if (Test[i] >= REAL(0.0)) {
1723 Positive++;
1724 if (PIndex < 0) {
1725 PIndex = i;
1726 }
1727 }
1728 else Negative++;
1729 }
1730
1731 if (Positive > 0) {
1732 if (Negative > 0) {
1733 // plane transversely intersects polygon
1734 dVector3 CV[8];
1735 int CQuantity = 0, Cur, Prv;
1736 dReal T;
1737
1738 if (PIndex > 0) {
1739 // first clip vertex on line
1740 Cur = PIndex;
1741 Prv = Cur - 1;
1742 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1743 CV[CQuantity][0] = Contacts.Points[Cur][0]
1744 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1745 CV[CQuantity][1] = Contacts.Points[Cur][1]
1746 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1747 CV[CQuantity][2] = Contacts.Points[Cur][2]
1748 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1749 CV[CQuantity][3] = Contacts.Points[Cur][3]
1750 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1751 CQuantity++;
1752
1753 // vertices on positive side of line
1754 while (Cur < Quantity && Test[Cur] >= REAL(0.0)) {
1755 CV[CQuantity][0] = Contacts.Points[Cur][0];
1756 CV[CQuantity][1] = Contacts.Points[Cur][1];
1757 CV[CQuantity][2] = Contacts.Points[Cur][2];
1758 CV[CQuantity][3] = Contacts.Points[Cur][3];
1759 CQuantity++;
1760 Cur++;
1761 }
1762
1763 // last clip vertex on line
1764 if (Cur < Quantity) {
1765 Prv = Cur - 1;
1766 }
1767 else {
1768 Cur = 0;
1769 Prv = Quantity - 1;
1770 }
1771
1772 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1773 CV[CQuantity][0] = Contacts.Points[Cur][0]
1774 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1775 CV[CQuantity][1] = Contacts.Points[Cur][1]
1776 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1777 CV[CQuantity][2] = Contacts.Points[Cur][2]
1778 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1779 CV[CQuantity][3] = Contacts.Points[Cur][3]
1780 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1781 CQuantity++;
1782 }
1783 else {
1784 // iPIndex is 0
1785 // vertices on positive side of line
1786 Cur = 0;
1787 while (Cur < Quantity && Test[Cur] >= REAL(0.0)) {
1788 CV[CQuantity][0] = Contacts.Points[Cur][0];
1789 CV[CQuantity][1] = Contacts.Points[Cur][1];
1790 CV[CQuantity][2] = Contacts.Points[Cur][2];
1791 CV[CQuantity][3] = Contacts.Points[Cur][3];
1792 CQuantity++;
1793 Cur++;
1794 }
1795
1796 // last clip vertex on line
1797 Prv = Cur - 1;
1798 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1799 CV[CQuantity][0] = Contacts.Points[Cur][0]
1800 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1801 CV[CQuantity][1] = Contacts.Points[Cur][1]
1802 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1803 CV[CQuantity][2] = Contacts.Points[Cur][2]
1804 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1805 CV[CQuantity][3] = Contacts.Points[Cur][3]
1806 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1807 CQuantity++;
1808
1809 // skip vertices on negative side
1810 while (Cur < Quantity && Test[Cur] < REAL(0.0)) {
1811 Cur++;
1812 }
1813
1814 // first clip vertex on line
1815 if (Cur < Quantity) {
1816 Prv = Cur - 1;
1817 T = Test[Cur] / (Test[Cur] - Test[Prv]);
1818 CV[CQuantity][0] = Contacts.Points[Cur][0]
1819 + T * (Contacts.Points[Prv][0] - Contacts.Points[Cur][0]);
1820 CV[CQuantity][1] = Contacts.Points[Cur][1]
1821 + T * (Contacts.Points[Prv][1] - Contacts.Points[Cur][1]);
1822 CV[CQuantity][2] = Contacts.Points[Cur][2]
1823 + T * (Contacts.Points[Prv][2] - Contacts.Points[Cur][2]);
1824 CV[CQuantity][3] = Contacts.Points[Cur][3]
1825 + T * (Contacts.Points[Prv][3] - Contacts.Points[Cur][3]);
1826 CQuantity++;
1827
1828 // vertices on positive side of line
1829 while (Cur < Quantity && Test[Cur] >= REAL(0.0)) {
1830 CV[CQuantity][0] = Contacts.Points[Cur][0];
1831 CV[CQuantity][1] = Contacts.Points[Cur][1];
1832 CV[CQuantity][2] = Contacts.Points[Cur][2];
1833 CV[CQuantity][3] = Contacts.Points[Cur][3];
1834 CQuantity++;
1835 Cur++;
1836 }
1837 }
1838 else {
1839 // iCur = 0
1840 Prv = Quantity - 1;
1841 T = Test[0] / (Test[0] - Test[Prv]);
1842 CV[CQuantity][0] = Contacts.Points[0][0]
1843 + T * (Contacts.Points[Prv][0] - Contacts.Points[0][0]);
1844 CV[CQuantity][1] = Contacts.Points[0][1]
1845 + T * (Contacts.Points[Prv][1] - Contacts.Points[0][1]);
1846 CV[CQuantity][2] = Contacts.Points[0][2]
1847 + T * (Contacts.Points[Prv][2] - Contacts.Points[0][2]);
1848 CV[CQuantity][3] = Contacts.Points[0][3]
1849 + T * (Contacts.Points[Prv][3] - Contacts.Points[0][3]);
1850 CQuantity++;
1851 }
1852 }
1853 Quantity = CQuantity;
1854 memcpy( Contacts.Points, CV, CQuantity * sizeof(dVector3) );
1855 }
1856 // else polygon fully on positive side of plane, nothing to do
1857 Contacts.Count = Quantity;
1858 }
1859 else {
1860 Contacts.Count = 0; // This should not happen, but for safety
1861 }
1862
1863 }
1864
1865
1866
1867 // Determine if a potential collision point is
1868 //
1869 //
1870 static int
ExamineContactPoint(dVector3 * v_col,dVector3 in_n,dVector3 in_point)1871 ExamineContactPoint(dVector3* v_col, dVector3 in_n, dVector3 in_point)
1872 {
1873 // Cast a ray from in_point, along the collison normal. Does it intersect the
1874 // collision face.
1875 dReal t, u, v;
1876
1877 if (!RayTriangleIntersect(in_point, in_n, v_col[0], v_col[1], v_col[2],
1878 &t, &u, &v))
1879 return 0;
1880 else
1881 return 1;
1882 }
1883
1884
1885
1886 // RayTriangleIntersect - If an intersection is found, t contains the
1887 // distance along the ray (dir) and u/v contain u/v coordinates into
1888 // the triangle. Returns 0 if no hit is found
1889 // From "Real-Time Rendering," page 305
1890 //
1891 static int
RayTriangleIntersect(const dVector3 orig,const dVector3 dir,const dVector3 vert0,const dVector3 vert1,const dVector3 vert2,dReal * t,dReal * u,dReal * v)1892 RayTriangleIntersect(const dVector3 orig, const dVector3 dir,
1893 const dVector3 vert0, const dVector3 vert1,const dVector3 vert2,
1894 dReal *t,dReal *u,dReal *v)
1895
1896 {
1897 dReal edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
1898 dReal det,inv_det;
1899
1900 // find vectors for two edges sharing vert0
1901 SUB(edge1, vert1, vert0);
1902 SUB(edge2, vert2, vert0);
1903
1904 // begin calculating determinant - also used to calculate U parameter
1905 CROSS(pvec, dir, edge2);
1906
1907 // if determinant is near zero, ray lies in plane of triangle
1908 det = DOT(edge1, pvec);
1909
1910 if ((det > REAL(-0.001)) && (det < REAL(0.001)))
1911 return 0;
1912 inv_det = 1.0 / det;
1913
1914 // calculate distance from vert0 to ray origin
1915 SUB(tvec, orig, vert0);
1916
1917 // calculate U parameter and test bounds
1918 *u = DOT(tvec, pvec) * inv_det;
1919 if ((*u < 0.0) || (*u > 1.0))
1920 return 0;
1921
1922 // prepare to test V parameter
1923 CROSS(qvec, tvec, edge1);
1924
1925 // calculate V parameter and test bounds
1926 *v = DOT(dir, qvec) * inv_det;
1927 if ((*v < 0.0) || ((*u + *v) > 1.0))
1928 return 0;
1929
1930 // calculate t, ray intersects triangle
1931 *t = DOT(edge2, qvec) * inv_det;
1932
1933 return 1;
1934 }
1935
1936
1937
1938 static bool
SimpleUnclippedTest(dVector3 in_CoplanarPt,dVector3 in_v,dVector3 in_elt,dVector3 in_n,dVector3 * in_col_v,dReal & out_depth)1939 SimpleUnclippedTest(dVector3 in_CoplanarPt, dVector3 in_v, dVector3 in_elt,
1940 dVector3 in_n, dVector3* in_col_v, dReal &out_depth)
1941 {
1942 dReal dp = 0.0;
1943 dReal contact_elt_length;
1944
1945 DEPTH(dp, in_CoplanarPt, in_v, in_n);
1946
1947 if (dp >= 0.0) {
1948 // if the penetration depth (calculated above) is more than
1949 // the contact point's ELT, then we've chosen the wrong face
1950 // and should switch faces
1951 contact_elt_length = dFabs(dDOT(in_elt, in_n));
1952
1953 if (dp == 0.0)
1954 dp = dMin(DISTANCE_EPSILON, contact_elt_length);
1955
1956 if ((contact_elt_length < SMALL_ELT) && (dp < EXPANDED_ELT_THRESH))
1957 dp = contact_elt_length;
1958
1959 if ( (dp > 0.0) && (dp <= contact_elt_length)) {
1960 // Add a contact
1961
1962 if ( ExamineContactPoint(in_col_v, in_n, in_v) ) {
1963 out_depth = dp;
1964 return true;
1965 }
1966 }
1967 }
1968
1969 return false;
1970 }
1971
1972
1973
1974
1975 // Generate a "unique" contact. A unique contact has a unique
1976 // position or normal. If the potential contact has the same
1977 // position and normal as an existing contact, but a larger
1978 // penetration depth, this new depth is used instead
1979 //
1980 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)1981 GenerateContact(int in_Flags, dContactGeom* in_Contacts, int in_Stride,
1982 dxTriMesh* in_TriMesh1, dxTriMesh* in_TriMesh2,
1983 int TriIndex1, int TriIndex2,
1984 const dVector3 in_ContactPos, const dVector3 in_Normal, dReal in_Depth,
1985 int& OutTriCount)
1986 {
1987 /*
1988 NOTE by Oleh_Derevenko:
1989 This function is called after maximal number of contacts has already been
1990 collected because it has a side effect of replacing penetration depth of
1991 existing contact with larger penetration depth of another matching normal contact.
1992 If this logic is not necessary any more, you can bail out on reach of contact
1993 number maximum immediately in dCollideTTL(). You will also need to correct
1994 conditional statements after invocations of GenerateContact() in dCollideTTL().
1995 */
1996 dIASSERT(in_Depth >= 0.0);
1997 //if (in_Depth < 0.0) -- the function is always called with depth >= 0
1998 // return;
1999
2000 do
2001 {
2002 dContactGeom* Contact;
2003 dVector3 diff;
2004
2005 if (!(in_Flags & CONTACTS_UNIMPORTANT))
2006 {
2007 bool duplicate = false;
2008
2009 for (int i=0; i<OutTriCount; i++)
2010 {
2011 Contact = SAFECONTACT(in_Flags, in_Contacts, i, in_Stride);
2012
2013 // same position?
2014 SUB(diff, in_ContactPos, Contact->pos);
2015 if (dDOT(diff, diff) < dEpsilon)
2016 {
2017 // same normal?
2018 if (dFabs(dDOT(in_Normal, Contact->normal)) > (REAL(1.0)-dEpsilon))
2019 {
2020 if (in_Depth > Contact->depth) {
2021 Contact->depth = in_Depth;
2022 SMULT( Contact->normal, in_Normal, -1.0);
2023 Contact->normal[3] = 0.0;
2024 }
2025 duplicate = true;
2026 /*
2027 NOTE by Oleh_Derevenko:
2028 There may be a case when two normals are close to each other but no duplicate
2029 while third normal is detected to be duplicate for both of them.
2030 This is the only reason I can think of, there is no "break" statement.
2031 Perhaps author considered it to be logical that the third normal would
2032 replace the depth in both of initial contacts.
2033 However, I consider it a questionable practice which should not
2034 be applied without deep understanding of underlaying physics.
2035 Even more, is this situation with close normal triplet acceptable at all?
2036 Should not be two initial contacts reduced to one (replaced with the latter)?
2037 If you know the answers for these questions, you may want to change this code.
2038 See the same statement in GenerateContact() of collision_trimesh_box.cpp
2039 */
2040 }
2041 }
2042 }
2043
2044 if (duplicate || OutTriCount == (in_Flags & NUMC_MASK))
2045 {
2046 break;
2047 }
2048 }
2049 else
2050 {
2051 dIASSERT(OutTriCount < (in_Flags & NUMC_MASK));
2052 }
2053
2054 // Add a new contact
2055 Contact = SAFECONTACT(in_Flags, in_Contacts, OutTriCount, in_Stride);
2056
2057 SET( Contact->pos, in_ContactPos );
2058 Contact->pos[3] = 0.0;
2059
2060 SMULT( Contact->normal, in_Normal, -1.0);
2061 Contact->normal[3] = 0.0;
2062
2063 Contact->depth = in_Depth;
2064
2065 Contact->g1 = in_TriMesh1;
2066 Contact->g2 = in_TriMesh2;
2067
2068 Contact->side1 = TriIndex1;
2069 Contact->side2 = TriIndex2;
2070
2071 OutTriCount++;
2072 }
2073 while (false);
2074 }
2075
2076 #endif // dTRIMESH_OPCODE
2077 #endif // dTRIMESH_USE_OLD_TRIMESH_TRIMESH_COLLIDER
2078 #endif // dTRIMESH_ENABLED
2079