aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp
diff options
context:
space:
mode:
authordan miller2007-10-19 05:15:33 +0000
committerdan miller2007-10-19 05:15:33 +0000
commit79eca25c945a535a7a0325999034bae17da92412 (patch)
tree40ff433d94859d629aac933d5ec73b382f62ba1a /libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp
parentadding ode source to /libraries (diff)
downloadopensim-SC-79eca25c945a535a7a0325999034bae17da92412.zip
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.gz
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.bz2
opensim-SC-79eca25c945a535a7a0325999034bae17da92412.tar.xz
resubmitting ode
Diffstat (limited to 'libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp')
-rw-r--r--libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp767
1 files changed, 767 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp
new file mode 100644
index 0000000..028d000
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_OBBCollider.cpp
@@ -0,0 +1,767 @@
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}
generated by cgit v1.1 at 2024-12-28 08:34:30 +0000