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