diff options
author | dan miller | 2007-10-19 05:24:38 +0000 |
---|---|---|
committer | dan miller | 2007-10-19 05:24:38 +0000 |
commit | f205de7847da7ae1c10212d82e7042d0100b4ce0 (patch) | |
tree | 9acc9608a6880502aaeda43af52c33e278e95b9c /libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp | |
parent | trying to fix my screwup part deux (diff) | |
download | opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.zip opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.gz opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.bz2 opensim-SC-f205de7847da7ae1c10212d82e7042d0100b4ce0.tar.xz |
from the start... checking in ode-0.9
Diffstat (limited to 'libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp')
-rw-r--r-- | libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp | 739 |
1 files changed, 739 insertions, 0 deletions
diff --git a/libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp b/libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp new file mode 100644 index 0000000..65350b7 --- /dev/null +++ b/libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp | |||
@@ -0,0 +1,739 @@ | |||
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 | |||
37 | using 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
63 | SphereCollider::SphereCollider() | ||
64 | { | ||
65 | mCenter.Zero(); | ||
66 | mRadius2 = 0.0f; | ||
67 | } | ||
68 | |||
69 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
70 | /** | ||
71 | * Destructor. | ||
72 | */ | ||
73 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
74 | SphereCollider::~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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
94 | bool 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
179 | BOOL 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
297 | bool 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
324 | inline_ 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
356 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
383 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
410 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
442 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
474 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
496 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
518 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
545 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
572 | void 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 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
603 | HybridSphereCollider::HybridSphereCollider() | ||
604 | { | ||
605 | } | ||
606 | |||
607 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
608 | /** | ||
609 | * Destructor. | ||
610 | */ | ||
611 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
612 | HybridSphereCollider::~HybridSphereCollider() | ||
613 | { | ||
614 | } | ||
615 | |||
616 | bool 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 | } | ||