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