OPC_RayCollider.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 
88 
91 
100 
103 
111 
114 // Precompiled Header
115 #include "Stdafx.h"
116 
117 using namespace Opcode;
118 
119 #include "OPC_RayAABBOverlap.h"
120 #include "OPC_RayTriOverlap.h"
121 
122 #define SET_CONTACT(prim_index, flag) \
123  mNbIntersections++; \
124  /* Set contact status */ \
125  mFlags |= flag; \
126  /* In any case the contact has been found and recorded in mStabbedFace */ \
127  mStabbedFace.mFaceID = prim_index;
128 
129 #ifdef OPC_RAYHIT_CALLBACK
130 
131  #define HANDLE_CONTACT(prim_index, flag) \
132  SET_CONTACT(prim_index, flag) \
133  \
134  if(mHitCallback) (mHitCallback)(mStabbedFace, mUserData);
135 
136  #define UPDATE_CACHE \
137  if(cache && GetContactStatus()) \
138  { \
139  *cache = mStabbedFace.mFaceID; \
140  }
141 #else
142 
143  #define HANDLE_CONTACT(prim_index, flag) \
144  SET_CONTACT(prim_index, flag) \
145  \
146  /* Now we can also record it in mStabbedFaces if available */ \
147  if(mStabbedFaces) \
148  { \
149  /* If we want all faces or if that's the first one we hit */ \
150  if(!mClosestHit || !mStabbedFaces->GetNbFaces()) \
151  { \
152  mStabbedFaces->AddFace(mStabbedFace); \
153  } \
154  else \
155  { \
156  /* We only keep closest hit */ \
157  CollisionFace* Current = const_cast<CollisionFace*>(mStabbedFaces->GetFaces()); \
158  if(Current && mStabbedFace.mDistance<Current->mDistance) \
159  { \
160  *Current = mStabbedFace; \
161  } \
162  } \
163  }
164 
165  #define UPDATE_CACHE \
166  if(cache && GetContactStatus() && mStabbedFaces) \
167  { \
168  const CollisionFace* Current = mStabbedFaces->GetFaces(); \
169  if(Current) *cache = Current->mFaceID; \
170  else *cache = INVALID_ID; \
171  }
172 #endif
173 
174 #define SEGMENT_PRIM(prim_index, flag) \
175  /* Request vertices from the app */ \
176  VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
177  \
178  /* Perform ray-tri overlap test and return */ \
179  if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \
180  { \
181  /* Intersection point is valid if dist < segment's length */ \
182  /* We know dist>0 so we can use integers */ \
183  if(IR(mStabbedFace.mDistance)<IR(mMaxDist)) \
184  { \
185  HANDLE_CONTACT(prim_index, flag) \
186  } \
187  }
188 
189 #define RAY_PRIM(prim_index, flag) \
190  /* Request vertices from the app */ \
191  VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
192  \
193  /* Perform ray-tri overlap test and return */ \
194  if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \
195  { \
196  HANDLE_CONTACT(prim_index, flag) \
197  }
198 
199 
201 
206 #ifdef OPC_RAYHIT_CALLBACK
207  mHitCallback (null),
208  mUserData (0),
209 #else
210  mStabbedFaces (null),
211 #endif
212  mNbRayBVTests (0),
213  mNbRayPrimTests (0),
214  mNbIntersections (0),
215  mMaxDist (MAX_FLOAT),
216 #ifndef OPC_RAYHIT_CALLBACK
217  mClosestHit (false),
218 #endif
219  mCulling (true)
220 {
221 }
222 
224 
229 {
230 }
231 
233 
237 const char* RayCollider::ValidateSettings()
239 {
240  if(mMaxDist<0.0f) return "Higher distance bound must be positive!";
241  if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
242 #ifndef OPC_RAYHIT_CALLBACK
243  if(mClosestHit && FirstContactEnabled()) return "Closest hit doesn't work with ""First contact"" mode!";
244  if(TemporalCoherenceEnabled() && mClosestHit) return "Temporal coherence can't guarantee to report closest hit!";
245 #endif
246  if(SkipPrimitiveTests()) return "SkipPrimitiveTests not possible for RayCollider ! (not implemented)";
247  return null;
248 }
249 
251 
263 bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world, udword* cache)
265 {
266  // Checkings
267  if(!Setup(&model)) return false;
268 
269  // Init collision query
270  if(InitQuery(world_ray, world, cache)) return true;
271 
272  if(!model.HasLeafNodes())
273  {
274  if(model.IsQuantized())
275  {
276  const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
277 
278  // Setup dequantization coeffs
279  mCenterCoeff = Tree->mCenterCoeff;
281 
282  // Perform stabbing query
283  if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
284  else _RayStab(Tree->GetNodes());
285  }
286  else
287  {
288  const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
289 
290  // Perform stabbing query
291  if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
292  else _RayStab(Tree->GetNodes());
293  }
294  }
295  else
296  {
297  if(model.IsQuantized())
298  {
299  const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
300 
301  // Setup dequantization coeffs
302  mCenterCoeff = Tree->mCenterCoeff;
304 
305  // Perform stabbing query
306  if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
307  else _RayStab(Tree->GetNodes());
308  }
309  else
310  {
311  const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
312 
313  // Perform stabbing query
314  if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(Tree->GetNodes());
315  else _RayStab(Tree->GetNodes());
316  }
317  }
318 
319  // Update cache if needed
321  return true;
322 }
323 
324 
326 
338 BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword* face_id)
340 {
341  // Reset stats & contact status
343  mNbRayBVTests = 0;
344  mNbRayPrimTests = 0;
345  mNbIntersections = 0;
346 #ifndef OPC_RAYHIT_CALLBACK
347  if(mStabbedFaces) mStabbedFaces->Reset();
348 #endif
349 
350  // Compute ray in local space
351  // The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests)
352  if(world)
353  {
354  Matrix3x3 InvWorld = *world;
355  mDir = InvWorld * world_ray.mDir;
356 
358  InvertPRMatrix(World, *world);
359  mOrigin = world_ray.mOrig * World;
360  }
361  else
362  {
363  mDir = world_ray.mDir;
364  mOrigin = world_ray.mOrig;
365  }
366 
367  // 4) Special case: 1-triangle meshes [Opcode 1.3]
369  {
370  // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
371  if(!SkipPrimitiveTests())
372  {
373  // Perform overlap test between the unique triangle and the ray (and set contact status if needed)
375 
376  // Return immediately regardless of status
377  return TRUE;
378  }
379  }
380 
381  // Check temporal coherence :
382 
383  // Test previously colliding primitives first
384  if(TemporalCoherenceEnabled() && FirstContactEnabled() && face_id && *face_id!=INVALID_ID)
385  {
386 #ifdef OLD_CODE
387 #ifndef OPC_RAYHIT_CALLBACK
388  if(!mClosestHit)
389 #endif
390  {
391  // Request vertices from the app
392  VertexPointers VP;
393  mIMesh->GetTriangle(VP, *face_id);
394  // Perform ray-cached tri overlap test
395  if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
396  {
397  // Intersection point is valid if:
398  // - distance is positive (else it can just be a face behind the orig point)
399  // - distance is smaller than a given max distance (useful for shadow feelers)
400 // if(mStabbedFace.mDistance>0.0f && mStabbedFace.mDistance<mMaxDist)
401  if(IR(mStabbedFace.mDistance)<IR(mMaxDist)) // The other test is already performed in RayTriOverlap
402  {
403  // Set contact status
405 
406  mStabbedFace.mFaceID = *face_id;
407 
408 #ifndef OPC_RAYHIT_CALLBACK
409  if(mStabbedFaces) mStabbedFaces->AddFace(mStabbedFace);
410 #endif
411  return TRUE;
412  }
413  }
414  }
415 #else
416  // New code
417  // We handle both Segment/ray queries with the same segment code, and a possible infinite limit
419 
420  // Return immediately if possible
421  if(GetContactStatus()) return TRUE;
422 #endif
423  }
424 
425  // Precompute data (moved after temporal coherence since only needed for ray-AABB)
427  {
428  // For Segment-AABB overlap
429  mData = 0.5f * mDir * mMaxDist;
430  mData2 = mOrigin + mData;
431 
432  // Precompute mFDir;
433  mFDir.x = fabsf(mData.x);
434  mFDir.y = fabsf(mData.y);
435  mFDir.z = fabsf(mData.z);
436  }
437  else
438  {
439  // For Ray-AABB overlap
440 // udword x = SIR(mDir.x)-1;
441 // udword y = SIR(mDir.y)-1;
442 // udword z = SIR(mDir.z)-1;
443 // mData.x = FR(x);
444 // mData.y = FR(y);
445 // mData.z = FR(z);
446 
447  // Precompute mFDir;
448  mFDir.x = fabsf(mDir.x);
449  mFDir.y = fabsf(mDir.y);
450  mFDir.z = fabsf(mDir.z);
451  }
452 
453  return FALSE;
454 }
455 
457 
464 bool RayCollider::Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices)
466 {
467  // ### bad design here
468 
469  // This is typically called for a scene tree, full of -AABBs-, not full of triangles.
470  // So we don't really have "primitives" to deal with. Hence it doesn't work with
471  // "FirstContact" + "TemporalCoherence".
473 
474  // Checkings
475  if(!tree) return false;
476 
477  // Init collision query
478  // Basically this is only called to initialize precomputed data
479  if(InitQuery(world_ray)) return true;
480 
481  // Perform stabbing query
482  if(IR(mMaxDist)!=IEEE_MAX_FLOAT) _SegmentStab(tree, box_indices);
483  else _RayStab(tree, box_indices);
484 
485  return true;
486 }
487 
488 
490 
496 {
497  // Perform Segment-AABB overlap test
498  if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
499 
500  if(node->IsLeaf())
501  {
502  SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
503  }
504  else
505  {
506  _SegmentStab(node->GetPos());
507 
508  if(ContactFound()) return;
509 
510  _SegmentStab(node->GetNeg());
511  }
512 }
513 
515 
521 {
522  // Dequantize box
523  const QuantizedAABB& Box = node->mAABB;
524  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
525  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
526 
527  // Perform Segment-AABB overlap test
528  if(!SegmentAABBOverlap(Center, Extents)) return;
529 
530  if(node->IsLeaf())
531  {
532  SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
533  }
534  else
535  {
536  _SegmentStab(node->GetPos());
537 
538  if(ContactFound()) return;
539 
540  _SegmentStab(node->GetNeg());
541  }
542 }
543 
545 
551 {
552  // Perform Segment-AABB overlap test
553  if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
554 
555  if(node->HasPosLeaf())
556  {
557  SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
558  }
559  else _SegmentStab(node->GetPos());
560 
561  if(ContactFound()) return;
562 
563  if(node->HasNegLeaf())
564  {
565  SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
566  }
567  else _SegmentStab(node->GetNeg());
568 }
569 
571 
577 {
578  // Dequantize box
579  const QuantizedAABB& Box = node->mAABB;
580  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
581  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
582 
583  // Perform Segment-AABB overlap test
584  if(!SegmentAABBOverlap(Center, Extents)) return;
585 
586  if(node->HasPosLeaf())
587  {
588  SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
589  }
590  else _SegmentStab(node->GetPos());
591 
592  if(ContactFound()) return;
593 
594  if(node->HasNegLeaf())
595  {
596  SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
597  }
598  else _SegmentStab(node->GetNeg());
599 }
600 
602 
607 void RayCollider::_SegmentStab(const AABBTreeNode* node, Container& box_indices)
609 {
610  // Test the box against the segment
611  Point Center, Extents;
612  node->GetAABB()->GetCenter(Center);
613  node->GetAABB()->GetExtents(Extents);
614  if(!SegmentAABBOverlap(Center, Extents)) return;
615 
616  if(node->IsLeaf())
617  {
618  box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
619  }
620  else
621  {
622  _SegmentStab(node->GetPos(), box_indices);
623  _SegmentStab(node->GetNeg(), box_indices);
624  }
625 }
626 
628 
634 {
635  // Perform Ray-AABB overlap test
636  if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
637 
638  if(node->IsLeaf())
639  {
640  RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
641  }
642  else
643  {
644  _RayStab(node->GetPos());
645 
646  if(ContactFound()) return;
647 
648  _RayStab(node->GetNeg());
649  }
650 }
651 
653 
659 {
660  // Dequantize box
661  const QuantizedAABB& Box = node->mAABB;
662  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
663  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
664 
665  // Perform Ray-AABB overlap test
666  if(!RayAABBOverlap(Center, Extents)) return;
667 
668  if(node->IsLeaf())
669  {
670  RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
671  }
672  else
673  {
674  _RayStab(node->GetPos());
675 
676  if(ContactFound()) return;
677 
678  _RayStab(node->GetNeg());
679  }
680 }
681 
683 
687 void RayCollider::_RayStab(const AABBNoLeafNode* node)
689 {
690  // Perform Ray-AABB overlap test
691  if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
692 
693  if(node->HasPosLeaf())
694  {
695  RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
696  }
697  else _RayStab(node->GetPos());
698 
699  if(ContactFound()) return;
700 
701  if(node->HasNegLeaf())
702  {
703  RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
704  }
705  else _RayStab(node->GetNeg());
706 }
707 
709 
715 {
716  // Dequantize box
717  const QuantizedAABB& Box = node->mAABB;
718  const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
719  const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
720 
721  // Perform Ray-AABB overlap test
722  if(!RayAABBOverlap(Center, Extents)) return;
723 
724  if(node->HasPosLeaf())
725  {
726  RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
727  }
728  else _RayStab(node->GetPos());
729 
730  if(ContactFound()) return;
731 
732  if(node->HasNegLeaf())
733  {
734  RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
735  }
736  else _RayStab(node->GetNeg());
737 }
738 
740 
745 void RayCollider::_RayStab(const AABBTreeNode* node, Container& box_indices)
747 {
748  // Test the box against the ray
749  Point Center, Extents;
750  node->GetAABB()->GetCenter(Center);
751  node->GetAABB()->GetExtents(Extents);
752  if(!RayAABBOverlap(Center, Extents)) return;
753 
754  if(node->IsLeaf())
755  {
756  mFlags |= OPC_CONTACT;
757  box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
758  }
759  else
760  {
761  _RayStab(node->GetPos(), box_indices);
762  _RayStab(node->GetNeg(), box_indices);
763  }
764 }
inline_ BOOL FirstContactEnabled() const
Definition: Opcode.h:61
const MeshInterface * mIMesh
User-defined mesh interface.
Definition: Opcode.h:149
#define IR(x)
Integer representation of a floating-point value.
Definition: IceFPU.h:18
Point mCenterCoeff
Definition: Opcode.h:199
virtual inline_ void InitQuery()
Definition: Opcode.h:174
#define IEEE_MAX_FLOAT
integer representation of MAX_FLOAT
Definition: IceTypes.h:134
#define FALSE
Definition: OPC_IceHook.h:9
#define null
our own NULL pointer
Definition: IceTypes.h:57
inline_ udword GetNbPrimitives() const
Definition: Opcode.h:90
const BaseModel * mCurrentModel
Current model for collision query (owner of touched faces)
Definition: Opcode.h:147
void _SegmentStab(const AABBCollisionNode *node)
bool Collide(const Ray &world_ray, const Model &model, const Matrix4x4 *world=null, udword *cache=null)
inline_ BOOL RayTriOverlap(const Point &vert0, const Point &vert1, const Point &vert2)
#define TRUE
Definition: OPC_IceHook.h:13
const Point * Vertex[3]
Definition: Opcode.h:26
uword mExtents[3]
Quantized extents.
Definition: Opcode.h:115
#define UPDATE_CACHE
Point mOrigin
Ray origin.
Definition: Opcode.h:181
udword mNbRayPrimTests
Number of Ray-Primitive tests.
Definition: Opcode.h:195
sword mCenter[3]
Quantized center.
Definition: Opcode.h:114
#define OPC_RAYHIT_CALLBACK
Use a callback in the ray collider.
Definition: Opcode.h:46
inline_ const udword * GetPrimitives() const
Definition: Opcode.h:89
virtual const char * ValidateSettings()=0
int BOOL
Another boolean type.
Definition: IceTypes.h:102
Point mFDir
fabsf(mDir)
Definition: Opcode.h:183
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
inline_ BOOL GetContactStatus() const
Definition: Opcode.h:53
CollisionFace mStabbedFace
Current stabbed face.
Definition: Opcode.h:186
inline_ BOOL ContactFound() const
Definition: Opcode.h:77
inline_ BOOL Setup(const BaseModel *model)
Definition: Opcode.h:159
#define RAY_PRIM(prim_index, flag)
udword mFlags
Bit flags.
Definition: Opcode.h:146
inline_ Container & Add(udword entry)
Definition: OPC_IceHook.h:48
#define MAX_FLOAT
max possible float value
Definition: IceTypes.h:130
inline_ BOOL RayAABBOverlap(const Point &center, const Point &extents)
inline_ void GetTriangle(VertexPointers &vp, udword index) const
Definition: Opcode.h:119
Final contact status after a collision query.
Definition: Opcode.h:28
inline_ BOOL SkipPrimitiveTests() const
Definition: Opcode.h:93
Point mOrig
Ray origin.
Definition: OPC_IceHook.h:31
float mDistance
Distance from collider to hitpoint.
Definition: Opcode.h:33
float mMaxDist
Valid segment on the ray.
Definition: Opcode.h:202
#define ASSERT(exp)
Definition: OPC_IceHook.h:24
#define SEGMENT_PRIM(prim_index, flag)
ICEMATHS_API void InvertPRMatrix(Matrix4x4 &dest, const Matrix4x4 &src)
udword mNbRayBVTests
Number of Ray-BV tests.
Definition: Opcode.h:194
udword mFaceID
Index of touched face.
Definition: Opcode.h:32
void _RayStab(const AABBCollisionNode *node)
#define INVALID_ID
Invalid dword ID (counterpart of null pointers)
Definition: IceTypes.h:92
inline_ BOOL IsQuantized() const
Definition: Opcode.h:132
Point mDir
Normalized direction.
Definition: OPC_IceHook.h:32
Point mExtentsCoeff
Definition: Opcode.h:200
inline_ BOOL TemporalCoherenceEnabled() const
Definition: Opcode.h:69
inline_ BOOL HasSingleNode() const
Definition: Opcode.h:140
inline_ BOOL HasLeafNodes() const
Definition: Opcode.h:124
Point mDir
Ray direction (normalized)
Definition: Opcode.h:182
udword mNbIntersections
Number of valid intersections.
Definition: Opcode.h:197
inline_ const AABBOptimizedTree * GetTree() const
Definition: Opcode.h:99
inline_ BOOL SegmentAABBOverlap(const Point &center, const Point &extents)
virtual ~RayCollider()


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