Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/OPCODE/OPC_TreeCollider.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 17 years ago

[Physik] add ode-0.9
File size: 37.5 KB
Line 
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}
Note: See TracBrowser for help on using the repository browser.