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