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