aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:22:23 +0000
committerdan miller2007-10-19 05:22:23 +0000
commit1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43 (patch)
tree51bcae7a1b8381a6bf6fd8025a7de1e30fe0045d /libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp
parentone more for the gipper (diff)
downloadopensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.zip
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.gz
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.bz2
opensim-SC-1ec410ecd725f5a3ccb2d2fc16f48730d9d9fe43.tar.xz
trying to fix my screwup, please hold on
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp1304
1 files changed, 0 insertions, 1304 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp b/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp
deleted file mode 100644
index c4af41c..0000000
--- a/libraries/ode-0.9/ode/src/collision_trimesh_trimesh_new.cpp
+++ /dev/null
@@ -1,1304 +0,0 @@
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
35// New Implementation
36#if dTRIMESH_OPCODE_USE_NEW_TRIMESH_TRIMESH_COLLIDER
37
38#if dTRIMESH_ENABLED
39
40#include "collision_util.h"
41#define TRIMESH_INTERNAL
42#include "collision_trimesh_internal.h"
43
44#if dTRIMESH_OPCODE
45
46#define SMALL_ELT REAL(2.5e-4)
47#define EXPANDED_ELT_THRESH REAL(1.0e-3)
48#define DISTANCE_EPSILON REAL(1.0e-8)
49#define VELOCITY_EPSILON REAL(1.0e-5)
50#define TINY_PENETRATION REAL(5.0e-6)
51
52struct LineContactSet
53{
54 enum
55 {
56 MAX_POINTS = 8
57 };
58
59 dVector3 Points[MAX_POINTS];
60 int Count;
61};
62
63
64static void GetTriangleGeometryCallback(udword, VertexPointers&, udword);
65inline void dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B);
66static void dInvertMatrix4( dMatrix4& B, dMatrix4& Binv );
67static int IntersectLineSegmentRay(dVector3, dVector3, dVector3, dVector3, dVector3);
68static void ClipConvexPolygonAgainstPlane( const dVector3, dReal, LineContactSet& );
69static int RayTriangleIntersect(const dVector3 orig, const dVector3 dir,
70 const dVector3 vert0, const dVector3 vert1,const dVector3 vert2,
71 dReal *t,dReal *u,dReal *v);
72
73
74///returns the penetration depth
75static dReal MostDeepPoints(
76 LineContactSet & points,
77 const dVector3 plane_normal,
78 dReal plane_dist,
79 LineContactSet & deep_points);
80///returns the penetration depth
81static dReal FindMostDeepPointsInTetra(
82 LineContactSet contact_points,
83 const dVector3 sourcetri[3],///triangle which contains contact_points
84 const dVector3 tetra[4],
85 const dVector4 tetraplanes[4],
86 dVector3 separating_normal,
87 LineContactSet deep_points);
88
89static bool ClipTriByTetra(const dVector3 tri[3],
90 const dVector3 tetra[4],
91 LineContactSet& Contacts);
92static bool TriTriContacts(const dVector3 tr1[3],
93 const dVector3 tr2[3],
94 dxGeom* g1, dxGeom* g2, int Flags,
95 dContactGeom* Contacts, int Stride,
96 int &contactcount);
97
98
99/* some math macros */
100#define CROSS(dest,v1,v2) { dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
101 dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
102 dest[2]=v1[0]*v2[1]-v1[1]*v2[0]; }
103
104#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
105
106#define SUB(dest,v1,v2) { dest[0]=v1[0]-v2[0]; dest[1]=v1[1]-v2[1]; dest[2]=v1[2]-v2[2]; }
107
108#define ADD(dest,v1,v2) { dest[0]=v1[0]+v2[0]; dest[1]=v1[1]+v2[1]; dest[2]=v1[2]+v2[2]; }
109
110#define MULT(dest,v,factor) { dest[0]=factor*v[0]; dest[1]=factor*v[1]; dest[2]=factor*v[2]; }
111
112#define SET(dest,src) { dest[0]=src[0]; dest[1]=src[1]; dest[2]=src[2]; }
113
114#define SMULT(p,q,s) { p[0]=q[0]*s; p[1]=q[1]*s; p[2]=q[2]*s; }
115
116#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]; }
117
118#define LENGTH(x) ((dReal) 1.0f/InvSqrt(dDOT(x, x)))
119
120#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];
121
122inline const dReal dMin(const dReal x, const dReal y)
123{
124 return x < y ? x : y;
125}
126
127
128inline void
129SwapNormals(dVector3 *&pen_v, dVector3 *&col_v, dVector3* v1, dVector3* v2,
130 dVector3 *&pen_elt, dVector3 *elt_f1, dVector3 *elt_f2,
131 dVector3 n, dVector3 n1, dVector3 n2)
132{
133 if (pen_v == v1) {
134 pen_v = v2;
135 pen_elt = elt_f2;
136 col_v = v1;
137 SET(n, n1);
138 }
139 else {
140 pen_v = v1;
141 pen_elt = elt_f1;
142 col_v = v2;
143 SET(n, n2);
144 }
145}
146
147///////////////////////MECHANISM FOR AVOID CONTACT REDUNDANCE///////////////////////////////
148////* Written by Francisco León (http://gimpact.sourceforge.net) *///
149#define CONTACT_DIFF_EPSILON REAL(0.00001)
150#if defined(dDOUBLE)
151#define CONTACT_NORMAL_ZERO REAL(0.0000001)
152#else // if defined(dSINGLE)
153#define CONTACT_NORMAL_ZERO REAL(0.00001)
154#endif
155#define CONTACT_POS_HASH_QUOTIENT REAL(10000.0)
156#define dSQRT3 REAL(1.7320508075688773)
157
158struct CONTACT_KEY
159{
160 dContactGeom * m_contact;
161 unsigned int m_key;
162};
163
164#define MAXCONTACT_X_NODE 4
165struct CONTACT_KEY_HASH_NODE
166{
167 CONTACT_KEY m_keyarray[MAXCONTACT_X_NODE];
168 int m_keycount;
169};
170
171#define CONTACTS_HASHSIZE 256
172CONTACT_KEY_HASH_NODE g_hashcontactset[CONTACTS_HASHSIZE];
173
174
175
176void UpdateContactKey(CONTACT_KEY & key, dContactGeom * contact)
177{
178 key.m_contact = contact;
179
180 unsigned int hash=0;
181
182 int i = 0;
183
184 while (true)
185 {
186 dReal coord = contact->pos[i];
187 coord = dFloor(coord * CONTACT_POS_HASH_QUOTIENT);
188
189 unsigned int hash_input = ((unsigned int *)&coord)[0];
190 if (sizeof(dReal) / sizeof(unsigned int) != 1)
191 {
192 dIASSERT(sizeof(dReal) / sizeof(unsigned int) == 2);
193 hash_input ^= ((unsigned int *)&coord)[1];
194 }
195
196 hash = (( hash << 4 ) + (hash_input >> 24)) ^ ( hash >> 28 );
197 hash = (( hash << 4 ) + ((hash_input >> 16) & 0xFF)) ^ ( hash >> 28 );
198 hash = (( hash << 4 ) + ((hash_input >> 8) & 0xFF)) ^ ( hash >> 28 );
199 hash = (( hash << 4 ) + (hash_input & 0xFF)) ^ ( hash >> 28 );
200
201 if (++i == 3)
202 {
203 break;
204 }
205
206 hash = (hash << 11) | (hash >> 21);
207 }
208
209 key.m_key = hash;
210}
211
212
213static inline unsigned int MakeContactIndex(unsigned int key)
214{
215 dIASSERT(CONTACTS_HASHSIZE == 256);
216
217 unsigned int index = key ^ (key >> 16);
218 index = (index ^ (index >> 8)) & 0xFF;
219 return index;
220}
221
222dContactGeom *AddContactToNode(const CONTACT_KEY * contactkey,CONTACT_KEY_HASH_NODE * node)
223{
224 for(int i=0;i<node->m_keycount;i++)
225 {
226 if(node->m_keyarray[i].m_key == contactkey->m_key)
227 {
228 dContactGeom *contactfound = node->m_keyarray[i].m_contact;
229 if (dDISTANCE(contactfound->pos, contactkey->m_contact->pos) < REAL(1.00001) /*for comp. errors*/ * dSQRT3 / CONTACT_POS_HASH_QUOTIENT /*cube diagonal*/)
230 {
231 return contactfound;
232 }
233 }
234 }
235
236 if (node->m_keycount < MAXCONTACT_X_NODE)
237 {
238 node->m_keyarray[node->m_keycount].m_contact = contactkey->m_contact;
239 node->m_keyarray[node->m_keycount].m_key = contactkey->m_key;
240 node->m_keycount++;
241 }
242 else
243 {
244 dDEBUGMSG("Trimesh-trimesh contach hash table bucket overflow - close contacts might not be culled");
245 }
246
247 return contactkey->m_contact;
248}
249
250void RemoveNewContactFromNode(const CONTACT_KEY * contactkey, CONTACT_KEY_HASH_NODE * node)
251{
252 dIASSERT(node->m_keycount > 0);
253
254 if (node->m_keyarray[node->m_keycount - 1].m_contact == contactkey->m_contact)
255 {
256 node->m_keycount -= 1;
257 }
258 else
259 {
260 dIASSERT(node->m_keycount == MAXCONTACT_X_NODE);
261 }
262}
263
264void RemoveArbitraryContactFromNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node)
265{
266 dIASSERT(node->m_keycount > 0);
267
268 int keyindex, lastkeyindex = node->m_keycount - 1;
269
270 // Do not check the last contact
271 for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
272 {
273 if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
274 {
275 node->m_keyarray[keyindex] = node->m_keyarray[lastkeyindex];
276 break;
277 }
278 }
279
280 dIASSERT(keyindex < lastkeyindex ||
281 node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
282
283 node->m_keycount = lastkeyindex;
284}
285
286void UpdateArbitraryContactInNode(const CONTACT_KEY *contactkey, CONTACT_KEY_HASH_NODE *node,
287 dContactGeom *pwithcontact)
288{
289 dIASSERT(node->m_keycount > 0);
290
291 int keyindex, lastkeyindex = node->m_keycount - 1;
292
293 // Do not check the last contact
294 for (keyindex = 0; keyindex < lastkeyindex; keyindex++)
295 {
296 if (node->m_keyarray[keyindex].m_contact == contactkey->m_contact)
297 {
298 break;
299 }
300 }
301
302 dIASSERT(keyindex < lastkeyindex ||
303 node->m_keyarray[keyindex].m_contact == contactkey->m_contact); // It has been either the break from loop or last element should match
304
305 node->m_keyarray[keyindex].m_contact = pwithcontact;
306}
307
308void ClearContactSet()
309{
310 memset(g_hashcontactset,0,sizeof(CONTACT_KEY_HASH_NODE)*CONTACTS_HASHSIZE);
311}
312
313//return true if found
314dContactGeom *InsertContactInSet(const CONTACT_KEY &newkey)
315{
316 unsigned int index = MakeContactIndex(newkey.m_key);
317
318 return AddContactToNode(&newkey, &g_hashcontactset[index]);
319}
320
321void RemoveNewContactFromSet(const CONTACT_KEY &contactkey)
322{
323 unsigned int index = MakeContactIndex(contactkey.m_key);
324
325 RemoveNewContactFromNode(&contactkey, &g_hashcontactset[index]);
326}
327
328void RemoveArbitraryContactFromSet(const CONTACT_KEY &contactkey)
329{
330 unsigned int index = MakeContactIndex(contactkey.m_key);
331
332 RemoveArbitraryContactFromNode(&contactkey, &g_hashcontactset[index]);
333}
334
335void UpdateArbitraryContactInSet(const CONTACT_KEY &contactkey,
336 dContactGeom *pwithcontact)
337{
338 unsigned int index = MakeContactIndex(contactkey.m_key);
339
340 UpdateArbitraryContactInNode(&contactkey, &g_hashcontactset[index], pwithcontact);
341}
342
343bool AllocNewContact(
344 const dVector3 newpoint, dContactGeom *& out_pcontact,
345 int Flags, dContactGeom* Contacts,
346 int Stride, int &contactcount)
347{
348 bool allocated_new = false;
349
350 dContactGeom dLocalContact;
351
352 dContactGeom * pcontact = contactcount != (Flags & NUMC_MASK) ?
353 SAFECONTACT(Flags, Contacts, contactcount, Stride) : &dLocalContact;
354
355 pcontact->pos[0] = newpoint[0];
356 pcontact->pos[1] = newpoint[1];
357 pcontact->pos[2] = newpoint[2];
358 pcontact->pos[3] = 1.0f;
359
360 CONTACT_KEY newkey;
361 UpdateContactKey(newkey, pcontact);
362
363 dContactGeom *pcontactfound = InsertContactInSet(newkey);
364 if (pcontactfound == pcontact)
365 {
366 if (pcontactfound != &dLocalContact)
367 {
368 contactcount++;
369 }
370 else
371 {
372 RemoveNewContactFromSet(newkey);
373 pcontactfound = NULL;
374 }
375
376 allocated_new = true;
377 }
378
379 out_pcontact = pcontactfound;
380 return allocated_new;
381}
382
383void FreeExistingContact(dContactGeom *pcontact,
384 int Flags, dContactGeom *Contacts,
385 int Stride, int &contactcount)
386{
387 CONTACT_KEY contactKey;
388 UpdateContactKey(contactKey, pcontact);
389
390 RemoveArbitraryContactFromSet(contactKey);
391
392 int lastContactIndex = contactcount - 1;
393 dContactGeom *plastContact = SAFECONTACT(Flags, Contacts, lastContactIndex, Stride);
394
395 if (pcontact != plastContact)
396 {
397 *pcontact = *plastContact;
398
399 CONTACT_KEY lastContactKey;
400 UpdateContactKey(lastContactKey, plastContact);
401
402 UpdateArbitraryContactInSet(lastContactKey, pcontact);
403 }
404
405 contactcount = lastContactIndex;
406}
407
408
409dContactGeom * PushNewContact( dxGeom* g1, dxGeom* g2,
410 const dVector3 point,
411 dVector3 normal,
412 dReal depth,
413 int Flags,
414 dContactGeom* Contacts, int Stride,
415 int &contactcount)
416{
417 dIASSERT(dFabs(dVector3Length((const dVector3 &)(*normal)) - REAL(1.0)) < REAL(1e-6)); // This assumption is used in the code
418
419 dContactGeom * pcontact;
420
421 if (!AllocNewContact(point, pcontact, Flags, Contacts, Stride, contactcount))
422 {
423 const dReal depthDifference = depth - pcontact->depth;
424
425 if (depthDifference > CONTACT_DIFF_EPSILON)
426 {
427 pcontact->normal[0] = normal[0];
428 pcontact->normal[1] = normal[1];
429 pcontact->normal[2] = normal[2];
430 pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
431 pcontact->depth = depth;
432
433 pcontact->g1 = g1;
434 pcontact->g2 = g2;
435 }
436 else if (depthDifference >= -CONTACT_DIFF_EPSILON) ///average
437 {
438 if(pcontact->g1 == g2)
439 {
440 MULT(normal,normal, REAL(-1.0));
441 }
442
443 const dReal oldLen = pcontact->normal[3];
444 COMBO(pcontact->normal, normal, oldLen, pcontact->normal);
445
446 const dReal len = LENGTH(pcontact->normal);
447 if (len > CONTACT_NORMAL_ZERO)
448 {
449 MULT(pcontact->normal, pcontact->normal, REAL(1.0) / len);
450 pcontact->normal[3] = len;
451 }
452 else
453 {
454 FreeExistingContact(pcontact, Flags, Contacts, Stride, contactcount);
455 }
456 }
457 }
458 // Contact can be not available if buffer is full
459 else if (pcontact)
460 {
461 pcontact->normal[0] = normal[0];
462 pcontact->normal[1] = normal[1];
463 pcontact->normal[2] = normal[2];
464 pcontact->normal[3] = REAL(1.0); // used to store length of vector sum for averaging
465 pcontact->depth = depth;
466 pcontact->g1 = g1;
467 pcontact->g2 = g2;
468 }
469
470 return pcontact;
471}
472
473////////////////////////////////////////////////////////////////////////////////////////////
474
475
476
477
478
479int
480dCollideTTL(dxGeom* g1, dxGeom* g2, int Flags, dContactGeom* Contacts, int Stride)
481{
482 dIASSERT (Stride >= (int)sizeof(dContactGeom));
483 dIASSERT (g1->type == dTriMeshClass);
484 dIASSERT (g2->type == dTriMeshClass);
485 dIASSERT ((Flags & NUMC_MASK) >= 1);
486
487 dxTriMesh* TriMesh1 = (dxTriMesh*) g1;
488 dxTriMesh* TriMesh2 = (dxTriMesh*) g2;
489
490 dReal * TriNormals1 = (dReal *) TriMesh1->Data->Normals;
491 dReal * TriNormals2 = (dReal *) TriMesh2->Data->Normals;
492
493 const dVector3& TLPosition1 = *(const dVector3*) dGeomGetPosition(TriMesh1);
494 // TLRotation1 = column-major order
495 const dMatrix3& TLRotation1 = *(const dMatrix3*) dGeomGetRotation(TriMesh1);
496
497 const dVector3& TLPosition2 = *(const dVector3*) dGeomGetPosition(TriMesh2);
498 // TLRotation2 = column-major order
499 const dMatrix3& TLRotation2 = *(const dMatrix3*) dGeomGetRotation(TriMesh2);
500
501 AABBTreeCollider& Collider = TriMesh1->_AABBTreeCollider;
502
503
504 static BVTCache ColCache;
505 ColCache.Model0 = &TriMesh1->Data->BVTree;
506 ColCache.Model1 = &TriMesh2->Data->BVTree;
507
508 ////Prepare contact list
509 ClearContactSet();
510
511 // Collision query
512 Matrix4x4 amatrix, bmatrix;
513 BOOL IsOk = Collider.Collide(ColCache,
514 &MakeMatrix(TLPosition1, TLRotation1, amatrix),
515 &MakeMatrix(TLPosition2, TLRotation2, bmatrix) );
516
517
518 // Make "double" versions of these matrices, if appropriate
519 dMatrix4 A, B;
520 dMakeMatrix4(TLPosition1, TLRotation1, A);
521 dMakeMatrix4(TLPosition2, TLRotation2, B);
522
523
524
525
526 if (IsOk) {
527 // Get collision status => if true, objects overlap
528 if ( Collider.GetContactStatus() ) {
529 // Number of colliding pairs and list of pairs
530 int TriCount = Collider.GetNbPairs();
531 const Pair* CollidingPairs = Collider.GetPairs();
532
533 if (TriCount > 0) {
534 // step through the pairs, adding contacts
535 int id1, id2;
536 int OutTriCount = 0;
537 dVector3 v1[3], v2[3];
538
539 // only do these expensive inversions once
540 /*dMatrix4 InvMatrix1, InvMatrix2;
541 dInvertMatrix4(A, InvMatrix1);
542 dInvertMatrix4(B, InvMatrix2);*/
543
544
545 for (int i = 0; i < TriCount; i++)
546 {
547 id1 = CollidingPairs[i].id0;
548 id2 = CollidingPairs[i].id1;
549
550 // grab the colliding triangles
551 FetchTriangle((dxTriMesh*) g1, id1, TLPosition1, TLRotation1, v1);
552 FetchTriangle((dxTriMesh*) g2, id2, TLPosition2, TLRotation2, v2);
553 // Since we'll be doing matrix transformations, we need to
554 // make sure that all vertices have four elements
555 for (int j=0; j<3; j++) {
556 v1[j][3] = 1.0;
557 v2[j][3] = 1.0;
558 }
559
560 TriTriContacts(v1,v2,
561 g1, g2, Flags,
562 Contacts,Stride,OutTriCount);
563
564 // Continue loop even after contacts are full
565 // as existing contacts' normals/depths might be updated
566 // Break only if contacts are not important
567 if ((OutTriCount | CONTACTS_UNIMPORTANT) == (Flags & (NUMC_MASK | CONTACTS_UNIMPORTANT)))
568 {
569 break;
570 }
571 }
572
573 // Return the number of contacts
574 return OutTriCount;
575
576 }
577 }
578 }
579
580
581 // There was some kind of failure during the Collide call or
582 // there are no faces overlapping
583 return 0;
584}
585
586
587
588static void
589GetTriangleGeometryCallback(udword triangleindex, VertexPointers& triangle, udword user_data)
590{
591 dVector3 Out[3];
592
593 FetchTriangle((dxTriMesh*) user_data, (int) triangleindex, Out);
594
595 for (int i = 0; i < 3; i++)
596 triangle.Vertex[i] = (const Point*) ((dReal*) Out[i]);
597}
598
599
600//
601//
602//
603#define B11 B[0]
604#define B12 B[1]
605#define B13 B[2]
606#define B14 B[3]
607#define B21 B[4]
608#define B22 B[5]
609#define B23 B[6]
610#define B24 B[7]
611#define B31 B[8]
612#define B32 B[9]
613#define B33 B[10]
614#define B34 B[11]
615#define B41 B[12]
616#define B42 B[13]
617#define B43 B[14]
618#define B44 B[15]
619
620#define Binv11 Binv[0]
621#define Binv12 Binv[1]
622#define Binv13 Binv[2]
623#define Binv14 Binv[3]
624#define Binv21 Binv[4]
625#define Binv22 Binv[5]
626#define Binv23 Binv[6]
627#define Binv24 Binv[7]
628#define Binv31 Binv[8]
629#define Binv32 Binv[9]
630#define Binv33 Binv[10]
631#define Binv34 Binv[11]
632#define Binv41 Binv[12]
633#define Binv42 Binv[13]
634#define Binv43 Binv[14]
635#define Binv44 Binv[15]
636
637inline void
638dMakeMatrix4(const dVector3 Position, const dMatrix3 Rotation, dMatrix4 &B)
639{
640 B11 = Rotation[0]; B21 = Rotation[1]; B31 = Rotation[2]; B41 = Position[0];
641 B12 = Rotation[4]; B22 = Rotation[5]; B32 = Rotation[6]; B42 = Position[1];
642 B13 = Rotation[8]; B23 = Rotation[9]; B33 = Rotation[10]; B43 = Position[2];
643
644 B14 = 0.0; B24 = 0.0; B34 = 0.0; B44 = 1.0;
645}
646
647
648static void
649dInvertMatrix4( dMatrix4& B, dMatrix4& Binv )
650{
651 dReal det = (B11 * B22 - B12 * B21) * (B33 * B44 - B34 * B43)
652 -(B11 * B23 - B13 * B21) * (B32 * B44 - B34 * B42)
653 +(B11 * B24 - B14 * B21) * (B32 * B43 - B33 * B42)
654 +(B12 * B23 - B13 * B22) * (B31 * B44 - B34 * B41)
655 -(B12 * B24 - B14 * B22) * (B31 * B43 - B33 * B41)
656 +(B13 * B24 - B14 * B23) * (B31 * B42 - B32 * B41);
657
658 dAASSERT (det != 0.0);
659
660 det = 1.0 / det;
661
662 Binv11 = (dReal) (det * ((B22 * B33) - (B23 * B32)));
663 Binv12 = (dReal) (det * ((B32 * B13) - (B33 * B12)));
664 Binv13 = (dReal) (det * ((B12 * B23) - (B13 * B22)));
665 Binv14 = 0.0f;
666 Binv21 = (dReal) (det * ((B23 * B31) - (B21 * B33)));
667 Binv22 = (dReal) (det * ((B33 * B11) - (B31 * B13)));
668 Binv23 = (dReal) (det * ((B13 * B21) - (B11 * B23)));
669 Binv24 = 0.0f;
670 Binv31 = (dReal) (det * ((B21 * B32) - (B22 * B31)));
671 Binv32 = (dReal) (det * ((B31 * B12) - (B32 * B11)));
672 Binv33 = (dReal) (det * ((B11 * B22) - (B12 * B21)));
673 Binv34 = 0.0f;
674 Binv41 = (dReal) (det * (B21*(B33*B42 - B32*B43) + B22*(B31*B43 - B33*B41) + B23*(B32*B41 - B31*B42)));
675 Binv42 = (dReal) (det * (B31*(B13*B42 - B12*B43) + B32*(B11*B43 - B13*B41) + B33*(B12*B41 - B11*B42)));
676 Binv43 = (dReal) (det * (B41*(B13*B22 - B12*B23) + B42*(B11*B23 - B13*B21) + B43*(B12*B21 - B11*B22)));
677 Binv44 = 1.0f;
678}
679
680
681
682// Find the intersectiojn point between a coplanar line segement,
683// defined by X1 and X2, and a ray defined by X3 and direction N.
684//
685// This forumla for this calculation is:
686// (c x b) . (a x b)
687// Q = x1 + a -------------------
688// | a x b | ^2
689//
690// where a = x2 - x1
691// b = x4 - x3
692// c = x3 - x1
693// x1 and x2 are the edges of the triangle, and x3 is CoplanarPt
694// and x4 is (CoplanarPt - n)
695static int
696IntersectLineSegmentRay(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 = dDOT(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 (dDOT(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 (dDOT(a,b) < 0.0)
732 return 1;
733 else
734 return 0;
735}
736
737
738
739void 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) */
755static void
756ClipConvexPolygonAgainstPlane( 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
839bool 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
858bool 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
877bool 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/*
899Positive penetration
900Negative number: they are separated
901*/
902dReal 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
929void 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
947dReal 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
984void 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
1022dReal 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 ////find interval face1
1039
1040 bl = BuildPlane(tri1[0],tri1[1],tri1[2],
1041 tri1plane,tri1plane[3]);
1042 clipped_points1.Count = 0;
1043
1044 if(bl)
1045 {
1046 ClipPointsByTri(
1047 tri2, 3,
1048 tri1,
1049 tri1plane,
1050 tri1plane[3],
1051 clipped_points1,false);
1052
1053
1054
1055 maxdeep = MostDeepPoints(
1056 clipped_points1,
1057 tri1plane,
1058 tri1plane[3],
1059 deep_points1);
1060 SET(separating_normal,tri1plane);
1061
1062 }
1063 currdir++;
1064
1065 ////find interval face2
1066
1067 bl = BuildPlane(tri2[0],tri2[1],tri2[2],
1068 tri2plane,tri2plane[3]);
1069
1070
1071 clipped_points2.Count = 0;
1072 if(bl)
1073 {
1074 ClipPointsByTri(
1075 tri1, 3,
1076 tri2,
1077 tri2plane,
1078 tri2plane[3],
1079 clipped_points2,false);
1080
1081
1082
1083 dist = MostDeepPoints(
1084 clipped_points2,
1085 tri2plane,
1086 tri2plane[3],
1087 deep_points2);
1088
1089
1090
1091 if(dist<maxdeep)
1092 {
1093 maxdeep = dist;
1094 mostdir = currdir;
1095 mostface = 1;
1096 SET(separating_normal,tri2plane);
1097 }
1098 }
1099 currdir++;
1100
1101
1102 ///find edge edge distances
1103 ///test each edge plane
1104
1105 /*for(i=0;i<3;i++)
1106 {
1107
1108
1109 for(j=0;j<3;j++)
1110 {
1111
1112
1113 bl = BuildEdgesDir(
1114 tri1[i],tri1[(i+1)%3],
1115 tri2[j],tri2[(j+1)%3],
1116 crossdir);
1117
1118 ////find plane distance
1119
1120 if(bl)
1121 {
1122 FindInterval(tri1,3,crossdir,vmin1,vmax1);
1123 FindInterval(tri2,3,crossdir,vmin2,vmax2);
1124
1125 dist = IntervalPenetration(
1126 vmin1,
1127 vmax1,
1128 vmin2,
1129 vmax2);
1130 if(dist<maxdeep)
1131 {
1132 maxdeep = dist;
1133 mostdir = currdir;
1134 SET(separating_normal,crossdir);
1135 }
1136 }
1137 currdir++;
1138 }
1139 }*/
1140
1141
1142 ////check most dir for contacts
1143 if(mostdir==0)
1144 {
1145 ///find most deep points
1146 deep_points.Count = deep_points1.Count;
1147 memcpy(
1148 &deep_points.Points[0],
1149 &deep_points1.Points[0],
1150 deep_points1.Count*sizeof(dVector3));
1151
1152 ///invert normal for point to tri1
1153 MULT(separating_normal,separating_normal,-1.0f);
1154 }
1155 else if(mostdir==1)
1156 {
1157 deep_points.Count = deep_points2.Count;
1158 memcpy(
1159 &deep_points.Points[0],
1160 &deep_points2.Points[0],
1161 deep_points2.Count*sizeof(dVector3));
1162
1163 }
1164 /*else
1165 {///edge separation
1166 mostdir -= 2;
1167
1168 //edge 2
1169 j = mostdir%3;
1170 //edge 1
1171 i = mostdir/3;
1172
1173 ///find edge closest points
1174 dClosestLineSegmentPoints(
1175 tri1[i],tri1[(i+1)%3],
1176 tri2[j],tri2[(j+1)%3],
1177 pt1,pt2);
1178 ///find correct direction
1179
1180 SUB(crossdir,pt2,pt1);
1181
1182 vmin1 = LENGTH(crossdir);
1183 if(vmin1<REAL(0.000001))
1184 {
1185
1186 if(mostface==0)
1187 {
1188 vmin1 = DOT(separating_normal,tri1plane);
1189 if(vmin1>0.0)
1190 {
1191 MULT(separating_normal,separating_normal,-1.0f);
1192 deep_points.Count = 1;
1193 SET(deep_points.Points[0],pt2);
1194 }
1195 else
1196 {
1197 deep_points.Count = 1;
1198 SET(deep_points.Points[0],pt2);
1199 }
1200
1201 }
1202 else
1203 {
1204 vmin1 = DOT(separating_normal,tri2plane);
1205 if(vmin1<0.0)
1206 {
1207 MULT(separating_normal,separating_normal,-1.0f);
1208 deep_points.Count = 1;
1209 SET(deep_points.Points[0],pt2);
1210 }
1211 else
1212 {
1213 deep_points.Count = 1;
1214 SET(deep_points.Points[0],pt2);
1215 }
1216
1217 }
1218
1219
1220
1221
1222 }
1223 else
1224 {
1225 MULT(separating_normal,crossdir,1.0f/vmin1);
1226
1227 vmin1 = DOT(separating_normal,tri1plane);
1228 if(vmin1>0.0)
1229 {
1230 MULT(separating_normal,separating_normal,-1.0f);
1231 deep_points.Count = 1;
1232 SET(deep_points.Points[0],pt2);
1233 }
1234 else
1235 {
1236 deep_points.Count = 1;
1237 SET(deep_points.Points[0],pt2);
1238 }
1239
1240
1241 }
1242
1243
1244 }*/
1245 return maxdeep;
1246}
1247
1248
1249
1250///SUPPORT UP TO 8 CONTACTS
1251bool TriTriContacts(const dVector3 tr1[3],
1252 const dVector3 tr2[3],
1253 dxGeom* g1, dxGeom* g2, int Flags,
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,
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
generated by cgit v1.1 at 2024-12-27 00:03:05 +0000