aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:24:38 +0000
committerdan miller2007-10-19 05:24:38 +0000
commitf205de7847da7ae1c10212d82e7042d0100b4ce0 (patch)
tree9acc9608a6880502aaeda43af52c33e278e95b9c /libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp
parenttrying to fix my screwup part deux (diff)
downloadopensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.zip
opensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.gz
opensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.bz2
opensim-SC_OLD-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.xz
from the start... checking in ode-0.9
Diffstat (limited to 'libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp')
-rw-r--r--libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp943
1 files changed, 943 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp
new file mode 100644
index 0000000..f21b39b
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_TreeCollider.cpp
@@ -0,0 +1,943 @@
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
34using 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45AABBTreeCollider::AABBTreeCollider() :
46 mNbBVBVTests (0),
47 mNbPrimPrimTests (0),
48 mNbBVPrimTests (0),
49 mFullBoxBoxTest (true),
50 mFullPrimBoxTest (true),
51 mIMesh0 (null),
52 mIMesh1 (null)
53{
54}
55
56///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
57/**
58 * Destructor.
59 */
60///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61AABBTreeCollider::~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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71const 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
91bool 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;
129 static bool GJKInitDone=false;
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;
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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
235void 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
277bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
310bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
338bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
366bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
409bool 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
447void 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
477void 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///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
528void AABBTreeCollider::PrimTest(udword id0, udword id1)
529{
530 // Request vertices from the app
531 VertexPointers VP0;
532 VertexPointers VP1;
533 mIMesh0->GetTriangle(VP0, id0);
534 mIMesh1->GetTriangle(VP1, id1);
535
536 // Transform from space 1 to space 0
537 Point u0,u1,u2;
538 TransformPoint(u0, *VP1.Vertex[0], mR1to0, mT1to0);
539 TransformPoint(u1, *VP1.Vertex[1], mR1to0, mT1to0);
540 TransformPoint(u2, *VP1.Vertex[2], mR1to0, mT1to0);
541
542 // Perform triangle-triangle overlap test
543 if(TriTriOverlap(*VP0.Vertex[0], *VP0.Vertex[1], *VP0.Vertex[2], u0, u1, u2))
544 {
545 // Keep track of colliding pairs
546 mPairs.Add(id0).Add(id1);
547 // Set contact status
548 mFlags |= OPC_CONTACT;
549 }
550}
551
552///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
553/**
554 * Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B.
555 * \param id1 [in] leaf-triangle index from tree B
556 */
557///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
558inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
559{
560 // Request vertices from the app
561 VertexPointers VP;
562 mIMesh1->GetTriangle(VP, id1);
563
564 // Perform triangle-triangle overlap test
565 if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
566 {
567 // Keep track of colliding pairs
568 mPairs.Add(mLeafIndex).Add(id1);
569 // Set contact status
570 mFlags |= OPC_CONTACT;
571 }
572}
573
574///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
575/**
576 * Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A.
577 * \param id0 [in] leaf-triangle index from tree A
578 */
579///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
580inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
581{
582 // Request vertices from the app
583 VertexPointers VP;
584 mIMesh0->GetTriangle(VP, id0);
585
586 // Perform triangle-triangle overlap test
587 if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
588 {
589 // Keep track of colliding pairs
590 mPairs.Add(id0).Add(mLeafIndex);
591 // Set contact status
592 mFlags |= OPC_CONTACT;
593 }
594}
595
596///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
597/**
598 * Recursive collision of a leaf node from A and a branch from B.
599 * \param b [in] collision node from second tree
600 */
601///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
602void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
603{
604 // Perform triangle-box overlap test
605 if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
606
607 // Keep same triangle, deal with first child
608 if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
609 else _CollideTriBox(b->GetPos());
610
611 if(ContactFound()) return;
612
613 // Keep same triangle, deal with second child
614 if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
615 else _CollideTriBox(b->GetNeg());
616}
617
618///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
619/**
620 * Recursive collision of a leaf node from B and a branch from A.
621 * \param b [in] collision node from first tree
622 */
623///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
624void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
625{
626 // Perform triangle-box overlap test
627 if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
628
629 // Keep same triangle, deal with first child
630 if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
631 else _CollideBoxTri(b->GetPos());
632
633 if(ContactFound()) return;
634
635 // Keep same triangle, deal with second child
636 if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
637 else _CollideBoxTri(b->GetNeg());
638}
639
640//! Request triangle vertices from the app and transform them
641#define FETCH_LEAF(prim_index, imesh, rot, trans) \
642 mLeafIndex = prim_index; \
643 /* Request vertices from the app */ \
644 VertexPointers VP; imesh->GetTriangle(VP, prim_index); \
645 /* Transform them in a common space */ \
646 TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \
647 TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \
648 TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);
649
650///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
651/**
652 * Recursive collision query for no-leaf AABB trees.
653 * \param a [in] collision node from first tree
654 * \param b [in] collision node from second tree
655 */
656///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
657void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
658{
659 // Perform BV-BV overlap test
660 if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return;
661
662 // Catch leaf status
663 BOOL BHasPosLeaf = b->HasPosLeaf();
664 BOOL BHasNegLeaf = b->HasNegLeaf();
665
666 if(a->HasPosLeaf())
667 {
668 FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
669
670 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
671 else _CollideTriBox(b->GetPos());
672
673 if(ContactFound()) return;
674
675 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
676 else _CollideTriBox(b->GetNeg());
677 }
678 else
679 {
680 if(BHasPosLeaf)
681 {
682 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
683
684 _CollideBoxTri(a->GetPos());
685 }
686 else _Collide(a->GetPos(), b->GetPos());
687
688 if(ContactFound()) return;
689
690 if(BHasNegLeaf)
691 {
692 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
693
694 _CollideBoxTri(a->GetPos());
695 }
696 else _Collide(a->GetPos(), b->GetNeg());
697 }
698
699 if(ContactFound()) return;
700
701 if(a->HasNegLeaf())
702 {
703 FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
704
705 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
706 else _CollideTriBox(b->GetPos());
707
708 if(ContactFound()) return;
709
710 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
711 else _CollideTriBox(b->GetNeg());
712 }
713 else
714 {
715 if(BHasPosLeaf)
716 {
717 // ### That leaf has possibly already been fetched
718 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
719
720 _CollideBoxTri(a->GetNeg());
721 }
722 else _Collide(a->GetNeg(), b->GetPos());
723
724 if(ContactFound()) return;
725
726 if(BHasNegLeaf)
727 {
728 // ### That leaf has possibly already been fetched
729 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
730
731 _CollideBoxTri(a->GetNeg());
732 }
733 else _Collide(a->GetNeg(), b->GetNeg());
734 }
735}
736
737///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
738// Quantized trees
739///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
740
741///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
742/**
743 * Recursive collision query for quantized AABB trees.
744 * \param b0 [in] collision node from first tree
745 * \param b1 [in] collision node from second tree
746 * \param a [in] extent from box A
747 * \param Pa [in] center from box A
748 * \param b [in] extent from box B
749 * \param Pb [in] center from box B
750 */
751///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
752void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb)
753{
754 // Perform BV-BV overlap test
755 if(!BoxBoxOverlap(a, Pa, b, Pb)) return;
756
757 if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
758
759 if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
760 {
761 // Dequantize box
762 const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
763 const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
764 const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
765 _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);
766
767 if(ContactFound()) return;
768
769 // Dequantize box
770 Box = &b0->GetPos()->mAABB;
771 const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
772 const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
773 _Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
774 }
775 else
776 {
777 // Dequantize box
778 const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
779 const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
780 const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
781 _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);
782
783 if(ContactFound()) return;
784
785 // Dequantize box
786 Box = &b1->GetPos()->mAABB;
787 const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
788 const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
789 _Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
790 }
791}
792
793///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
794// Quantized no-leaf trees
795///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
796
797///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
798/**
799 * Recursive collision of a leaf node from A and a quantized branch from B.
800 * \param leaf [in] leaf triangle from first tree
801 * \param b [in] collision node from second tree
802 */
803///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
804void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
805{
806 // Dequantize box
807 const QuantizedAABB* bb = &b->mAABB;
808 const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
809 const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
810
811 // Perform triangle-box overlap test
812 if(!TriBoxOverlap(Pb, eb)) return;
813
814 if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
815 else _CollideTriBox(b->GetPos());
816
817 if(ContactFound()) return;
818
819 if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
820 else _CollideTriBox(b->GetNeg());
821}
822
823///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
824/**
825 * Recursive collision of a leaf node from B and a quantized branch from A.
826 * \param b [in] collision node from first tree
827 * \param leaf [in] leaf triangle from second tree
828 */
829///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
830void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
831{
832 // Dequantize box
833 const QuantizedAABB* bb = &b->mAABB;
834 const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
835 const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);
836
837 // Perform triangle-box overlap test
838 if(!TriBoxOverlap(Pa, ea)) return;
839
840 if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
841 else _CollideBoxTri(b->GetPos());
842
843 if(ContactFound()) return;
844
845 if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
846 else _CollideBoxTri(b->GetNeg());
847}
848
849///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
850/**
851 * Recursive collision query for quantized no-leaf AABB trees.
852 * \param a [in] collision node from first tree
853 * \param b [in] collision node from second tree
854 */
855///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
856void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
857{
858 // Dequantize box A
859 const QuantizedAABB* ab = &a->mAABB;
860 const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
861 const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
862 // Dequantize box B
863 const QuantizedAABB* bb = &b->mAABB;
864 const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
865 const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
866
867 // Perform BV-BV overlap test
868 if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return;
869
870 // Catch leaf status
871 BOOL BHasPosLeaf = b->HasPosLeaf();
872 BOOL BHasNegLeaf = b->HasNegLeaf();
873
874 if(a->HasPosLeaf())
875 {
876 FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
877
878 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
879 else _CollideTriBox(b->GetPos());
880
881 if(ContactFound()) return;
882
883 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
884 else _CollideTriBox(b->GetNeg());
885 }
886 else
887 {
888 if(BHasPosLeaf)
889 {
890 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
891
892 _CollideBoxTri(a->GetPos());
893 }
894 else _Collide(a->GetPos(), b->GetPos());
895
896 if(ContactFound()) return;
897
898 if(BHasNegLeaf)
899 {
900 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
901
902 _CollideBoxTri(a->GetPos());
903 }
904 else _Collide(a->GetPos(), b->GetNeg());
905 }
906
907 if(ContactFound()) return;
908
909 if(a->HasNegLeaf())
910 {
911 FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
912
913 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
914 else _CollideTriBox(b->GetPos());
915
916 if(ContactFound()) return;
917
918 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
919 else _CollideTriBox(b->GetNeg());
920 }
921 else
922 {
923 if(BHasPosLeaf)
924 {
925 // ### That leaf has possibly already been fetched
926 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
927
928 _CollideBoxTri(a->GetNeg());
929 }
930 else _Collide(a->GetNeg(), b->GetPos());
931
932 if(ContactFound()) return;
933
934 if(BHasNegLeaf)
935 {
936 // ### That leaf has possibly already been fetched
937 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
938
939 _CollideBoxTri(a->GetNeg());
940 }
941 else _Collide(a->GetNeg(), b->GetNeg());
942 }
943}
generated by cgit v1.1 at 2024-11-14 00:28:24 +0000