Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

[Physik] add ode-0.9
File size: 28.3 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 an OBB collider.
12 *      \file           OPC_OBBCollider.cpp
13 *      \author         Pierre Terdiman
14 *      \date           January, 1st, 2002
15 */
16///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
17
18///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
19/**
20 *      Contains an OBB-vs-tree collider.
21 *
22 *      \class          OBBCollider
23 *      \author         Pierre Terdiman
24 *      \version        1.3
25 *      \date           January, 1st, 2002
26*/
27///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
28
29///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
30// Precompiled Header
31#include "Stdafx.h"
32
33using namespace Opcode;
34
35#include "OPC_BoxBoxOverlap.h"
36#include "OPC_TriBoxOverlap.h"
37
38#define SET_CONTACT(prim_index, flag)                                                                                   \
39        /* Set contact status */                                                                                                        \
40        mFlags |= flag;                                                                                                                         \
41        mTouchedPrimitives->Add(udword(prim_index));
42
43//! OBB-triangle test
44#define OBB_PRIM(prim_index, flag)                                                                                              \
45        /* Request vertices from the app */                                                                                     \
46        VertexPointers VP;      mIMesh->GetTriangle(VP, prim_index);                                    \
47        /* Transform them in a common space */                                                                          \
48        TransformPoint(mLeafVerts[0], *VP.Vertex[0], mRModelToBox, mTModelToBox);       \
49        TransformPoint(mLeafVerts[1], *VP.Vertex[1], mRModelToBox, mTModelToBox);       \
50        TransformPoint(mLeafVerts[2], *VP.Vertex[2], mRModelToBox, mTModelToBox);       \
51        /* Perform triangle-box overlap test */                                                                         \
52        if(TriBoxOverlap())                                                                                                                     \
53        {                                                                                                                                                       \
54                SET_CONTACT(prim_index, flag)                                                                                   \
55        }
56
57///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
58/**
59 *      Constructor.
60 */
61///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
62OBBCollider::OBBCollider() : mFullBoxBoxTest(true)
63{
64}
65
66///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67/**
68 *      Destructor.
69 */
70///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
71OBBCollider::~OBBCollider()
72{
73}
74
75///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
76/**
77 *      Validates current settings. You should call this method after all the settings and callbacks have been defined.
78 *      \return         null if everything is ok, else a string describing the problem
79 */
80///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
81const char* OBBCollider::ValidateSettings()
82{
83        if(TemporalCoherenceEnabled() && !FirstContactEnabled())        return "Temporal coherence only works with ""First contact"" mode!";
84
85        return VolumeCollider::ValidateSettings();
86}
87
88///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
89/**
90 *      Generic collision query for generic OPCODE models. After the call, access the results:
91 *      - with GetContactStatus()
92 *      - with GetNbTouchedPrimitives()
93 *      - with GetTouchedPrimitives()
94 *
95 *      \param          cache           [in/out] a box cache
96 *      \param          box                     [in] collision OBB in local space
97 *      \param          model           [in] Opcode model to collide with
98 *      \param          worldb          [in] OBB's world matrix, or null
99 *      \param          worldm          [in] model's world matrix, or null
100 *      \return         true if success
101 *      \warning        SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
102 */
103///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
104bool OBBCollider::Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
105{
106        // Checkings
107        if(!Setup(&model))      return false;
108
109        // Init collision query
110        if(InitQuery(cache, box, worldb, worldm))       return true;
111
112        if(!model.HasLeafNodes())
113        {
114                if(model.IsQuantized())
115                {
116                        const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
117
118                        // Setup dequantization coeffs
119                        mCenterCoeff    = Tree->mCenterCoeff;
120                        mExtentsCoeff   = Tree->mExtentsCoeff;
121
122                        // Perform collision query
123                        if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
124                        else                                            _Collide(Tree->GetNodes());
125                }
126                else
127                {
128                        const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
129
130                        // Perform collision query
131                        if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
132                        else                                            _Collide(Tree->GetNodes());
133                }
134        }
135        else
136        {
137                if(model.IsQuantized())
138                {
139                        const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
140
141                        // Setup dequantization coeffs
142                        mCenterCoeff    = Tree->mCenterCoeff;
143                        mExtentsCoeff   = Tree->mExtentsCoeff;
144
145                        // Perform collision query
146                        if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
147                        else                                            _Collide(Tree->GetNodes());
148                }
149                else
150                {
151                        const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
152
153                        // Perform collision query
154                        if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
155                        else                                            _Collide(Tree->GetNodes());
156                }
157        }
158
159        return true;
160}
161
162///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
163/**
164 *      Initializes a collision query :
165 *      - reset stats & contact status
166 *      - setup matrices
167 *      - check temporal coherence
168 *
169 *      \param          cache           [in/out] a box cache
170 *      \param          box                     [in] obb in local space
171 *      \param          worldb          [in] obb's world matrix, or null
172 *      \param          worldm          [in] model's world matrix, or null
173 *      \return         TRUE if we can return immediately
174 *      \warning        SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
175 */
176///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
177BOOL OBBCollider::InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb, const Matrix4x4* worldm)
178{
179        // 1) Call the base method
180        VolumeCollider::InitQuery();
181
182        // 2) Compute obb in world space
183        mBoxExtents = box.mExtents;
184
185        Matrix4x4 WorldB;
186
187        if(worldb)
188        {
189                WorldB = Matrix4x4( box.mRot * Matrix3x3(*worldb) );
190                WorldB.SetTrans(box.mCenter * *worldb);
191        }
192        else
193        {
194                WorldB = box.mRot;
195                WorldB.SetTrans(box.mCenter);
196        }
197
198        // Setup matrices
199        Matrix4x4 InvWorldB;
200        InvertPRMatrix(InvWorldB, WorldB);
201
202        if(worldm)
203        {
204                Matrix4x4 InvWorldM;
205                InvertPRMatrix(InvWorldM, *worldm);
206
207                Matrix4x4 WorldBtoM = WorldB * InvWorldM;
208                Matrix4x4 WorldMtoB = *worldm * InvWorldB;
209
210                mRModelToBox = WorldMtoB;               WorldMtoB.GetTrans(mTModelToBox);
211                mRBoxToModel = WorldBtoM;               WorldBtoM.GetTrans(mTBoxToModel);
212        }
213        else
214        {
215                mRModelToBox = InvWorldB;       InvWorldB.GetTrans(mTModelToBox);
216                mRBoxToModel = WorldB;          WorldB.GetTrans(mTBoxToModel);
217        }
218
219        // 3) Setup destination pointer
220        mTouchedPrimitives = &cache.TouchedPrimitives;
221
222        // 4) Special case: 1-triangle meshes [Opcode 1.3]
223        if(mCurrentModel && mCurrentModel->HasSingleNode())
224        {
225                if(!SkipPrimitiveTests())
226                {
227                        // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
228                        mTouchedPrimitives->Reset();
229
230                        // Perform overlap test between the unique triangle and the box (and set contact status if needed)
231                        OBB_PRIM(udword(0), OPC_CONTACT)
232
233                        // Return immediately regardless of status
234                        return TRUE;
235                }
236        }
237
238        // 5) Check temporal coherence:
239        if(TemporalCoherenceEnabled())
240        {
241                // Here we use temporal coherence
242                // => check results from previous frame before performing the collision query
243                if(FirstContactEnabled())
244                {
245                        // We're only interested in the first contact found => test the unique previously touched face
246                        if(mTouchedPrimitives->GetNbEntries())
247                        {
248                                // Get index of previously touched face = the first entry in the array
249                                udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
250
251                                // Then reset the array:
252                                // - if the overlap test below is successful, the index we'll get added back anyway
253                                // - if it isn't, then the array should be reset anyway for the normal query
254                                mTouchedPrimitives->Reset();
255
256                                // Perform overlap test between the cached triangle and the box (and set contact status if needed)
257                                OBB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
258
259                                // Return immediately if possible
260                                if(GetContactStatus())  return TRUE;
261                        }
262                        // else no face has been touched during previous query
263                        // => we'll have to perform a normal query
264                }
265                else
266                {
267                        // ### rewrite this
268                        OBB TestBox(mTBoxToModel, mBoxExtents, mRBoxToModel);
269
270                        // We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious):
271                        if(IsCacheValid(cache) && TestBox.IsInside(cache.FatBox))
272                        {
273                                // - if N is included in P, return previous list
274                                // => we simply leave the list (mTouchedFaces) unchanged
275
276                                // Set contact status if needed
277                                if(mTouchedPrimitives->GetNbEntries())  mFlags |= OPC_TEMPORAL_CONTACT;
278
279                                // In any case we don't need to do a query
280                                return TRUE;
281                        }
282                        else
283                        {
284                                // - else do the query using a fat N
285
286                                // Reset cache since we'll about to perform a real query
287                                mTouchedPrimitives->Reset();
288
289                                // Make a fat box so that coherence will work for subsequent frames
290                                TestBox.mExtents *= cache.FatCoeff;
291                                mBoxExtents *= cache.FatCoeff;
292
293                                // Update cache with query data (signature for cached faces)
294                                cache.FatBox = TestBox;
295                        }
296                }
297        }
298        else
299        {
300                // Here we don't use temporal coherence => do a normal query
301                mTouchedPrimitives->Reset();
302        }
303
304        // Now we can precompute box-box data
305
306        // Precompute absolute box-to-model rotation matrix
307        for(udword i=0;i<3;i++)
308        {
309                for(udword j=0;j<3;j++)
310                {
311                        // Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
312                        mAR.m[i][j] = 1e-6f + fabsf(mRBoxToModel.m[i][j]);
313                }
314        }
315
316        // Precompute bounds for box-in-box test
317        mB0 = mBoxExtents - mTModelToBox;
318        mB1 = - mBoxExtents - mTModelToBox;
319
320        // Precompute box-box data - Courtesy of Erwin de Vries
321        mBBx1 = mBoxExtents.x*mAR.m[0][0] + mBoxExtents.y*mAR.m[1][0] + mBoxExtents.z*mAR.m[2][0];
322        mBBy1 = mBoxExtents.x*mAR.m[0][1] + mBoxExtents.y*mAR.m[1][1] + mBoxExtents.z*mAR.m[2][1];
323        mBBz1 = mBoxExtents.x*mAR.m[0][2] + mBoxExtents.y*mAR.m[1][2] + mBoxExtents.z*mAR.m[2][2];
324
325        mBB_1 = mBoxExtents.y*mAR.m[2][0] + mBoxExtents.z*mAR.m[1][0];
326        mBB_2 = mBoxExtents.x*mAR.m[2][0] + mBoxExtents.z*mAR.m[0][0];
327        mBB_3 = mBoxExtents.x*mAR.m[1][0] + mBoxExtents.y*mAR.m[0][0];
328        mBB_4 = mBoxExtents.y*mAR.m[2][1] + mBoxExtents.z*mAR.m[1][1];
329        mBB_5 = mBoxExtents.x*mAR.m[2][1] + mBoxExtents.z*mAR.m[0][1];
330        mBB_6 = mBoxExtents.x*mAR.m[1][1] + mBoxExtents.y*mAR.m[0][1];
331        mBB_7 = mBoxExtents.y*mAR.m[2][2] + mBoxExtents.z*mAR.m[1][2];
332        mBB_8 = mBoxExtents.x*mAR.m[2][2] + mBoxExtents.z*mAR.m[0][2];
333        mBB_9 = mBoxExtents.x*mAR.m[1][2] + mBoxExtents.y*mAR.m[0][2];
334
335        return FALSE;
336}
337
338///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
339/**
340 *      Checks the OBB completely contains the box. In which case we can end the query sooner.
341 *      \param          bc      [in] box center
342 *      \param          be      [in] box extents
343 *      \return         true if the OBB contains the whole box
344 */
345///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
346inline_ BOOL OBBCollider::OBBContainsBox(const Point& bc, const Point& be)
347{
348        // I assume if all 8 box vertices are inside the OBB, so does the whole box.
349        // Sounds ok but maybe there's a better way?
350/*
351#define TEST_PT(a,b,c)                                                                                                                                                                                                                          \
352        p.x=a;  p.y=b;  p.z=c;          p+=bc;                                                                                                                                                                                          \
353        f = p.x * mRModelToBox.m[0][0] + p.y * mRModelToBox.m[1][0] + p.z * mRModelToBox.m[2][0];       if(f>mB0.x || f<mB1.x) return FALSE;\
354        f = p.x * mRModelToBox.m[0][1] + p.y * mRModelToBox.m[1][1] + p.z * mRModelToBox.m[2][1];       if(f>mB0.y || f<mB1.y) return FALSE;\
355        f = p.x * mRModelToBox.m[0][2] + p.y * mRModelToBox.m[1][2] + p.z * mRModelToBox.m[2][2];       if(f>mB0.z || f<mB1.z) return FALSE;
356
357        Point p;
358        float f;
359
360        TEST_PT(be.x, be.y, be.z)
361        TEST_PT(-be.x, be.y, be.z)
362        TEST_PT(be.x, -be.y, be.z)
363        TEST_PT(-be.x, -be.y, be.z)
364        TEST_PT(be.x, be.y, -be.z)
365        TEST_PT(-be.x, be.y, -be.z)
366        TEST_PT(be.x, -be.y, -be.z)
367        TEST_PT(-be.x, -be.y, -be.z)
368
369        return TRUE;
370*/
371
372        // Yes there is:
373        // - compute model-box's AABB in OBB space
374        // - test AABB-in-AABB
375        float NCx = bc.x * mRModelToBox.m[0][0] + bc.y * mRModelToBox.m[1][0] + bc.z * mRModelToBox.m[2][0];
376        float NEx = fabsf(mRModelToBox.m[0][0] * be.x) + fabsf(mRModelToBox.m[1][0] * be.y) + fabsf(mRModelToBox.m[2][0] * be.z);
377
378        if(mB0.x < NCx+NEx)     return FALSE;
379        if(mB1.x > NCx-NEx)     return FALSE;
380
381        float NCy = bc.x * mRModelToBox.m[0][1] + bc.y * mRModelToBox.m[1][1] + bc.z * mRModelToBox.m[2][1];
382        float NEy = fabsf(mRModelToBox.m[0][1] * be.x) + fabsf(mRModelToBox.m[1][1] * be.y) + fabsf(mRModelToBox.m[2][1] * be.z);
383
384        if(mB0.y < NCy+NEy)     return FALSE;
385        if(mB1.y > NCy-NEy)     return FALSE;
386
387        float NCz = bc.x * mRModelToBox.m[0][2] + bc.y * mRModelToBox.m[1][2] + bc.z * mRModelToBox.m[2][2];
388        float NEz = fabsf(mRModelToBox.m[0][2] * be.x) + fabsf(mRModelToBox.m[1][2] * be.y) + fabsf(mRModelToBox.m[2][2] * be.z);
389
390        if(mB0.z < NCz+NEz)     return FALSE;
391        if(mB1.z > NCz-NEz)     return FALSE;
392
393        return TRUE;
394}
395
396#define TEST_BOX_IN_OBB(center, extents)        \
397        if(OBBContainsBox(center, extents))             \
398        {                                                                               \
399                /* Set contact status */                        \
400                mFlags |= OPC_CONTACT;                          \
401                _Dump(node);                                            \
402                return;                                                         \
403        }
404
405///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
406/**
407 *      Recursive collision query for normal AABB trees.
408 *      \param          node    [in] current collision node
409 */
410///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
411void OBBCollider::_Collide(const AABBCollisionNode* node)
412{
413        // Perform OBB-AABB overlap test
414        if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;
415
416        TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
417
418        if(node->IsLeaf())
419        {
420                OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
421        }
422        else
423        {
424                _Collide(node->GetPos());
425
426                if(ContactFound()) return;
427
428                _Collide(node->GetNeg());
429        }
430}
431
432///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
433/**
434 *      Recursive collision query for normal AABB trees, without primitive tests.
435 *      \param          node    [in] current collision node
436 */
437///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
438void OBBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
439{
440        // Perform OBB-AABB overlap test
441        if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;
442
443        TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
444
445        if(node->IsLeaf())
446        {
447                SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
448        }
449        else
450        {
451                _CollideNoPrimitiveTest(node->GetPos());
452
453                if(ContactFound()) return;
454
455                _CollideNoPrimitiveTest(node->GetNeg());
456        }
457}
458
459///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
460/**
461 *      Recursive collision query for quantized AABB trees.
462 *      \param          node    [in] current collision node
463 */
464///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
465void OBBCollider::_Collide(const AABBQuantizedNode* node)
466{
467        // Dequantize box
468        const QuantizedAABB& Box = node->mAABB;
469        const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
470        const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
471
472        // Perform OBB-AABB overlap test
473        if(!BoxBoxOverlap(Extents, Center))     return;
474
475        TEST_BOX_IN_OBB(Center, Extents)
476
477        if(node->IsLeaf())
478        {
479                OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
480        }
481        else
482        {
483                _Collide(node->GetPos());
484
485                if(ContactFound()) return;
486
487                _Collide(node->GetNeg());
488        }
489}
490
491///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
492/**
493 *      Recursive collision query for quantized AABB trees, without primitive tests.
494 *      \param          node    [in] current collision node
495 */
496///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
497void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
498{
499        // Dequantize box
500        const QuantizedAABB& Box = node->mAABB;
501        const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
502        const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
503
504        // Perform OBB-AABB overlap test
505        if(!BoxBoxOverlap(Extents, Center))     return;
506
507        TEST_BOX_IN_OBB(Center, Extents)
508
509        if(node->IsLeaf())
510        {
511                SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
512        }
513        else
514        {
515                _CollideNoPrimitiveTest(node->GetPos());
516
517                if(ContactFound()) return;
518
519                _CollideNoPrimitiveTest(node->GetNeg());
520        }
521}
522
523///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
524/**
525 *      Recursive collision query for no-leaf AABB trees.
526 *      \param          node    [in] current collision node
527 */
528///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
529void OBBCollider::_Collide(const AABBNoLeafNode* node)
530{
531        // Perform OBB-AABB overlap test
532        if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;
533
534        TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
535
536        if(node->HasPosLeaf())  { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
537        else                                    _Collide(node->GetPos());
538
539        if(ContactFound()) return;
540
541        if(node->HasNegLeaf())  { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
542        else                                    _Collide(node->GetNeg());
543}
544
545///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
546/**
547 *      Recursive collision query for no-leaf AABB trees, without primitive tests.
548 *      \param          node    [in] current collision node
549 */
550///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
551void OBBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
552{
553        // Perform OBB-AABB overlap test
554        if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;
555
556        TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)
557
558        if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
559        else                                    _CollideNoPrimitiveTest(node->GetPos());
560
561        if(ContactFound()) return;
562
563        if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
564        else                                    _CollideNoPrimitiveTest(node->GetNeg());
565}
566
567///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
568/**
569 *      Recursive collision query for quantized no-leaf AABB trees.
570 *      \param          node    [in] current collision node
571 */
572///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
573void OBBCollider::_Collide(const AABBQuantizedNoLeafNode* node)
574{
575        // Dequantize box
576        const QuantizedAABB& Box = node->mAABB;
577        const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
578        const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
579
580        // Perform OBB-AABB overlap test
581        if(!BoxBoxOverlap(Extents, Center))     return;
582
583        TEST_BOX_IN_OBB(Center, Extents)
584
585        if(node->HasPosLeaf())  { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
586        else                                    _Collide(node->GetPos());
587
588        if(ContactFound()) return;
589
590        if(node->HasNegLeaf())  { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
591        else                                    _Collide(node->GetNeg());
592}
593
594///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
595/**
596 *      Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
597 *      \param          node    [in] current collision node
598 */
599///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
600void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
601{
602        // Dequantize box
603        const QuantizedAABB& Box = node->mAABB;
604        const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
605        const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
606
607        // Perform OBB-AABB overlap test
608        if(!BoxBoxOverlap(Extents, Center))     return;
609
610        TEST_BOX_IN_OBB(Center, Extents)
611
612        if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
613        else                                    _CollideNoPrimitiveTest(node->GetPos());
614
615        if(ContactFound()) return;
616
617        if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
618        else                                    _CollideNoPrimitiveTest(node->GetNeg());
619}
620
621
622
623
624
625
626///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
627/**
628 *      Constructor.
629 */
630///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
631HybridOBBCollider::HybridOBBCollider()
632{
633}
634
635///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
636/**
637 *      Destructor.
638 */
639///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
640HybridOBBCollider::~HybridOBBCollider()
641{
642}
643
644bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
645{
646        // We don't want primitive tests here!
647        mFlags |= OPC_NO_PRIMITIVE_TESTS;
648
649        // Checkings
650        if(!Setup(&model))      return false;
651
652        // Init collision query
653        if(InitQuery(cache, box, worldb, worldm))       return true;
654
655        // Special case for 1-leaf trees
656        if(mCurrentModel && mCurrentModel->HasSingleNode())
657        {
658                // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
659                udword Nb = mIMesh->GetNbTriangles();
660
661                // Loop through all triangles
662                for(udword i=0;i<Nb;i++)
663                {
664                        OBB_PRIM(i, OPC_CONTACT)
665                }
666                return true;
667        }
668
669        // Override destination array since we're only going to get leaf boxes here
670        mTouchedBoxes.Reset();
671        mTouchedPrimitives = &mTouchedBoxes;
672
673        // Now, do the actual query against leaf boxes
674        if(!model.HasLeafNodes())
675        {
676                if(model.IsQuantized())
677                {
678                        const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
679
680                        // Setup dequantization coeffs
681                        mCenterCoeff    = Tree->mCenterCoeff;
682                        mExtentsCoeff   = Tree->mExtentsCoeff;
683
684                        // Perform collision query - we don't want primitive tests here!
685                        _CollideNoPrimitiveTest(Tree->GetNodes());
686                }
687                else
688                {
689                        const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
690
691                        // Perform collision query - we don't want primitive tests here!
692                        _CollideNoPrimitiveTest(Tree->GetNodes());
693                }
694        }
695        else
696        {
697                if(model.IsQuantized())
698                {
699                        const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
700
701                        // Setup dequantization coeffs
702                        mCenterCoeff    = Tree->mCenterCoeff;
703                        mExtentsCoeff   = Tree->mExtentsCoeff;
704
705                        // Perform collision query - we don't want primitive tests here!
706                        _CollideNoPrimitiveTest(Tree->GetNodes());
707                }
708                else
709                {
710                        const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
711
712                        // Perform collision query - we don't want primitive tests here!
713                        _CollideNoPrimitiveTest(Tree->GetNodes());
714                }
715        }
716
717        // We only have a list of boxes so far
718        if(GetContactStatus())
719        {
720                // Reset contact status, since it currently only reflects collisions with leaf boxes
721                Collider::InitQuery();
722
723                // Change dest container so that we can use built-in overlap tests and get collided primitives
724                cache.TouchedPrimitives.Reset();
725                mTouchedPrimitives = &cache.TouchedPrimitives;
726
727                // Read touched leaf boxes
728                udword Nb = mTouchedBoxes.GetNbEntries();
729                const udword* Touched = mTouchedBoxes.GetEntries();
730
731                const LeafTriangles* LT = model.GetLeafTriangles();
732                const udword* Indices = model.GetIndices();
733
734                // Loop through touched leaves
735                while(Nb--)
736                {
737                        const LeafTriangles& CurrentLeaf = LT[*Touched++];
738
739                        // Each leaf box has a set of triangles
740                        udword NbTris = CurrentLeaf.GetNbTriangles();
741                        if(Indices)
742                        {
743                                const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
744
745                                // Loop through triangles and test each of them
746                                while(NbTris--)
747                                {
748                                        udword TriangleIndex = *T++;
749                                        OBB_PRIM(TriangleIndex, OPC_CONTACT)
750                                }
751                        }
752                        else
753                        {
754                                udword BaseIndex = CurrentLeaf.GetTriangleIndex();
755
756                                // Loop through triangles and test each of them
757                                while(NbTris--)
758                                {
759                                        udword TriangleIndex = BaseIndex++;
760                                        OBB_PRIM(TriangleIndex, OPC_CONTACT)
761                                }
762                        }
763                }
764        }
765
766        return true;
767}
Note: See TracBrowser for help on using the repository browser.