1 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 /*
3 * OPCODE - Optimized Collision Detection
4 * Copyright (C) 2001 Pierre Terdiman
5 * Homepage: http://www.codercorner.com/Opcode.htm
6 */
7 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
8
9 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
10 /**
11 * Contains code for a tree collider.
12 * \file OPC_TreeCollider.cpp
13 * \author Pierre Terdiman
14 * \date March, 20, 2001
15 */
16 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
17
18 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
19 /**
20 * Contains an AABB tree collider.
21 * This class performs a collision test between two AABB trees.
22 *
23 * \class AABBTreeCollider
24 * \author Pierre Terdiman
25 * \version 1.3
26 * \date March, 20, 2001
27 */
28 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
29
30 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
31 // Precompiled Header
32 #include "Stdafx.h"
33
34 using namespace Opcode;
35
36 #include "OPC_BoxBoxOverlap.h"
37 #include "OPC_TriBoxOverlap.h"
38 #include "OPC_TriTriOverlap.h"
39
40 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
41 /**
42 * Constructor.
43 */
44 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
AABBTreeCollider()45 AABBTreeCollider::AABBTreeCollider() :
46 mIMesh0 (null),
47 mIMesh1 (null),
48 mNbBVBVTests (0),
49 mNbPrimPrimTests (0),
50 mNbBVPrimTests (0),
51 mFullBoxBoxTest (true),
52 mFullPrimBoxTest (true)
53 {
54 }
55
56 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57 /**
58 * Destructor.
59 */
60 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
~AABBTreeCollider()61 AABBTreeCollider::~AABBTreeCollider()
62 {
63 }
64
65 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
66 /**
67 * Validates current settings. You should call this method after all the settings and callbacks have been defined.
68 * \return null if everything is ok, else a string describing the problem
69 */
70 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ValidateSettings()71 const char* AABBTreeCollider::ValidateSettings()
72 {
73 if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
74 return null;
75 }
76
77 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78 /**
79 * Generic collision query for generic OPCODE models. After the call, access the results with:
80 * - GetContactStatus()
81 * - GetNbPairs()
82 * - GetPairs()
83 *
84 * \param cache [in] collision cache for model pointers and a colliding pair of primitives
85 * \param world0 [in] world matrix for first object
86 * \param world1 [in] world matrix for second object
87 * \return true if success
88 * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
89 */
90 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Collide(BVTCache & cache,const Matrix4x4 * world0,const Matrix4x4 * world1)91 bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1)
92 {
93 // Checkings
94 if(!cache.Model0 || !cache.Model1) return false;
95 if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false;
96 if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false;
97
98 /*
99
100 Rules:
101 - perform hull test
102 - when hulls collide, disable hull test
103 - if meshes overlap, reset countdown
104 - if countdown reaches 0, enable hull test
105
106 */
107
108 #ifdef __MESHMERIZER_H__
109 // Handle hulls
110 if(cache.HullTest)
111 {
112 if(cache.Model0->GetHull() && cache.Model1->GetHull())
113 {
114 struct Local
115 {
116 static Point* SVCallback(const Point& sv, udword& previndex, udword user_data)
117 {
118 CollisionHull* Hull = (CollisionHull*)user_data;
119 previndex = Hull->ComputeSupportingVertex(sv, previndex);
120 return (Point*)&Hull->GetVerts()[previndex];
121 }
122 };
123
124 bool Collide;
125
126 if(0)
127 {
128 static GJKEngine GJK; -- not thread safe, store in ThreadLocalData
129 static bool GJKInitDone=false; -- not thread safe, to be removed
130 if(!GJKInitDone)
131 {
132 GJK.Enable(GJK_BACKUP_PROCEDURE);
133 GJK.Enable(GJK_DEGENERATE);
134 GJK.Enable(GJK_HILLCLIMBING);
135 GJKInitDone = true;
136 }
137 GJK.SetCallbackObj0(Local::SVCallback);
138 GJK.SetCallbackObj1(Local::SVCallback);
139 GJK.SetUserData0(udword(cache.Model0->GetHull()));
140 GJK.SetUserData1(udword(cache.Model1->GetHull()));
141 Collide = GJK.Collide(*world0, *world1, &cache.SepVector);
142 }
143 else
144 {
145 static SVEngine SVE; -- not thread safe, store in ThreadLocalData
146 SVE.SetCallbackObj0(Local::SVCallback);
147 SVE.SetCallbackObj1(Local::SVCallback);
148 SVE.SetUserData0(udword(cache.Model0->GetHull()));
149 SVE.SetUserData1(udword(cache.Model1->GetHull()));
150 Collide = SVE.Collide(*world0, *world1, &cache.SepVector);
151 }
152
153 if(!Collide)
154 {
155 // Reset stats & contact status
156 mFlags &= ~OPC_CONTACT;
157 mNbBVBVTests = 0;
158 mNbPrimPrimTests = 0;
159 mNbBVPrimTests = 0;
160 mPairs.Reset();
161 return true;
162 }
163 }
164 }
165
166 // Here, hulls collide
167 cache.HullTest = false;
168 #endif // __MESHMERIZER_H__
169
170 // Checkings
171 if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
172
173 // Simple double-dispatch
174 bool Status;
175 if(!cache.Model0->HasLeafNodes())
176 {
177 if(cache.Model0->IsQuantized())
178 {
179 const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree();
180 const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree();
181 Status = Collide(T0, T1, world0, world1, &cache);
182 }
183 else
184 {
185 const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree();
186 const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree();
187 Status = Collide(T0, T1, world0, world1, &cache);
188 }
189 }
190 else
191 {
192 if(cache.Model0->IsQuantized())
193 {
194 const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree();
195 const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree();
196 Status = Collide(T0, T1, world0, world1, &cache);
197 }
198 else
199 {
200 const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
201 const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
202 Status = Collide(T0, T1, world0, world1, &cache);
203 }
204 }
205
206 #ifdef __MESHMERIZER_H__
207 if(Status)
208 {
209 // Reset counter as long as overlap occurs
210 if(GetContactStatus()) cache.ResetCountDown();
211
212 // Enable hull test again when counter reaches zero
213 cache.CountDown--;
214 if(!cache.CountDown)
215 {
216 cache.ResetCountDown();
217 cache.HullTest = true;
218 }
219 }
220 #endif
221 return Status;
222 }
223
224 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
225 /**
226 * Initializes a collision query :
227 * - reset stats & contact status
228 * - setup matrices
229 *
230 * \param world0 [in] world matrix for first object
231 * \param world1 [in] world matrix for second object
232 * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
233 */
234 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
InitQuery(const Matrix4x4 * world0,const Matrix4x4 * world1)235 void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1)
236 {
237 // Reset stats & contact status
238 Collider::InitQuery();
239 mNbBVBVTests = 0;
240 mNbPrimPrimTests = 0;
241 mNbBVPrimTests = 0;
242 mPairs.Reset();
243
244 // Setup matrices
245 Matrix4x4 InvWorld0, InvWorld1;
246 if(world0) InvertPRMatrix(InvWorld0, *world0);
247 else InvWorld0.Identity();
248
249 if(world1) InvertPRMatrix(InvWorld1, *world1);
250 else InvWorld1.Identity();
251
252 Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1;
253 Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0;
254
255 mR0to1 = World0to1; World0to1.GetTrans(mT0to1);
256 mR1to0 = World1to0; World1to0.GetTrans(mT1to0);
257
258 // Precompute absolute 1-to-0 rotation matrix
259 for(udword i=0;i<3;i++)
260 {
261 for(udword j=0;j<3;j++)
262 {
263 // Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
264 mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
265 }
266 }
267 }
268
269 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
270 /**
271 * Takes advantage of temporal coherence.
272 * \param cache [in] cache for a pair of previously colliding primitives
273 * \return true if we can return immediately
274 * \warning only works for "First Contact" mode
275 */
276 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
CheckTemporalCoherence(Pair * cache)277 bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache)
278 {
279 // Checkings
280 if(!cache) return false;
281
282 // Test previously colliding primitives first
283 if(TemporalCoherenceEnabled() && FirstContactEnabled())
284 {
285 PrimTest(cache->id0, cache->id1);
286 if(GetContactStatus()) return true;
287 }
288 return false;
289 }
290
291 #define UPDATE_CACHE \
292 if(cache && GetContactStatus()) \
293 { \
294 cache->id0 = mPairs.GetEntry(0); \
295 cache->id1 = mPairs.GetEntry(1); \
296 }
297
298 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
299 /**
300 * Collision query for normal AABB trees.
301 * \param tree0 [in] AABB tree from first object
302 * \param tree1 [in] AABB tree from second object
303 * \param world0 [in] world matrix for first object
304 * \param world1 [in] world matrix for second object
305 * \param cache [in/out] cache for a pair of previously colliding primitives
306 * \return true if success
307 * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
308 */
309 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Collide(const AABBCollisionTree * tree0,const AABBCollisionTree * tree1,const Matrix4x4 * world0,const Matrix4x4 * world1,Pair * cache)310 bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
311 {
312 // Init collision query
313 InitQuery(world0, world1);
314
315 // Check previous state
316 if(CheckTemporalCoherence(cache)) return true;
317
318 // Perform collision query
319 _Collide(tree0->GetNodes(), tree1->GetNodes());
320
321 UPDATE_CACHE
322
323 return true;
324 }
325
326 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
327 /**
328 * Collision query for no-leaf AABB trees.
329 * \param tree0 [in] AABB tree from first object
330 * \param tree1 [in] AABB tree from second object
331 * \param world0 [in] world matrix for first object
332 * \param world1 [in] world matrix for second object
333 * \param cache [in/out] cache for a pair of previously colliding primitives
334 * \return true if success
335 * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
336 */
337 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Collide(const AABBNoLeafTree * tree0,const AABBNoLeafTree * tree1,const Matrix4x4 * world0,const Matrix4x4 * world1,Pair * cache)338 bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
339 {
340 // Init collision query
341 InitQuery(world0, world1);
342
343 // Check previous state
344 if(CheckTemporalCoherence(cache)) return true;
345
346 // Perform collision query
347 _Collide(tree0->GetNodes(), tree1->GetNodes());
348
349 UPDATE_CACHE
350
351 return true;
352 }
353
354 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
355 /**
356 * Collision query for quantized AABB trees.
357 * \param tree0 [in] AABB tree from first object
358 * \param tree1 [in] AABB tree from second object
359 * \param world0 [in] world matrix for first object
360 * \param world1 [in] world matrix for second object
361 * \param cache [in/out] cache for a pair of previously colliding primitives
362 * \return true if success
363 * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
364 */
365 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Collide(const AABBQuantizedTree * tree0,const AABBQuantizedTree * tree1,const Matrix4x4 * world0,const Matrix4x4 * world1,Pair * cache)366 bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
367 {
368 // Init collision query
369 InitQuery(world0, world1);
370
371 // Check previous state
372 if(CheckTemporalCoherence(cache)) return true;
373
374 // Setup dequantization coeffs
375 mCenterCoeff0 = tree0->mCenterCoeff;
376 mExtentsCoeff0 = tree0->mExtentsCoeff;
377 mCenterCoeff1 = tree1->mCenterCoeff;
378 mExtentsCoeff1 = tree1->mExtentsCoeff;
379
380 // Dequantize box A
381 const AABBQuantizedNode* N0 = tree0->GetNodes();
382 const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z);
383 const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z);
384 // Dequantize box B
385 const AABBQuantizedNode* N1 = tree1->GetNodes();
386 const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z);
387 const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z);
388
389 // Perform collision query
390 _Collide(N0, N1, a, Pa, b, Pb);
391
392 UPDATE_CACHE
393
394 return true;
395 }
396
397 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
398 /**
399 * Collision query for quantized no-leaf AABB trees.
400 * \param tree0 [in] AABB tree from first object
401 * \param tree1 [in] AABB tree from second object
402 * \param world0 [in] world matrix for first object
403 * \param world1 [in] world matrix for second object
404 * \param cache [in/out] cache for a pair of previously colliding primitives
405 * \return true if success
406 * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
407 */
408 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Collide(const AABBQuantizedNoLeafTree * tree0,const AABBQuantizedNoLeafTree * tree1,const Matrix4x4 * world0,const Matrix4x4 * world1,Pair * cache)409 bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
410 {
411 // Init collision query
412 InitQuery(world0, world1);
413
414 // Check previous state
415 if(CheckTemporalCoherence(cache)) return true;
416
417 // Setup dequantization coeffs
418 mCenterCoeff0 = tree0->mCenterCoeff;
419 mExtentsCoeff0 = tree0->mExtentsCoeff;
420 mCenterCoeff1 = tree1->mCenterCoeff;
421 mExtentsCoeff1 = tree1->mExtentsCoeff;
422
423 // Perform collision query
424 _Collide(tree0->GetNodes(), tree1->GetNodes());
425
426 UPDATE_CACHE
427
428 return true;
429 }
430
431 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
432 // Standard trees
433 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
434
435 // The normal AABB tree can use 2 different descent rules (with different performances)
436 //#define ORIGINAL_CODE //!< UNC-like descent rules
437 #define ALTERNATIVE_CODE //!< Alternative descent rules
438
439 #ifdef ORIGINAL_CODE
440 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
441 /**
442 * Recursive collision query for normal AABB trees.
443 * \param b0 [in] collision node from first tree
444 * \param b1 [in] collision node from second tree
445 */
446 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_Collide(const AABBCollisionNode * b0,const AABBCollisionNode * b1)447 void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
448 {
449 // Perform BV-BV overlap test
450 if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return;
451
452 if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
453
454 if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
455 {
456 _Collide(b0->GetNeg(), b1);
457 if(ContactFound()) return;
458 _Collide(b0->GetPos(), b1);
459 }
460 else
461 {
462 _Collide(b0, b1->GetNeg());
463 if(ContactFound()) return;
464 _Collide(b0, b1->GetPos());
465 }
466 }
467 #endif
468
469 #ifdef ALTERNATIVE_CODE
470 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
471 /**
472 * Recursive collision query for normal AABB trees.
473 * \param b0 [in] collision node from first tree
474 * \param b1 [in] collision node from second tree
475 */
476 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_Collide(const AABBCollisionNode * b0,const AABBCollisionNode * b1)477 void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
478 {
479 // Perform BV-BV overlap test
480 if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))
481 {
482 return;
483 }
484
485 if(b0->IsLeaf())
486 {
487 if(b1->IsLeaf())
488 {
489 PrimTest(b0->GetPrimitive(), b1->GetPrimitive());
490 }
491 else
492 {
493 _Collide(b0, b1->GetNeg());
494 if(ContactFound()) return;
495 _Collide(b0, b1->GetPos());
496 }
497 }
498 else if(b1->IsLeaf())
499 {
500 _Collide(b0->GetNeg(), b1);
501 if(ContactFound()) return;
502 _Collide(b0->GetPos(), b1);
503 }
504 else
505 {
506 _Collide(b0->GetNeg(), b1->GetNeg());
507 if(ContactFound()) return;
508 _Collide(b0->GetNeg(), b1->GetPos());
509 if(ContactFound()) return;
510 _Collide(b0->GetPos(), b1->GetNeg());
511 if(ContactFound()) return;
512 _Collide(b0->GetPos(), b1->GetPos());
513 }
514 }
515 #endif
516
517 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
518 // No-leaf trees
519 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
520
521 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
522 /**
523 * Leaf-leaf test for two primitive indices.
524 * \param id0 [in] index from first leaf-triangle
525 * \param id1 [in] index from second leaf-triangle
526 */
527 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PrimTest(udword id0,udword id1)528 void AABBTreeCollider::PrimTest(udword id0, udword id1)
529 {
530 // Request vertices from the app
531 VertexPointers VP0;
532 VertexPointers VP1;
533 ConversionArea VC0;
534 ConversionArea VC1;
535 mIMesh0->GetTriangle(VP0, id0, VC0);
536 mIMesh1->GetTriangle(VP1, id1, VC1);
537
538 // Transform from space 1 to space 0
539 Point u0,u1,u2;
540 TransformPoint(u0, *VP1.Vertex[0], mR1to0, mT1to0);
541 TransformPoint(u1, *VP1.Vertex[1], mR1to0, mT1to0);
542 TransformPoint(u2, *VP1.Vertex[2], mR1to0, mT1to0);
543
544 // Perform triangle-triangle overlap test
545 if(TriTriOverlap(*VP0.Vertex[0], *VP0.Vertex[1], *VP0.Vertex[2], u0, u1, u2))
546 {
547 // Keep track of colliding pairs
548 mPairs.Add(id0).Add(id1);
549 // Set contact status
550 mFlags |= OPC_CONTACT;
551 }
552 }
553
554 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
555 /**
556 * Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B.
557 * \param id1 [in] leaf-triangle index from tree B
558 */
559 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PrimTestTriIndex(udword id1)560 inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
561 {
562 // Request vertices from the app
563 VertexPointers VP;
564 ConversionArea VC;
565 mIMesh1->GetTriangle(VP, id1, VC);
566
567 // Perform triangle-triangle overlap test
568 if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
569 {
570 // Keep track of colliding pairs
571 mPairs.Add(mLeafIndex).Add(id1);
572 // Set contact status
573 mFlags |= OPC_CONTACT;
574 }
575 }
576
577 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
578 /**
579 * Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A.
580 * \param id0 [in] leaf-triangle index from tree A
581 */
582 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
PrimTestIndexTri(udword id0)583 inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
584 {
585 // Request vertices from the app
586 VertexPointers VP;
587 ConversionArea VC;
588 mIMesh0->GetTriangle(VP, id0, VC);
589
590 // Perform triangle-triangle overlap test
591 if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
592 {
593 // Keep track of colliding pairs
594 mPairs.Add(id0).Add(mLeafIndex);
595 // Set contact status
596 mFlags |= OPC_CONTACT;
597 }
598 }
599
600 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
601 /**
602 * Recursive collision of a leaf node from A and a branch from B.
603 * \param b [in] collision node from second tree
604 */
605 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_CollideTriBox(const AABBNoLeafNode * b)606 void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
607 {
608 // Perform triangle-box overlap test
609 if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
610
611 // Keep same triangle, deal with first child
612 if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
613 else _CollideTriBox(b->GetPos());
614
615 if(ContactFound()) return;
616
617 // Keep same triangle, deal with second child
618 if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
619 else _CollideTriBox(b->GetNeg());
620 }
621
622 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
623 /**
624 * Recursive collision of a leaf node from B and a branch from A.
625 * \param b [in] collision node from first tree
626 */
627 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_CollideBoxTri(const AABBNoLeafNode * b)628 void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
629 {
630 // Perform triangle-box overlap test
631 if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
632
633 // Keep same triangle, deal with first child
634 if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
635 else _CollideBoxTri(b->GetPos());
636
637 if(ContactFound()) return;
638
639 // Keep same triangle, deal with second child
640 if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
641 else _CollideBoxTri(b->GetNeg());
642 }
643
644 //! Request triangle vertices from the app and transform them
645 #define FETCH_LEAF(prim_index, imesh, rot, trans) \
646 mLeafIndex = prim_index; \
647 /* Request vertices from the app */ \
648 VertexPointers VP; ConversionArea VC; imesh->GetTriangle(VP, prim_index, VC); \
649 /* Transform them in a common space */ \
650 TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \
651 TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \
652 TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);
653
654 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
655 /**
656 * Recursive collision query for no-leaf AABB trees.
657 * \param a [in] collision node from first tree
658 * \param b [in] collision node from second tree
659 */
660 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_Collide(const AABBNoLeafNode * a,const AABBNoLeafNode * b)661 void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
662 {
663 // Perform BV-BV overlap test
664 if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return;
665
666 // Catch leaf status
667 BOOL BHasPosLeaf = b->HasPosLeaf();
668 BOOL BHasNegLeaf = b->HasNegLeaf();
669
670 if(a->HasPosLeaf())
671 {
672 FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
673
674 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
675 else _CollideTriBox(b->GetPos());
676
677 if(ContactFound()) return;
678
679 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
680 else _CollideTriBox(b->GetNeg());
681 }
682 else
683 {
684 if(BHasPosLeaf)
685 {
686 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
687
688 _CollideBoxTri(a->GetPos());
689 }
690 else _Collide(a->GetPos(), b->GetPos());
691
692 if(ContactFound()) return;
693
694 if(BHasNegLeaf)
695 {
696 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
697
698 _CollideBoxTri(a->GetPos());
699 }
700 else _Collide(a->GetPos(), b->GetNeg());
701 }
702
703 if(ContactFound()) return;
704
705 if(a->HasNegLeaf())
706 {
707 FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
708
709 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
710 else _CollideTriBox(b->GetPos());
711
712 if(ContactFound()) return;
713
714 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
715 else _CollideTriBox(b->GetNeg());
716 }
717 else
718 {
719 if(BHasPosLeaf)
720 {
721 // ### That leaf has possibly already been fetched
722 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
723
724 _CollideBoxTri(a->GetNeg());
725 }
726 else _Collide(a->GetNeg(), b->GetPos());
727
728 if(ContactFound()) return;
729
730 if(BHasNegLeaf)
731 {
732 // ### That leaf has possibly already been fetched
733 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
734
735 _CollideBoxTri(a->GetNeg());
736 }
737 else _Collide(a->GetNeg(), b->GetNeg());
738 }
739 }
740
741 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
742 // Quantized trees
743 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
744
745 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
746 /**
747 * Recursive collision query for quantized AABB trees.
748 * \param b0 [in] collision node from first tree
749 * \param b1 [in] collision node from second tree
750 * \param a [in] extent from box A
751 * \param Pa [in] center from box A
752 * \param b [in] extent from box B
753 * \param Pb [in] center from box B
754 */
755 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_Collide(const AABBQuantizedNode * b0,const AABBQuantizedNode * b1,const Point & a,const Point & Pa,const Point & b,const Point & Pb)756 void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb)
757 {
758 // Perform BV-BV overlap test
759 if(!BoxBoxOverlap(a, Pa, b, Pb)) return;
760
761 if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
762
763 if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
764 {
765 // Dequantize box
766 const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
767 const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
768 const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
769 _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);
770
771 if(ContactFound()) return;
772
773 // Dequantize box
774 Box = &b0->GetPos()->mAABB;
775 const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
776 const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
777 _Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
778 }
779 else
780 {
781 // Dequantize box
782 const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
783 const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
784 const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
785 _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);
786
787 if(ContactFound()) return;
788
789 // Dequantize box
790 Box = &b1->GetPos()->mAABB;
791 const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
792 const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
793 _Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
794 }
795 }
796
797 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
798 // Quantized no-leaf trees
799 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
800
801 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
802 /**
803 * Recursive collision of a leaf node from A and a quantized branch from B.
804 * \param leaf [in] leaf triangle from first tree
805 * \param b [in] collision node from second tree
806 */
807 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_CollideTriBox(const AABBQuantizedNoLeafNode * b)808 void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
809 {
810 // Dequantize box
811 const QuantizedAABB* bb = &b->mAABB;
812 const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
813 const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
814
815 // Perform triangle-box overlap test
816 if(!TriBoxOverlap(Pb, eb)) return;
817
818 if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
819 else _CollideTriBox(b->GetPos());
820
821 if(ContactFound()) return;
822
823 if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
824 else _CollideTriBox(b->GetNeg());
825 }
826
827 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
828 /**
829 * Recursive collision of a leaf node from B and a quantized branch from A.
830 * \param b [in] collision node from first tree
831 * \param leaf [in] leaf triangle from second tree
832 */
833 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_CollideBoxTri(const AABBQuantizedNoLeafNode * b)834 void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
835 {
836 // Dequantize box
837 const QuantizedAABB* bb = &b->mAABB;
838 const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
839 const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);
840
841 // Perform triangle-box overlap test
842 if(!TriBoxOverlap(Pa, ea)) return;
843
844 if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
845 else _CollideBoxTri(b->GetPos());
846
847 if(ContactFound()) return;
848
849 if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
850 else _CollideBoxTri(b->GetNeg());
851 }
852
853 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
854 /**
855 * Recursive collision query for quantized no-leaf AABB trees.
856 * \param a [in] collision node from first tree
857 * \param b [in] collision node from second tree
858 */
859 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
_Collide(const AABBQuantizedNoLeafNode * a,const AABBQuantizedNoLeafNode * b)860 void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
861 {
862 // Dequantize box A
863 const QuantizedAABB* ab = &a->mAABB;
864 const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
865 const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
866 // Dequantize box B
867 const QuantizedAABB* bb = &b->mAABB;
868 const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
869 const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
870
871 // Perform BV-BV overlap test
872 if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return;
873
874 // Catch leaf status
875 BOOL BHasPosLeaf = b->HasPosLeaf();
876 BOOL BHasNegLeaf = b->HasNegLeaf();
877
878 if(a->HasPosLeaf())
879 {
880 FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
881
882 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
883 else _CollideTriBox(b->GetPos());
884
885 if(ContactFound()) return;
886
887 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
888 else _CollideTriBox(b->GetNeg());
889 }
890 else
891 {
892 if(BHasPosLeaf)
893 {
894 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
895
896 _CollideBoxTri(a->GetPos());
897 }
898 else _Collide(a->GetPos(), b->GetPos());
899
900 if(ContactFound()) return;
901
902 if(BHasNegLeaf)
903 {
904 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
905
906 _CollideBoxTri(a->GetPos());
907 }
908 else _Collide(a->GetPos(), b->GetNeg());
909 }
910
911 if(ContactFound()) return;
912
913 if(a->HasNegLeaf())
914 {
915 FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
916
917 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
918 else _CollideTriBox(b->GetPos());
919
920 if(ContactFound()) return;
921
922 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
923 else _CollideTriBox(b->GetNeg());
924 }
925 else
926 {
927 if(BHasPosLeaf)
928 {
929 // ### That leaf has possibly already been fetched
930 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
931
932 _CollideBoxTri(a->GetNeg());
933 }
934 else _Collide(a->GetNeg(), b->GetPos());
935
936 if(ContactFound()) return;
937
938 if(BHasNegLeaf)
939 {
940 // ### That leaf has possibly already been fetched
941 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
942
943 _CollideBoxTri(a->GetNeg());
944 }
945 else _Collide(a->GetNeg(), b->GetNeg());
946 }
947 }
948