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
24 // Written at 2006-10-28 by Francisco Le�n (http://gimpact.sourceforge.net)
25
26 #ifdef _MSC_VER
27 #pragma warning(disable:4244 4305) // for VC++, no precision loss complaints
28 #endif
29
30 #include <ode/collision.h>
31 #include <ode/rotation.h>
32 #include "config.h"
33 #include "matrix.h"
34 #include "odemath.h"
35
36
37 // New Implementation
38 #if !dTRIMESH_OPCODE_USE_OLD_TRIMESH_TRIMESH_COLLIDER
39
40 #if dTRIMESH_ENABLED
41
42 #include "collision_util.h"
43 #include "collision_trimesh_internal.h"
44
45
46 #if !dTLS_ENABLED
47 // Have collider cache instance unconditionally of OPCODE or GIMPACT selection
48 /*extern */TrimeshCollidersCache g_ccTrimeshCollidersCache;
49 #endif
50
51
52 #if dTRIMESH_OPCODE
53
54 #define SMALL_ELT REAL(2.5e-4)
55 #define EXPANDED_ELT_THRESH REAL(1.0e-3)
56 #define DISTANCE_EPSILON REAL(1.0e-8)
57 #define VELOCITY_EPSILON REAL(1.0e-5)
58 #define TINY_PENETRATION REAL(5.0e-6)
59
60 struct LineContactSet
61 {
62 enum
63 {
64 MAX_POINTS = 8
65 };
66
67 dVector3 Points[MAX_POINTS];
68 int Count;
69 };
70
71
72 // static void GetTriangleGeometryCallback(udword, VertexPointers&, udword); -- not used
73 inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
74 //static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
75 //static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3);
76 static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );
77
78
79 ///returns the penetration depth
80 static dReal MostDeepPoints(
81 LineContactSet & points,
82 const dVector3 plane_normal,
83 dReal plane_dist,
84 LineContactSet & deep_points);
85
86 static bool TriTriContacts(const dVector3 tr1[3],
87 const dVector3 tr2[3],
88 int TriIndex1, int TriIndex2,
89 dxGeom* g1, dxGeom* g2, int Flags,
90 CONTACT_KEY_HASH_TABLE &hashcontactset,
91 dContactGeom* Contacts, int Stride,
92 int &contactcount);
93
94
95 /* some math macros */
96 #define IS_ZERO(v) (!(v)[0] && !(v)[1] && !(v)[2])
97
98 #define CROSS(dest,v1,v2) dCalcVectorCross3(dest, v1, v2)
99
100 #define DOT(v1,v2) dCalcVectorDot3(v1, v2)
101
102 #define SUB(dest,v1,v2) dSubtractVectors3(dest, v1, v2)
103
104 #define ADD(dest,v1,v2) dAddVectors3(dest, v1, v2)
105
106 #define MULT(dest,v,factor) dCopyScaledVector3(dest, v, factor)
107
108 #define SET(dest,src) dCopyVector3(dest, src)
109
110 #define SMULT(p,q,s) dCopyScaledVector3(p, q, s)
111
112 #define COMBO(combo,p,t,q) dAddScaledVectors3(combo, p, q, REAL(1.0), t)
113
114 #define LENGTH(x) dCalcVectorLength3(x)
115
116 #define DEPTH(d, p, q, n) d = dCalcPointDepth3(q, p, n)
117
dMin(const dReal x,const dReal y)118 static inline const dReal dMin(const dReal x, const dReal y)
119 {
120 return x < y ? x : y;
121 }
122
123
124 inline void
SwapNormals(dVector3 * & pen_v,dVector3 * & col_v,dVector3 * v1,dVector3 * v2,dVector3 * & pen_elt,dVector3 * elt_f1,dVector3 * elt_f2,dVector3 n,dVector3 n1,dVector3 n2)125 SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2,
126 dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2,
127 dVector3 n, dVector3 n1, dVector3 n2)
128 {
129 if (pen_v == v1) {
130 pen_v = v2;
131 pen_elt = elt_f2;
132 col_v = v1;
133 SET(n, n1);
134 }
135 else {
136 pen_v = v1;
137 pen_elt = elt_f1;
138 col_v = v2;
139 SET(n, n2);
140 }
141 }
142
143 ///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE///////////////////////////////
144 ////* Written by Francisco Le�n (http://gimpact.sourceforge.net) *///
145 #define CONTACT_DIFF_EPSILON REAL(0.00001)
146 #if defined(dDOUBLE)
147 #define CONTACT_NORMAL_ZERO REAL(0.0000001)
148 #else // if defined(dSINGLE)
149 #define CONTACT_NORMAL_ZERO REAL(0.00001)
150 #endif
151 #define CONTACT_POS_HASH_QUOTIENT REAL(10000.0)
152 #define dSQRT3 REAL(1.7320508075688773)
153
UpdateContactKey(CONTACT_KEY & key,dContactGeom * contact)154 void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact)
155 {
156 key.m_contact = contact;
157
158 unsigned int hash=0;
159
160 int i = 0;
161
162 while (true)
163 {
164 dReal coord = contact->pos[i];
165 coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT);
166
167 const int sz = sizeof(coord) / sizeof(unsigned);
168 dIASSERT(sizeof(coord) % sizeof(unsigned) == 0);
169
170 unsigned hash_v[ sz ];
171 memcpy(hash_v, &coord, sizeof(coord));
172
173 unsigned int hash_input = hash_v[0];
174 for (int i=1; i<sz; ++i)
175 hash_input ^= hash_v[i];
176
177 hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 );
178 hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 );
179 hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 );
180 hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 );
181
182 if (++i == 3)
183 {
184 break;
185 }
186
187 hash = (hash << 11) | (hash >> 21);
188 }
189
190 key.m_key = hash;
191 }
192
193
MakeContactIndex(unsigned int key)194 static inline unsigned int MakeContactIndex(unsigned int key)
195 {
196 dIASSERT(CONTACTS_HASHSIZE == 256);
197
198 unsigned int index = key ^ (key >> 16);
199 index = (index ^ (index >> 8)) & 0xFF;
200
201 return index;
202 }
203
AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)204 dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)
205 {
206 for(int i=0;i<node->m_keycount;i++)
207 {
208 if(node->m_keyarray[i].m_key == contactkey->m_key)
209 {
210 dContactGeom *contactfound = node->m_keyarray[i].m_contact;
211 if (dCalcPointsDistance3(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/)
212 {
213 return contactfound;
214 }
215 }
216 }
217
218 if (node->m_keycount < MAXCONTACT_X_NODE)
219 {
220 node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact;
221 node->m_keyarray[node->m_keycount].m_key = contactkey->m_key;
222 node->m_keycount++;
223 }
224 else
225 {
226 dDEBUGMSG("Trimesh-trimesh contach hash table bucket overflow - close contacts might not be culled");
227 }
228
229 return contactkey->m_contact;
230 }
231
RemoveNewContactFromNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)232 void RemoveNewContactFromNode(const CONTACT_KEY * contactkey, CONTACT_KEY_HASH_NODE * node)
233 {
234 dIASSERT(node->m_keycount > 0);
235
236 if (node->m_keyarray[node->m_keycount - 1].m_contact == contactkey->m_contact)
237 {
238 node->m_keycount -= 1;
239 }
240 else
241 {
242 dIASSERT(node->m_keycount == MAXCONTACT_X_NODE);
243 }
244 }
245
RemoveArbitraryContactFromNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)246 void RemoveArbitraryContactFromNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node)
247 {
248 dIASSERT(node->m_keycount > 0);
249
250 int keyindex, lastkeyindex = node->m_keycount - 1;
251
252 // Do not check the last contact
253 for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
254 {
255 if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
256 {
257 node->m_keyarray[keyindex] = node->m_keyarray[lastkeyindex];
258 break;
259 }
260 }
261
262 dIASSERT(keyindex < lastkeyindex ||
263 node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
264
265 node->m_keycount = lastkeyindex;
266 }
267
UpdateArbitraryContactInNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node,dContactGeom * pwithcontact)268 void UpdateArbitraryContactInNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node,
269 dContactGeom *pwithcontact)
270 {
271 dIASSERT(node->m_keycount > 0);
272
273 int keyindex, lastkeyindex = node->m_keycount - 1;
274
275 // Do not check the last contact
276 for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
277 {
278 if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
279 {
280 break;
281 }
282 }
283
284 dIASSERT(keyindex < lastkeyindex ||
285 node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
286
287 node->m_keyarray[keyindex].m_contact = pwithcontact;
288 }
289
ClearContactSet(CONTACT_KEY_HASH_TABLE & hashcontactset)290 void ClearContactSet(CONTACT_KEY_HASH_TABLE &hashcontactset)
291 {
292 memset(&hashcontactset, 0, sizeof(CONTACT_KEY_HASH_TABLE));
293 }
294
295 //return true if found
InsertContactInSet(CONTACT_KEY_HASH_TABLE & hashcontactset,const CONTACT_KEY & newkey)296 dContactGeom *InsertContactInSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &newkey)
297 {
298 unsigned int index = MakeContactIndex(newkey.m_key);
299
300 return AddContactToNode(&newkey, &hashcontactset[index]);
301 }
302
RemoveNewContactFromSet(CONTACT_KEY_HASH_TABLE & hashcontactset,const CONTACT_KEY & contactkey)303 void RemoveNewContactFromSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey)
304 {
305 unsigned int index = MakeContactIndex(contactkey.m_key);
306
307 RemoveNewContactFromNode(&contactkey, &hashcontactset[index]);
308 }
309
RemoveArbitraryContactFromSet(CONTACT_KEY_HASH_TABLE & hashcontactset,const CONTACT_KEY & contactkey)310 void RemoveArbitraryContactFromSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey)
311 {
312 unsigned int index = MakeContactIndex(contactkey.m_key);
313
314 RemoveArbitraryContactFromNode(&contactkey, &hashcontactset[index]);
315 }
316
UpdateArbitraryContactInSet(CONTACT_KEY_HASH_TABLE & hashcontactset,const CONTACT_KEY & contactkey,dContactGeom * pwithcontact)317 void UpdateArbitraryContactInSet(CONTACT_KEY_HASH_TABLE &hashcontactset, const CONTACT_KEY &contactkey,
318 dContactGeom *pwithcontact)
319 {
320 unsigned int index = MakeContactIndex(contactkey.m_key);
321
322 UpdateArbitraryContactInNode(&contactkey, &hashcontactset[index], pwithcontact);
323 }
324
AllocNewContact(const dVector3 newpoint,dContactGeom * & out_pcontact,int Flags,CONTACT_KEY_HASH_TABLE & hashcontactset,dContactGeom * Contacts,int Stride,int & contactcount)325 bool AllocNewContact(
326 const dVector3 newpoint, dContactGeom *& out_pcontact,
327 int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset,
328 dContactGeom* Contacts, int Stride, int &contactcount)
329 {
330 bool allocated_new = false;
331
332 dContactGeom dLocalContact;
333
334 dContactGeom * pcontact = contactcount != (Flags & NUMC_MASK) ?
335 SAFECONTACT(Flags, Contacts, contactcount, Stride) : &dLocalContact;
336
337 pcontact->pos[0] = newpoint[0];
338 pcontact->pos[1] = newpoint[1];
339 pcontact->pos[2] = newpoint[2];
340 pcontact->pos[3] = 1.0f;
341
342 CONTACT_KEY newkey;
343 UpdateContactKey(newkey, pcontact);
344
345 dContactGeom *pcontactfound = InsertContactInSet(hashcontactset, newkey);
346 if (pcontactfound == pcontact)
347 {
348 if (pcontactfound != &dLocalContact)
349 {
350 contactcount++;
351 }
352 else
353 {
354 RemoveNewContactFromSet(hashcontactset, newkey);
355 pcontactfound = NULL;
356 }
357
358 allocated_new = true;
359 }
360
361 out_pcontact = pcontactfound;
362 return allocated_new;
363 }
364
FreeExistingContact(dContactGeom * pcontact,int Flags,CONTACT_KEY_HASH_TABLE & hashcontactset,dContactGeom * Contacts,int Stride,int & contactcount)365 void FreeExistingContact(dContactGeom *pcontact,
366 int Flags, CONTACT_KEY_HASH_TABLE &hashcontactset,
367 dContactGeom *Contacts, int Stride, int &contactcount)
368 {
369 CONTACT_KEY contactKey;
370 UpdateContactKey(contactKey, pcontact);
371
372 RemoveArbitraryContactFromSet(hashcontactset, contactKey);
373
374 int lastContactIndex = contactcount - 1;
375 dContactGeom *plastContact = SAFECONTACT(Flags, Contacts, lastContactIndex, Stride);
376
377 if (pcontact != plastContact)
378 {
379 *pcontact = *plastContact;
380
381 CONTACT_KEY lastContactKey;
382 UpdateContactKey(lastContactKey, plastContact);
383
384 UpdateArbitraryContactInSet(hashcontactset, lastContactKey, pcontact);
385 }
386
387 contactcount = lastContactIndex;
388 }
389
390
PushNewContact(dxGeom * g1,dxGeom * g2,int TriIndex1,int TriIndex2,const dVector3 point,dVector3 normal,dReal depth,int Flags,CONTACT_KEY_HASH_TABLE & hashcontactset,dContactGeom * Contacts,int Stride,int & contactcount)391 dContactGeom * PushNewContact( dxGeom* g1, dxGeom* g2, int TriIndex1, int TriIndex2,
392 const dVector3 point,
393 dVector3 normal,
394 dReal depth,
395 int Flags,
396 CONTACT_KEY_HASH_TABLE &hashcontactset,
397 dContactGeom* Contacts, int Stride,
398 int &contactcount)
399 {
400 dIASSERT(dFabs(dVector3Length((const dVector3 &)(*normal)) - REAL(1.0)) < REAL(1e-6)); // This assumption is used in the code
401
402 dContactGeom * pcontact;
403
404 if (!AllocNewContact(point, pcontact, Flags, hashcontactset, Contacts, Stride, contactcount))
405 {
406 const dReal depthDifference = depth - pcontact->depth;
407
408 if (depthDifference > CONTACT_DIFF_EPSILON)
409 {
410 pcontact->normal[0] = normal[0];
411 pcontact->normal[1] = normal[1];
412 pcontact->normal[2] = normal[2];
413 pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
414 pcontact->depth = depth;
415
416 pcontact->g1 = g1;
417 pcontact->g2 = g2;
418 pcontact->side1 = TriIndex1;
419 pcontact->side2 = TriIndex2;
420 }
421 else if (depthDifference >= -CONTACT_DIFF_EPSILON) ///average
422 {
423 if(pcontact->g1 == g2)
424 {
425 MULT(normal,normal, REAL(-1.0));
426 int tempInt = TriIndex1; TriIndex1 = TriIndex2; TriIndex2 = tempInt;
427 // This should be discarded by optimizer as g1 and g2 are
428 // not used any more but it's preferable to keep this line for
429 // the sake of consistency in variable values.
430 dxGeom *tempGeom = g1; g1 = g2; g2 = tempGeom;
431 }
432
433 const dReal oldLen = pcontact->normal[3];
434 COMBO(pcontact->normal, normal, oldLen, pcontact->normal);
435
436 const dReal len = LENGTH(pcontact->normal);
437 if (len > CONTACT_NORMAL_ZERO)
438 {
439 MULT(pcontact->normal, pcontact->normal, REAL(1.0) / len);
440 pcontact->normal[3] = len;
441
442 pcontact->side1 = ((dxTriMesh *)pcontact->g1)->TriMergeCallback ? ((dxTriMesh *)pcontact->g1)->TriMergeCallback((dxTriMesh *)pcontact->g1, pcontact->side1, TriIndex1) : -1;
443 pcontact->side2 = ((dxTriMesh *)pcontact->g2)->TriMergeCallback ? ((dxTriMesh *)pcontact->g2)->TriMergeCallback((dxTriMesh *)pcontact->g2, pcontact->side2, TriIndex2) : -1;
444 }
445 else
446 {
447 FreeExistingContact(pcontact, Flags, hashcontactset, Contacts, Stride, contactcount);
448 }
449 }
450 }
451 // Contact can be not available if buffer is full
452 else if (pcontact)
453 {
454 pcontact->normal[0] = normal[0];
455 pcontact->normal[1] = normal[1];
456 pcontact->normal[2] = normal[2];
457 pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
458 pcontact->depth = depth;
459 pcontact->g1 = g1;
460 pcontact->g2 = g2;
461 pcontact->side1 = TriIndex1;
462 pcontact->side2 = TriIndex2;
463 }
464
465 return pcontact;
466 }
467
468 ////////////////////////////////////////////////////////////////////////////////////////////
469
470
471
472
473
474 int
dCollideTTL(dxGeom * g1,dxGeom * g2,int Flags,dContactGeom * Contacts,int Stride)475 dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
476 {
477 dIASSERT (Stride >= (int)sizeof(dContactGeom));
478 dIASSERT (g1->type == dTriMeshClass);
479 dIASSERT (g2->type == dTriMeshClass);
480 dIASSERT ((Flags & NUMC_MASK) >= 1);
481
482 dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
483 dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
484
485 //dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals;
486 //dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals;
487
488 const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1);
489 // TLRotation1 = column-major order
490 const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1);
491
492 const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2);
493 // TLRotation2 = column-major order
494 const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2);
495
496 const unsigned uiTLSKind = TriMesh1->getParentSpaceTLSKind();
497 dIASSERT(uiTLSKind == TriMesh2->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method
498 TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind);
499 AABBTreeCollider& Collider = pccColliderCache->_AABBTreeCollider;
500 BVTCache &ColCache = pccColliderCache->ColCache;
501 CONTACT_KEY_HASH_TABLE &hashcontactset = pccColliderCache->_hashcontactset;
502
503 ColCache.Model0 = &TriMesh1->Data->BVTree;
504 ColCache.Model1 = &TriMesh2->Data->BVTree;
505
506 ////Prepare contact list
507 ClearContactSet(hashcontactset);
508
509 // Collision query
510 Matrix4x4 amatrix, bmatrix;
511 BOOL IsOk = Collider.Collide(ColCache,
512 &MakeMatrix(TLPosition1, TLRotation1, amatrix),
513 &MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
514
515
516 // Make "double" versions of these matrices, if appropriate
517 dMatrix4 A, B;
518 dMakeMatrix4(TLPosition1, TLRotation1, A);
519 dMakeMatrix4(TLPosition2, TLRotation2, B);
520
521
522
523
524 if (IsOk) {
525 // Get collision status => if true, objects overlap
526 if ( Collider.GetContactStatus() ) {
527 // Number of colliding pairs and list of pairs
528 int TriCount = Collider.GetNbPairs();
529 const Pair* CollidingPairs = Collider.GetPairs();
530
531 if (TriCount > 0) {
532 // step through the pairs, adding contacts
533 int id1, id2;
534 int OutTriCount = 0;
535 dVector3 v1[3], v2[3];
536
537 // only do these expensive inversions once
538 /*dMatrix4 InvMatrix1, InvMatrix2;
539 dInvertMatrix4(A, InvMatrix1);
540 dInvertMatrix4(B, InvMatrix2);*/
541
542
543 for (int i = 0; i < TriCount; i++)
544 {
545 id1 = CollidingPairs[i].id0;
546 id2 = CollidingPairs[i].id1;
547
548 // grab the colliding triangles
549 FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1);
550 FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2);
551
552 // Since we'll be doing matrix transformations, we need to
553 // make sure that all vertices have four elements
554 for (int j=0; j<3; j++) {
555 v1[j][3] = 1.0;
556 v2[j][3] = 1.0;
557 }
558
559 TriTriContacts(v1,v2, id1,id2,
560 g1, g2, Flags, hashcontactset,
561 Contacts,Stride,OutTriCount);
562
563 // Continue loop even after contacts are full
564 // as existing contacts' normals/depths might be updated
565 // Break only if contacts are not important
566 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
567 {
568 break;
569 }
570 }
571
572 // Return the number of contacts
573 return OutTriCount;
574
575 }
576 }
577 }
578
579
580 // There was some kind of failure during the Collide call or
581 // there are no faces overlapping
582 return 0;
583 }
584
585
586 /* -- not used
587 static void
588 GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data)
589 {
590 dVector3 Out[3];
591
592 FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out);
593
594 for (int i = 0; i < 3; i++)
595 triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]);
596 }
597 */
598
599 //
600 //
601 //
602 #define B11 B[0]
603 #define B12 B[1]
604 #define B13 B[2]
605 #define B14 B[3]
606 #define B21 B[4]
607 #define B22 B[5]
608 #define B23 B[6]
609 #define B24 B[7]
610 #define B31 B[8]
611 #define B32 B[9]
612 #define B33 B[10]
613 #define B34 B[11]
614 #define B41 B[12]
615 #define B42 B[13]
616 #define B43 B[14]
617 #define B44 B[15]
618
619 #define Binv11 Binv[0]
620 #define Binv12 Binv[1]
621 #define Binv13 Binv[2]
622 #define Binv14 Binv[3]
623 #define Binv21 Binv[4]
624 #define Binv22 Binv[5]
625 #define Binv23 Binv[6]
626 #define Binv24 Binv[7]
627 #define Binv31 Binv[8]
628 #define Binv32 Binv[9]
629 #define Binv33 Binv[10]
630 #define Binv34 Binv[11]
631 #define Binv41 Binv[12]
632 #define Binv42 Binv[13]
633 #define Binv43 Binv[14]
634 #define Binv44 Binv[15]
635
636 inline void
dMakeMatrix4(const dVector3 Position,const dMatrix3 Rotation,dMatrix4 & B)637 dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B)
638 {
639 B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0];
640 B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1];
641 B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2];
642
643 B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0;
644 }
645
646 #if 0
647 static void
648 dInvertMatrix4( dMatrix4& B, dMatrix4& Binv )
649 {
650 dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43)
651 -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42)
652 +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42)
653 +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41)
654 -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41)
655 +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41);
656
657 dAASSERT (det != 0.0);
658
659 det = 1.0 / det;
660
661 Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32)));
662 Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12)));
663 Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22)));
664 Binv14 = 0.0f;
665 Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33)));
666 Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13)));
667 Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23)));
668 Binv24 = 0.0f;
669 Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31)));
670 Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11)));
671 Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21)));
672 Binv34 = 0.0f;
673 Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42)));
674 Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42)));
675 Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22)));
676 Binv44 = 1.0f;
677 }
678 #endif
679
680
681 // Find the intersectiojn point between a coplanar line segement,
682 // defined by X1 and X2, and a ray defined by X3 and direction N.
683 //
684 // This forumla for this calculation is:
685 // (c x b) . (a x b)
686 // Q = x1 + a -------------------
687 // | a x b | ^2
688 //
689 // where a = x2 - x1
690 // b = x4 - x3
691 // c = x3 - x1
692 // x1 and x2 are the edges of the triangle, and x3 is CoplanarPt
693 // and x4 is (CoplanarPt - n)
694 #if 0
695 static int
696 IntersectLineSegmentRay(dVector3 x1, dVector3 x2, dVector3 x3, dVector3 n,
697 dVector3 out_pt)
698 {
699 dVector3 a, b, c, x4;
700
701 ADD(x4, x3, n); // x4 = x3 + n
702
703 SUB(a, x2, x1); // a = x2 - x1
704 SUB(b, x4, x3);
705 SUB(c, x3, x1);
706
707 dVector3 tmp1, tmp2;
708 CROSS(tmp1, c, b);
709 CROSS(tmp2, a, b);
710
711 dReal num, denom;
712 num = dCalcVectorDot3(tmp1, tmp2);
713 denom = LENGTH( tmp2 );
714
715 dReal s;
716 s = num /(denom*denom);
717
718 for (int i=0; i<3; i++)
719 out_pt[i] = x1[i] + a[i]*s;
720
721 // Test if this intersection is "behind" x3, w.r.t. n
722 SUB(a, x3, out_pt);
723 if (dCalcVectorDot3(a, n) > 0.0)
724 return 0;
725
726 // Test if this intersection point is outside the edge limits,
727 // if (dot( (out_pt-x1), (out_pt-x2) ) < 0) it's inside
728 // else outside
729 SUB(a, out_pt, x1);
730 SUB(b, out_pt, x2);
731 if (dCalcVectorDot3(a,b) < 0.0)
732 return 1;
733 else
734 return 0;
735 }
736 #endif
737
738
PlaneClipSegment(const dVector3 s1,const dVector3 s2,const dVector3 N,dReal C,dVector3 clipped)739 void PlaneClipSegment( const dVector3 s1, const dVector3 s2,
740 const dVector3 N, dReal C, dVector3 clipped)
741 {
742 dReal dis1,dis2;
743 dis1 = DOT(s1,N)-C;
744 SUB(clipped,s2,s1);
745 dis2 = DOT(clipped,N);
746 MULT(clipped,clipped,-dis1/dis2);
747 ADD(clipped,clipped,s1);
748 clipped[3] = 1.0f;
749 }
750
751 /* ClipConvexPolygonAgainstPlane - Clip a a convex polygon, described by
752 CONTACTS, with a plane (described by N and C distance from origin).
753 Note: the input vertices are assumed to be in invcounterclockwise order.
754 changed by Francisco Leon (http://gimpact.sourceforge.net) */
755 static void
ClipConvexPolygonAgainstPlane(const dVector3 N,dReal C,LineContactSet & Contacts)756 ClipConvexPolygonAgainstPlane( const dVector3 N, dReal C,
757 LineContactSet& Contacts )
758 {
759 int i, vi, prevclassif=32000, classif;
760 /*
761 classif 0 : back, 1 : front
762 */
763
764 dReal d;
765 dVector3 clipped[8];
766 int clippedcount =0;
767
768 if(Contacts.Count==0)
769 {
770 return;
771 }
772 for(i=0;i<=Contacts.Count;i++)
773 {
774 vi = i%Contacts.Count;
775
776 d = DOT(N,Contacts.Points[vi]) - C;
777 ////classify point
778 if(d>REAL(1.0e-8)) classif = 1;
779 else classif = 0;
780
781 if(classif == 0)//back
782 {
783 if(i>0)
784 {
785 if(prevclassif==1)///in front
786 {
787
788 ///add clipped point
789 if(clippedcount<8)
790 {
791 PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi],
792 N,C,clipped[clippedcount]);
793 clippedcount++;
794 }
795 }
796 }
797 ///add point
798 if(clippedcount<8&&i<Contacts.Count)
799 {
800 clipped[clippedcount][0] = Contacts.Points[vi][0];
801 clipped[clippedcount][1] = Contacts.Points[vi][1];
802 clipped[clippedcount][2] = Contacts.Points[vi][2];
803 clipped[clippedcount][3] = 1.0f;
804 clippedcount++;
805 }
806 }
807 else
808 {
809
810 if(i>0)
811 {
812 if(prevclassif==0)
813 {
814 ///add point
815 if(clippedcount<8)
816 {
817 PlaneClipSegment(Contacts.Points[i-1],Contacts.Points[vi],
818 N,C,clipped[clippedcount]);
819 clippedcount++;
820 }
821 }
822 }
823 }
824
825 prevclassif = classif;
826 }
827
828 if(clippedcount==0)
829 {
830 Contacts.Count = 0;
831 return;
832 }
833 Contacts.Count = clippedcount;
834 memcpy( Contacts.Points, clipped, clippedcount * sizeof(dVector3) );
835 return;
836 }
837
838
BuildPlane(const dVector3 s0,const dVector3 s1,const dVector3 s2,dVector3 Normal,dReal & Dist)839 bool BuildPlane(const dVector3 s0, const dVector3 s1,const dVector3 s2,
840 dVector3 Normal, dReal & Dist)
841 {
842 dVector3 e0,e1;
843 SUB(e0,s1,s0);
844 SUB(e1,s2,s0);
845
846 CROSS(Normal,e0,e1);
847
848 if (!dSafeNormalize3(Normal))
849 {
850 return false;
851 }
852
853 Dist = DOT(Normal,s0);
854 return true;
855
856 }
857
BuildEdgesDir(const dVector3 s0,const dVector3 s1,const dVector3 t0,const dVector3 t1,dVector3 crossdir)858 bool BuildEdgesDir(const dVector3 s0, const dVector3 s1,
859 const dVector3 t0, const dVector3 t1,
860 dVector3 crossdir)
861 {
862 dVector3 e0,e1;
863
864 SUB(e0,s1,s0);
865 SUB(e1,t1,t0);
866 CROSS(crossdir,e0,e1);
867
868 if (!dSafeNormalize3(crossdir))
869 {
870 return false;
871 }
872 return true;
873 }
874
875
876
BuildEdgePlane(const dVector3 s0,const dVector3 s1,const dVector3 normal,dVector3 plane_normal,dReal & plane_dist)877 bool BuildEdgePlane(
878 const dVector3 s0, const dVector3 s1,
879 const dVector3 normal,
880 dVector3 plane_normal,
881 dReal & plane_dist)
882 {
883 dVector3 e0;
884
885 SUB(e0,s1,s0);
886 CROSS(plane_normal,e0,normal);
887 if (!dSafeNormalize3(plane_normal))
888 {
889 return false;
890 }
891 plane_dist = DOT(plane_normal,s0);
892 return true;
893 }
894
895
896
897
898 /*
899 Positive penetration
900 Negative number: they are separated
901 */
IntervalPenetration(dReal & vmin1,dReal & vmax1,dReal & vmin2,dReal & vmax2)902 dReal IntervalPenetration(dReal &vmin1,dReal &vmax1,
903 dReal &vmin2,dReal &vmax2)
904 {
905 if(vmax1<=vmin2)
906 {
907 return -(vmin2-vmax1);
908 }
909 else
910 {
911 if(vmax2<=vmin1)
912 {
913 return -(vmin1-vmax2);
914 }
915 else
916 {
917 if(vmax1<=vmax2)
918 {
919 return vmax1-vmin2;
920 }
921
922 return vmax2-vmin1;
923 }
924
925 }
926 return 0;
927 }
928
FindInterval(const dVector3 * vertices,int verticecount,dVector3 dir,dReal & vmin,dReal & vmax)929 void FindInterval(
930 const dVector3 * vertices, int verticecount,
931 dVector3 dir,dReal &vmin,dReal &vmax)
932 {
933
934 dReal dist;
935 int i;
936 vmin = DOT(vertices[0],dir);
937 vmax = vmin;
938 for(i=1;i<verticecount;i++)
939 {
940 dist = DOT(vertices[i],dir);
941 if(vmin>dist) vmin=dist;
942 else if(vmax<dist) vmax=dist;
943 }
944 }
945
946 ///returns the penetration depth
MostDeepPoints(LineContactSet & points,const dVector3 plane_normal,dReal plane_dist,LineContactSet & deep_points)947 dReal MostDeepPoints(
948 LineContactSet & points,
949 const dVector3 plane_normal,
950 dReal plane_dist,
951 LineContactSet & deep_points)
952 {
953 int i;
954 int max_candidates[8];
955 dReal maxdeep=-dInfinity;
956 dReal dist;
957
958 deep_points.Count = 0;
959 for(i=0;i<points.Count;i++)
960 {
961 dist = DOT(plane_normal,points.Points[i]) - plane_dist;
962 dist *= -1.0f;
963 if(dist>maxdeep)
964 {
965 maxdeep = dist;
966 deep_points.Count=1;
967 max_candidates[deep_points.Count-1] = i;
968 }
969 else if(dist+REAL(0.000001)>=maxdeep)
970 {
971 deep_points.Count++;
972 max_candidates[deep_points.Count-1] = i;
973 }
974 }
975
976 for(i=0;i<deep_points.Count;i++)
977 {
978 SET(deep_points.Points[i],points.Points[max_candidates[i]]);
979 }
980 return maxdeep;
981
982 }
983
ClipPointsByTri(const dVector3 * points,int pointcount,const dVector3 tri[3],const dVector3 triplanenormal,dReal triplanedist,LineContactSet & clipped_points,bool triplane_clips)984 void ClipPointsByTri(
985 const dVector3 * points, int pointcount,
986 const dVector3 tri[3],
987 const dVector3 triplanenormal,
988 dReal triplanedist,
989 LineContactSet & clipped_points,
990 bool triplane_clips)
991 {
992 ///build edges planes
993 int i;
994 dVector4 plane;
995
996 clipped_points.Count = pointcount;
997 memcpy(&clipped_points.Points[0],&points[0],pointcount*sizeof(dVector3));
998 for(i=0;i<3;i++)
999 {
1000 if (BuildEdgePlane(
1001 tri[i],tri[(i+1)%3],triplanenormal,
1002 plane,plane[3]))
1003 {
1004 ClipConvexPolygonAgainstPlane(
1005 plane,
1006 plane[3],
1007 clipped_points);
1008 }
1009 }
1010
1011 if(triplane_clips)
1012 {
1013 ClipConvexPolygonAgainstPlane(
1014 triplanenormal,
1015 triplanedist,
1016 clipped_points);
1017 }
1018 }
1019
1020
1021 ///returns the penetration depth
FindTriangleTriangleCollision(const dVector3 tri1[3],const dVector3 tri2[3],dVector3 separating_normal,LineContactSet & deep_points)1022 dReal FindTriangleTriangleCollision(
1023 const dVector3 tri1[3],
1024 const dVector3 tri2[3],
1025 dVector3 separating_normal,
1026 LineContactSet & deep_points)
1027 {
1028 dReal maxdeep=dInfinity;
1029 dReal dist;
1030 int mostdir=0, /*mostface=0,*/ currdir=0;
1031 // dReal vmin1,vmax1,vmin2,vmax2;
1032 // dVector3 crossdir, pt1,pt2;
1033 dVector4 tri1plane,tri2plane;
1034 separating_normal[3] = 0.0f;
1035 bool bl;
1036 LineContactSet clipped_points1,clipped_points2;
1037 LineContactSet deep_points1,deep_points2;
1038 // It is necessary to initialize the count because both conditional statements
1039 // might be skipped leading to uninitialized count being used for memcpy in if(mostdir==0)
1040 deep_points1.Count = 0;
1041
1042 ////find interval face1
1043
1044 bl = BuildPlane(tri1[0],tri1[1],tri1[2],
1045 tri1plane,tri1plane[3]);
1046 clipped_points1.Count = 0;
1047
1048 if(bl)
1049 {
1050 ClipPointsByTri(
1051 tri2, 3,
1052 tri1,
1053 tri1plane,
1054 tri1plane[3],
1055 clipped_points1,false);
1056
1057
1058
1059 maxdeep = MostDeepPoints(
1060 clipped_points1,
1061 tri1plane,
1062 tri1plane[3],
1063 deep_points1);
1064 SET(separating_normal,tri1plane);
1065
1066 }
1067 currdir++;
1068
1069 ////find interval face2
1070
1071 bl = BuildPlane(tri2[0],tri2[1],tri2[2],
1072 tri2plane,tri2plane[3]);
1073
1074
1075 clipped_points2.Count = 0;
1076 if(bl)
1077 {
1078 ClipPointsByTri(
1079 tri1, 3,
1080 tri2,
1081 tri2plane,
1082 tri2plane[3],
1083 clipped_points2,false);
1084
1085
1086
1087 dist = MostDeepPoints(
1088 clipped_points2,
1089 tri2plane,
1090 tri2plane[3],
1091 deep_points2);
1092
1093
1094
1095 if(dist<maxdeep)
1096 {
1097 maxdeep = dist;
1098 mostdir = currdir;
1099 //mostface = 1;
1100 SET(separating_normal,tri2plane);
1101 }
1102 }
1103 currdir++;
1104
1105
1106 ///find edge edge distances
1107 ///test each edge plane
1108
1109 /*for(i=0;i<3;i++)
1110 {
1111
1112
1113 for(j=0;j<3;j++)
1114 {
1115
1116
1117 bl = BuildEdgesDir(
1118 tri1[i],tri1[(i+1)%3],
1119 tri2[j],tri2[(j+1)%3],
1120 crossdir);
1121
1122 ////find plane distance
1123
1124 if(bl)
1125 {
1126 FindInterval(tri1,3,crossdir,vmin1,vmax1);
1127 FindInterval(tri2,3,crossdir,vmin2,vmax2);
1128
1129 dist = IntervalPenetration(
1130 vmin1,
1131 vmax1,
1132 vmin2,
1133 vmax2);
1134 if(dist<maxdeep)
1135 {
1136 maxdeep = dist;
1137 mostdir = currdir;
1138 SET(separating_normal,crossdir);
1139 }
1140 }
1141 currdir++;
1142 }
1143 }*/
1144
1145
1146 ////check most dir for contacts
1147 if(mostdir==0)
1148 {
1149 ///find most deep points
1150 deep_points.Count = deep_points1.Count;
1151 memcpy(
1152 &deep_points.Points[0],
1153 &deep_points1.Points[0],
1154 deep_points1.Count*sizeof(dVector3));
1155
1156 ///invert normal for point to tri1
1157 MULT(separating_normal,separating_normal,-1.0f);
1158 }
1159 else if(mostdir==1)
1160 {
1161 deep_points.Count = deep_points2.Count;
1162 memcpy(
1163 &deep_points.Points[0],
1164 &deep_points2.Points[0],
1165 deep_points2.Count*sizeof(dVector3));
1166
1167 }
1168 /*else
1169 {///edge separation
1170 mostdir -= 2;
1171
1172 //edge 2
1173 j = mostdir%3;
1174 //edge 1
1175 i = mostdir/3;
1176
1177 ///find edge closest points
1178 dClosestLineSegmentPoints(
1179 tri1[i],tri1[(i+1)%3],
1180 tri2[j],tri2[(j+1)%3],
1181 pt1,pt2);
1182 ///find correct direction
1183
1184 SUB(crossdir,pt2,pt1);
1185
1186 vmin1 = LENGTH(crossdir);
1187 if(vmin1<REAL(0.000001))
1188 {
1189
1190 if(mostface==0)
1191 {
1192 vmin1 = DOT(separating_normal,tri1plane);
1193 if(vmin1>0.0)
1194 {
1195 MULT(separating_normal,separating_normal,-1.0f);
1196 deep_points.Count = 1;
1197 SET(deep_points.Points[0],pt2);
1198 }
1199 else
1200 {
1201 deep_points.Count = 1;
1202 SET(deep_points.Points[0],pt2);
1203 }
1204
1205 }
1206 else
1207 {
1208 vmin1 = DOT(separating_normal,tri2plane);
1209 if(vmin1<0.0)
1210 {
1211 MULT(separating_normal,separating_normal,-1.0f);
1212 deep_points.Count = 1;
1213 SET(deep_points.Points[0],pt2);
1214 }
1215 else
1216 {
1217 deep_points.Count = 1;
1218 SET(deep_points.Points[0],pt2);
1219 }
1220
1221 }
1222
1223
1224
1225
1226 }
1227 else
1228 {
1229 MULT(separating_normal,crossdir,1.0f/vmin1);
1230
1231 vmin1 = DOT(separating_normal,tri1plane);
1232 if(vmin1>0.0)
1233 {
1234 MULT(separating_normal,separating_normal,-1.0f);
1235 deep_points.Count = 1;
1236 SET(deep_points.Points[0],pt2);
1237 }
1238 else
1239 {
1240 deep_points.Count = 1;
1241 SET(deep_points.Points[0],pt2);
1242 }
1243
1244
1245 }
1246
1247
1248 }*/
1249 return maxdeep;
1250 }
1251
1252
1253
1254 ///SUPPORT UP TO 8 CONTACTS
TriTriContacts(const dVector3 tr1[3],const dVector3 tr2[3],int TriIndex1,int TriIndex2,dxGeom * g1,dxGeom * g2,int Flags,CONTACT_KEY_HASH_TABLE & hashcontactset,dContactGeom * Contacts,int Stride,int & contactcount)1255 bool TriTriContacts(const dVector3 tr1[3],
1256 const dVector3 tr2[3],
1257 int TriIndex1, int TriIndex2,
1258 dxGeom* g1, dxGeom* g2, int Flags,
1259 CONTACT_KEY_HASH_TABLE &hashcontactset,
1260 dContactGeom* Contacts, int Stride,
1261 int &contactcount)
1262 {
1263
1264
1265 dVector4 normal;
1266 dReal depth;
1267 ///Test Tri Vs Tri
1268 // dContactGeom* pcontact;
1269 int ccount = 0;
1270 LineContactSet contactpoints;
1271 contactpoints.Count = 0;
1272
1273
1274
1275 ///find best direction
1276
1277 depth = FindTriangleTriangleCollision(
1278 tr1,
1279 tr2,
1280 normal,
1281 contactpoints);
1282
1283
1284
1285 if(depth<0.0f) return false;
1286
1287 ccount = 0;
1288 while (ccount<contactpoints.Count)
1289 {
1290 PushNewContact( g1, g2, TriIndex1, TriIndex2,
1291 contactpoints.Points[ccount],
1292 normal, depth, Flags, hashcontactset,
1293 Contacts,Stride,contactcount);
1294
1295 // Continue loop even after contacts are full
1296 // as existing contacts' normals/depths might be updated
1297 // Break only if contacts are not important
1298 if ((contactcount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
1299 {
1300 break;
1301 }
1302
1303 ccount++;
1304 }
1305 return true;
1306 }
1307
1308 #endif // dTRIMESH_OPCODE
1309 #endif // !dTRIMESH_USE_OLD_TRIMESH_TRIMESH_COLLIDER
1310 #endif // dTRIMESH_ENABLED
1311