OPC_PlanesCollider.cpp
Go to the documentation of this file.
1 /*
3  * OPCODE - Optimized Collision Detection
4  * Copyright (C) 2001 Pierre Terdiman
5  * Homepage: http://www.codercorner.com/Opcode.htm
6  */
8 
10 
16 
19 
27 
30 // Precompiled Header
31 #include "Stdafx.h"
32 
33 using namespace Opcode;
34 
35 #include "OPC_PlanesAABBOverlap.h"
36 #include "OPC_PlanesTriOverlap.h"
37 
38 #define SET_CONTACT(prim_index, flag) \
39  /* Set contact status */ \
40  mFlags |= flag; \
41  mTouchedPrimitives->Add(prim_index);
42 
44 #define PLANES_PRIM(prim_index, flag) \
45  /* Request vertices from the app */ \
46  mIMesh->GetTriangle(mVP, prim_index); \
47  /* Perform triangle-box overlap test */ \
48  if(PlanesTriOverlap(clip_mask)) \
49  { \
50  SET_CONTACT(prim_index, flag) \
51  }
52 
54 
59  collisionPairInserter(NULL),
60  mNbPlanes (0),
61  mPlanes (null)
62 {
63 }
64 
66 
71 {
73 }
74 
76 
82 {
83  if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
84 
86 }
87 
89 
103 bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm)
105 {
106  // Checkings
107  if(!Setup(&model)) return false;
108 
109  // Init collision query
110  if(InitQuery(cache, planes, nb_planes, worldm)) return true;
111 
112  udword PlaneMask = (1<<nb_planes)-1;
113 
114  if(!model.HasLeafNodes())
115  {
116  if(model.IsQuantized())
117  {
118  const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
119 
120  // Setup dequantization coeffs
121  mCenterCoeff = Tree->mCenterCoeff;
123 
124  // Perform collision query
125  if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
126  else _Collide(Tree->GetNodes(), PlaneMask);
127  }
128  else
129  {
130  const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
131 
132  // Perform collision query
133  if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
134  else _Collide(Tree->GetNodes(), PlaneMask);
135  }
136  }
137  else
138  {
139  if(model.IsQuantized())
140  {
141  const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
142 
143  // Setup dequantization coeffs
144  mCenterCoeff = Tree->mCenterCoeff;
146 
147  // Perform collision query
148  if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
149  else _Collide(Tree->GetNodes(), PlaneMask);
150  }
151  else
152  {
153  const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
154 
155  // Perform collision query
156  if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
157  else _Collide(Tree->GetNodes(), PlaneMask);
158  }
159  }
160  return true;
161 }
162 
164 
177 BOOL PlanesCollider::InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm)
179 {
180  // 1) Call the base method
182 
183  // 2) Compute planes in model space
184  if(nb_planes>mNbPlanes)
185  {
187  mPlanes = new Plane[nb_planes];
188  }
189  mNbPlanes = nb_planes;
190 
191  if(worldm)
192  {
193  Matrix4x4 InvWorldM;
194  InvertPRMatrix(InvWorldM, *worldm);
195 
196 // for(udword i=0;i<nb_planes;i++) mPlanes[i] = planes[i] * InvWorldM;
197  for(udword i=0;i<nb_planes;i++) TransformPlane(mPlanes[i], planes[i], InvWorldM);
198  }
199  else CopyMemory(mPlanes, planes, nb_planes*sizeof(Plane));
200 
201  // 3) Setup destination pointer
203 
204  // 4) Special case: 1-triangle meshes [Opcode 1.3]
206  {
207  if(!SkipPrimitiveTests())
208  {
209  // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
211 
212  // Perform overlap test between the unique triangle and the planes (and set contact status if needed)
213  udword clip_mask = (1<<mNbPlanes)-1;
215 
216  // Return immediately regardless of status
217  return TRUE;
218  }
219  }
220 
221  // 4) Check temporal coherence:
223  {
224  // Here we use temporal coherence
225  // => check results from previous frame before performing the collision query
226  if(FirstContactEnabled())
227  {
228  // We're only interested in the first contact found => test the unique previously touched face
230  {
231  // Get index of previously touched face = the first entry in the array
232  udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
233 
234  // Then reset the array:
235  // - if the overlap test below is successful, the index we'll get added back anyway
236  // - if it isn't, then the array should be reset anyway for the normal query
238 
239  // Perform overlap test between the cached triangle and the planes (and set contact status if needed)
240  udword clip_mask = (1<<mNbPlanes)-1;
241  PLANES_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
242 
243  // Return immediately if possible
244  if(GetContactStatus()) return TRUE;
245  }
246  // else no face has been touched during previous query
247  // => we'll have to perform a normal query
248  }
249  else mTouchedPrimitives->Reset();
250  }
251  else
252  {
253  // Here we don't use temporal coherence => do a normal query
255  }
256 
257  return FALSE;
258 }
259 
260 #define TEST_CLIP_MASK \
261  /* If the box is completely included, so are its children. We don't need to do extra tests, we */ \
262  /* can immediately output a list of visible children. Those ones won't need to be clipped. */ \
263  if(!OutClipMask) \
264  { \
265  /* Set contact status */ \
266  mFlags |= OPC_CONTACT; \
267  _Dump(node); \
268  return; \
269  }
270 
272 
276 void PlanesCollider::_Collide(const AABBCollisionNode* node, udword clip_mask)
278 {
279  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
280  udword OutClipMask;
281  if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
282 
284 
285  // Else the box straddles one or several planes, so we need to recurse down the tree.
286  if(node->IsLeaf())
287  {
288  PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
289  }
290  else
291  {
292  _Collide(node->GetPos(), OutClipMask);
293 
294  if(ContactFound()) return;
295 
296  _Collide(node->GetNeg(), OutClipMask);
297  }
298 }
299 
301 
307 {
308  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
309  udword OutClipMask;
310  if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
311 
313 
314  // Else the box straddles one or several planes, so we need to recurse down the tree.
315  if(node->IsLeaf())
316  {
317  SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
318  }
319  else
320  {
321  _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
322 
323  if(ContactFound()) return;
324 
325  _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
326  }
327 }
328 
330 
334 void PlanesCollider::_Collide(const AABBQuantizedNode* node, udword clip_mask)
336 {
337  // Dequantize box
338  const QuantizedAABB& Box = node->mAABB;
339  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
340  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
341 
342  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
343  udword OutClipMask;
344  if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
345 
347 
348  // Else the box straddles one or several planes, so we need to recurse down the tree.
349  if(node->IsLeaf())
350  {
351  PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
352  }
353  else
354  {
355  _Collide(node->GetPos(), OutClipMask);
356 
357  if(ContactFound()) return;
358 
359  _Collide(node->GetNeg(), OutClipMask);
360  }
361 }
362 
364 
370 {
371  // Dequantize box
372  const QuantizedAABB& Box = node->mAABB;
373  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
374  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
375 
376  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
377  udword OutClipMask;
378  if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
379 
381 
382  // Else the box straddles one or several planes, so we need to recurse down the tree.
383  if(node->IsLeaf())
384  {
385  SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
386  }
387  else
388  {
389  _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
390 
391  if(ContactFound()) return;
392 
393  _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
394  }
395 }
396 
398 
402 void PlanesCollider::_Collide(const AABBNoLeafNode* node, udword clip_mask)
404 {
405  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
406  udword OutClipMask;
407  if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
408 
410 
411  // Else the box straddles one or several planes, so we need to recurse down the tree.
412  if(node->HasPosLeaf()) { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
413  else _Collide(node->GetPos(), OutClipMask);
414 
415  if(ContactFound()) return;
416 
417  if(node->HasNegLeaf()) { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
418  else _Collide(node->GetNeg(), OutClipMask);
419 }
420 
422 
428 {
429  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
430  udword OutClipMask;
431  if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask)) return;
432 
434 
435  // Else the box straddles one or several planes, so we need to recurse down the tree.
436  if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
437  else _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
438 
439  if(ContactFound()) return;
440 
441  if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
442  else _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
443 }
444 
446 
450 void PlanesCollider::_Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask)
452 {
453  // Dequantize box
454  const QuantizedAABB& Box = node->mAABB;
455  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
456  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
457 
458  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
459  udword OutClipMask;
460  if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
461 
463 
464  // Else the box straddles one or several planes, so we need to recurse down the tree.
465  if(node->HasPosLeaf()) { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
466  else _Collide(node->GetPos(), OutClipMask);
467 
468  if(ContactFound()) return;
469 
470  if(node->HasNegLeaf()) { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
471  else _Collide(node->GetNeg(), OutClipMask);
472 }
473 
475 
481 {
482  // Dequantize box
483  const QuantizedAABB& Box = node->mAABB;
484  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
485  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
486 
487  // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
488  udword OutClipMask;
489  if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
490 
492 
493  // Else the box straddles one or several planes, so we need to recurse down the tree.
494  if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
495  else _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
496 
497  if(ContactFound()) return;
498 
499  if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
500  else _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
501 }
502 
503 
504 
505 
506 
507 
508 
510 
515 {
516 }
517 
519 
524 {
525 }
526 
527 bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm)
528 {
529  // We don't want primitive tests here!
531 
532  // Checkings
533  if(!Setup(&model)) return false;
534 
535  // Init collision query
536  if(InitQuery(cache, planes, nb_planes, worldm)) return true;
537 
538  // Special case for 1-leaf trees
540  {
541  // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
542  udword Nb = mIMesh->GetNbTriangles();
543 
544  // Loop through all triangles
545  udword clip_mask = (1<<mNbPlanes)-1;
546  for(udword i=0;i<Nb;i++)
547  {
549  }
550  return true;
551  }
552 
553  // Override destination array since we're only going to get leaf boxes here
554  mTouchedBoxes.Reset();
555  mTouchedPrimitives = &mTouchedBoxes;
556 
557  udword PlaneMask = (1<<nb_planes)-1;
558 
559  // Now, do the actual query against leaf boxes
560  if(!model.HasLeafNodes())
561  {
562  if(model.IsQuantized())
563  {
564  const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
565 
566  // Setup dequantization coeffs
567  mCenterCoeff = Tree->mCenterCoeff;
569 
570  // Perform collision query - we don't want primitive tests here!
571  _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
572  }
573  else
574  {
575  const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
576 
577  // Perform collision query - we don't want primitive tests here!
578  _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
579  }
580  }
581  else
582  {
583  if(model.IsQuantized())
584  {
585  const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
586 
587  // Setup dequantization coeffs
588  mCenterCoeff = Tree->mCenterCoeff;
590 
591  // Perform collision query - we don't want primitive tests here!
592  _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
593  }
594  else
595  {
596  const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
597 
598  // Perform collision query - we don't want primitive tests here!
599  _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
600  }
601  }
602 
603  // We only have a list of boxes so far
604  if(GetContactStatus())
605  {
606  // Reset contact status, since it currently only reflects collisions with leaf boxes
608 
609  // Change dest container so that we can use built-in overlap tests and get collided primitives
610  cache.TouchedPrimitives.Reset();
612 
613  // Read touched leaf boxes
614  udword Nb = mTouchedBoxes.GetNbEntries();
615  const udword* Touched = mTouchedBoxes.GetEntries();
616 
617  const LeafTriangles* LT = model.GetLeafTriangles();
618  const udword* Indices = model.GetIndices();
619 
620  // Loop through touched leaves
621  udword clip_mask = (1<<mNbPlanes)-1;
622  while(Nb--)
623  {
624  const LeafTriangles& CurrentLeaf = LT[*Touched++];
625 
626  // Each leaf box has a set of triangles
627  udword NbTris = CurrentLeaf.GetNbTriangles();
628  if(Indices)
629  {
630  const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
631 
632  // Loop through triangles and test each of them
633  while(NbTris--)
634  {
635  udword TriangleIndex = *T++;
636  PLANES_PRIM(TriangleIndex, OPC_CONTACT)
637  }
638  }
639  else
640  {
641  udword BaseIndex = CurrentLeaf.GetTriangleIndex();
642 
643  // Loop through triangles and test each of them
644  while(NbTris--)
645  {
646  udword TriangleIndex = BaseIndex++;
647  PLANES_PRIM(TriangleIndex, OPC_CONTACT)
648  }
649  }
650  }
651  }
652 
653  return true;
654 }
inline_ BOOL FirstContactEnabled() const
Definition: Opcode.h:61
const MeshInterface * mIMesh
User-defined mesh interface.
Definition: Opcode.h:149
virtual inline_ void InitQuery()
Definition: Opcode.h:174
inline_ udword GetEntry(udword i) const
Returns ith entry.
Definition: OPC_IceHook.h:172
#define FALSE
Definition: OPC_IceHook.h:9
#define null
our own NULL pointer
Definition: IceTypes.h:57
Container TouchedPrimitives
Indices of touched primitives.
Definition: Opcode.h:29
const BaseModel * mCurrentModel
Current model for collision query (owner of touched faces)
Definition: Opcode.h:147
inline_ udword GetNbTriangles() const
Definition: Opcode.h:35
#define TRUE
Definition: OPC_IceHook.h:13
#define DELETEARRAY(x)
Deletes an array.
inline_ const udword * GetIndices() const
Definition: Opcode.h:95
uword mExtents[3]
Quantized extents.
Definition: Opcode.h:115
png_uint_32 i
Definition: png.h:2735
#define PLANES_PRIM(prim_index, flag)
Planes-triangle test.
Leaf descriptor.
Definition: Opcode.h:25
sword mCenter[3]
Quantized center.
Definition: Opcode.h:114
inline_ void Reset()
Definition: OPC_IceHook.h:126
virtual const char * ValidateSettings()=0
int BOOL
Another boolean type.
Definition: IceTypes.h:102
#define SET_CONTACT(prim_index, flag)
inline_ const LeafTriangles * GetLeafTriangles() const
Definition: Opcode.h:87
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
void _CollideNoPrimitiveTest(const AABBCollisionNode *node, udword clip_mask)
inline_ BOOL GetContactStatus() const
Definition: Opcode.h:53
void _Collide(const AABBCollisionNode *node, udword clip_mask)
inline_ BOOL ContactFound() const
Definition: Opcode.h:77
inline_ BOOL Setup(const BaseModel *model)
Definition: Opcode.h:159
udword mFlags
Bit flags.
Definition: Opcode.h:146
Final contact status after a collision query.
Definition: Opcode.h:28
inline_ BOOL SkipPrimitiveTests() const
Definition: Opcode.h:93
bool Collide(PlanesCache &cache, const Plane *planes, udword nb_planes, const HybridModel &model, const Matrix4x4 *worldm=null)
virtual ~PlanesCollider()
#define TEST_CLIP_MASK
ICEMATHS_API void InvertPRMatrix(Matrix4x4 &dest, const Matrix4x4 &src)
inline_ void CopyMemory(void *dest, const void *src, udword size)
inline_ BOOL PlanesAABBOverlap(const Point &center, const Point &extents, udword &out_clip_mask, udword in_clip_mask)
inline_ BOOL IsQuantized() const
Definition: Opcode.h:132
bool Collide(PlanesCache &cache, const Plane *planes, udword nb_planes, const Model &model, const Matrix4x4 *worldm=null)
inline_ void TransformPlane(Plane &transformed, const Plane &plane, const Matrix4x4 &transform)
Definition: IcePlane.h:87
inline_ udword GetTriangleIndex() const
Definition: Opcode.h:43
inline_ BOOL TemporalCoherenceEnabled() const
Definition: Opcode.h:69
Container * mTouchedPrimitives
List of touched primitives.
Definition: Opcode.h:94
inline_ BOOL HasSingleNode() const
Definition: Opcode.h:140
inline_ udword GetNbEntries() const
Returns the current number of entries.
Definition: OPC_IceHook.h:171
Keep or discard primitive-bv tests in leaf nodes (volume-mesh queries)
Definition: Opcode.h:30
inline_ BOOL HasLeafNodes() const
Definition: Opcode.h:124
inline_ udword GetNbTriangles() const
Definition: Opcode.h:61
inline_ const AABBOptimizedTree * GetTree() const
Definition: Opcode.h:99


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:04