OPC_TreeCollider.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 
28 
30 #include"../CollisionPairInserter.h"
31 
33 // Precompiled Header
34 #include "Stdafx.h"
35 
36 #include<iostream>
37 
38 using namespace Opcode;
39 
40 #include "OPC_BoxBoxOverlap.h"
41 //#include "OPC_TriBoxOverlap.h"
42 #include "OPC_TriTriOverlap.h"
43 
45 
48 AABBTreeCollider::AABBTreeCollider() :
50  mIMesh0 (null),
51  mIMesh1 (null),
52  mNbBVBVTests (0),
53  mNbPrimPrimTests (0),
54  mNbBVPrimTests (0),
55  mFullBoxBoxTest (true),
56  mFullPrimBoxTest (true),
57  collisionPairInserter(0)
58 {
59 }
60 
61 
63 
66 AABBTreeCollider::~AABBTreeCollider()
68 {
69 }
70 
72 
76 const char* AABBTreeCollider::ValidateSettings()
78 {
79  if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
80  return null;
81 }
82 
84 
96 bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1)
98 {
99  // Checkings
100  if(!cache.Model0 || !cache.Model1) return false;
101  if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false;
102  if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false;
103 
104  /*
105 
106  Rules:
107  - perform hull test
108  - when hulls collide, disable hull test
109  - if meshes overlap, reset countdown
110  - if countdown reaches 0, enable hull test
111 
112  */
113 
114 #ifdef __MESHMERIZER_H__
115  // Handle hulls
116  if(cache.HullTest)
117  {
118  if(cache.Model0->GetHull() && cache.Model1->GetHull())
119  {
120  struct Local
121  {
122  static Point* SVCallback(const Point& sv, udword& previndex, udword user_data)
123  {
124  CollisionHull* Hull = (CollisionHull*)user_data;
125  previndex = Hull->ComputeSupportingVertex(sv, previndex);
126  return (Point*)&Hull->GetVerts()[previndex];
127  }
128  };
129 
130  bool Collide;
131 
132  if(0)
133  {
134  static GJKEngine GJK;
135  static bool GJKInitDone=false;
136  if(!GJKInitDone)
137  {
138  GJK.Enable(GJK_BACKUP_PROCEDURE);
139  GJK.Enable(GJK_DEGENERATE);
140  GJK.Enable(GJK_HILLCLIMBING);
141  GJKInitDone = true;
142  }
143  GJK.SetCallbackObj0(Local::SVCallback);
144  GJK.SetCallbackObj1(Local::SVCallback);
145  GJK.SetUserData0(udword(cache.Model0->GetHull()));
146  GJK.SetUserData1(udword(cache.Model1->GetHull()));
147  Collide = GJK.Collide(*world0, *world1, &cache.SepVector);
148  }
149  else
150  {
151  static SVEngine SVE;
152  SVE.SetCallbackObj0(Local::SVCallback);
153  SVE.SetCallbackObj1(Local::SVCallback);
154  SVE.SetUserData0(udword(cache.Model0->GetHull()));
155  SVE.SetUserData1(udword(cache.Model1->GetHull()));
156  Collide = SVE.Collide(*world0, *world1, &cache.SepVector);
157  }
158 
159  if(!Collide)
160  {
161  // Reset stats & contact status
162  mFlags &= ~OPC_CONTACT;
163  mNbBVBVTests = 0;
164  mNbPrimPrimTests = 0;
165  mNbBVPrimTests = 0;
166  mPairs.Reset();
167  return true;
168  }
169  }
170  }
171 
172  // Here, hulls collide
173  cache.HullTest = false;
174 #endif // __MESHMERIZER_H__
175 
176  // Checkings
177  if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
178 
179  // Simple double-dispatch
180  bool Status;
181  if(!cache.Model0->HasLeafNodes())
182  {
183  if(cache.Model0->IsQuantized())
184  {
185  const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree();
186  const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree();
187  Status = Collide(T0, T1, world0, world1, &cache);
188  }
189  else
190  {
191  const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree();
192  const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree();
193  Status = Collide(T0, T1, world0, world1, &cache);
194  }
195  }
196  else
197  {
198  if(cache.Model0->IsQuantized())
199  {
200  const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree();
201  const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree();
202  Status = Collide(T0, T1, world0, world1, &cache);
203  }
204  else
205  {
206  const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
207  const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
208  Status = Collide(T0, T1, world0, world1, &cache);
209  }
210  }
211 
212 #ifdef __MESHMERIZER_H__
213  if(Status)
214  {
215  // Reset counter as long as overlap occurs
216  if(GetContactStatus()) cache.ResetCountDown();
217 
218  // Enable hull test again when counter reaches zero
219  cache.CountDown--;
220  if(!cache.CountDown)
221  {
222  cache.ResetCountDown();
223  cache.HullTest = true;
224  }
225  }
226 #endif
227  return Status;
228 }
229 
231 
240 void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1)
242 {
243  // Reset stats & contact status
244  Collider::InitQuery();
245  mNbBVBVTests = 0;
246  mNbPrimPrimTests = 0;
247  mNbBVPrimTests = 0;
248  mPairs.Reset();
249 
250  // Setup matrices
251  Matrix4x4 InvWorld0, InvWorld1;
252  if(world0) InvertPRMatrix(InvWorld0, *world0);
253  else InvWorld0.Identity();
254 
255  if(world1) InvertPRMatrix(InvWorld1, *world1);
256  else InvWorld1.Identity();
257 
258  Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1;
259  Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0;
260 
261  mR0to1 = World0to1; World0to1.GetTrans(mT0to1);
262  mR1to0 = World1to0; World1to0.GetTrans(mT1to0);
263 
264  // Precompute absolute 1-to-0 rotation matrix
265  for(udword i=0;i<3;i++)
266  {
267  for(udword j=0;j<3;j++)
268  {
269  // Epsilon value prevents floating-point inaccuracies (strategy borrowed from CD)
270  mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
271  }
272  }
273 
274 
275  //Modified by S-cubed, Inc.
276  //$B9TNs$r(B Boost $B$N7A<0$K$"$o$;$k!#(B
277  // Prim-Prim $B%F%9%H$r(B TriOverlap.cpp $B$G9T$&$?$a!#(B
278  if(collisionPairInserter){
279 
280  for(udword i=0; i<3; i++){
281  for(udword j=0; j<3; j++){
282  collisionPairInserter->CD_Rot1(i,j) = (double) world0->m[j][i];
283  collisionPairInserter->CD_Rot2(i,j) = (double) world1->m[j][i];
284  }
285  }
286 
287  IceMaths::Point t0;
288  IceMaths::Point t1;
289  world0->GetTrans(t0);
290  world1->GetTrans(t1);
291  // Matrix3x3 w1 = world1;
292  // mT0to1 = w1 * (t1 - t0)
293  collisionPairInserter->CD_Trans1[0] = (double) t0.x;
294  collisionPairInserter->CD_Trans1[1] = (double) t0.y;
295  collisionPairInserter->CD_Trans1[2] = (double) t0.z;
296 
297  collisionPairInserter->CD_Trans2[0] = (double) t1.x;
298  collisionPairInserter->CD_Trans2[1] = (double) t1.y;
299  collisionPairInserter->CD_Trans2[2] = (double) t1.z;
300 
301  //s1, s2 $B$O(B 1.0 $B$N$_$J$N$G!"8GDj!#(B
302  collisionPairInserter->CD_s1 = 1.0;
303  collisionPairInserter->CD_s2 = 1.0;
304  }
305 }
306 
307 
309 
315 bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache)
317 {
318  // Checkings
319  if(!cache) return false;
320 
321  // Test previously colliding primitives first
322  if(TemporalCoherenceEnabled() && FirstContactEnabled())
323  {
324  PrimTest(cache->id0, cache->id1);
325  if(GetContactStatus()) return true;
326  }
327  return false;
328 }
329 
330 #define UPDATE_CACHE \
331  if(cache && GetContactStatus()) \
332  { \
333  cache->id0 = mPairs.GetEntry(0); \
334  cache->id1 = mPairs.GetEntry(1); \
335  }
336 
338 
348 bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
350 {
351  // Init collision query
352  InitQuery(world0, world1);
353 
354  // Check previous state
355  if(CheckTemporalCoherence(cache)) return true;
356 
357  // Perform collision query
358  _Collide(tree0->GetNodes(), tree1->GetNodes());
359 
360  UPDATE_CACHE
361 
362  return true;
363 }
364 
366 
376 bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
378 {
379  // Init collision query
380  InitQuery(world0, world1);
381 
382  // Check previous state
383  if(CheckTemporalCoherence(cache)) return true;
384 
385  // Perform collision query
386  _Collide(tree0->GetNodes(), tree1->GetNodes());
387 
388  UPDATE_CACHE
389 
390  return true;
391 }
392 
394 
404 bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
406 {
407  // Init collision query
408  InitQuery(world0, world1);
409 
410  // Check previous state
411  if(CheckTemporalCoherence(cache)) return true;
412 
413  // Setup dequantization coeffs
414  mCenterCoeff0 = tree0->mCenterCoeff;
415  mExtentsCoeff0 = tree0->mExtentsCoeff;
416  mCenterCoeff1 = tree1->mCenterCoeff;
417  mExtentsCoeff1 = tree1->mExtentsCoeff;
418 
419  // Dequantize box A
420  const AABBQuantizedNode* N0 = tree0->GetNodes();
421  const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z);
422  const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z);
423  // Dequantize box B
424  const AABBQuantizedNode* N1 = tree1->GetNodes();
425  const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z);
426  const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z);
427 
428  // Perform collision query
429  _Collide(N0, N1, a, Pa, b, Pb);
430 
431  UPDATE_CACHE
432 
433  return true;
434 }
435 
437 
447 bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
449 {
450  // Init collision query
451  InitQuery(world0, world1);
452 
453  // Check previous state
454  if(CheckTemporalCoherence(cache)) return true;
455 
456  // Setup dequantization coeffs
457  mCenterCoeff0 = tree0->mCenterCoeff;
458  mExtentsCoeff0 = tree0->mExtentsCoeff;
459  mCenterCoeff1 = tree1->mCenterCoeff;
460  mExtentsCoeff1 = tree1->mExtentsCoeff;
461 
462  // Perform collision query
463  _Collide(tree0->GetNodes(), tree1->GetNodes());
464 
465  UPDATE_CACHE
466 
467  return true;
468 }
469 
471 // Standard trees
473 
474 // The normal AABB tree can use 2 different descent rules (with different performances)
475 //#define ORIGINAL_CODE //!< UNC-like descent rules
476 #define ALTERNATIVE_CODE
477 
478 #ifdef ORIGINAL_CODE
479 
485 void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
487 {
488  mNowNode0 = b0;
489  mNowNode1 = b1;
490  // Perform BV-BV overlap test
491  if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return;
492 
493  if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
494 
495  if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
496  {
497  _Collide(b0->GetNeg(), b1);
498  if(ContactFound()) return;
499  _Collide(b0->GetPos(), b1);
500  }
501  else
502  {
503  _Collide(b0, b1->GetNeg());
504  if(ContactFound()) return;
505  _Collide(b0, b1->GetPos());
506  }
507 }
508 #endif
509 
510 #ifdef ALTERNATIVE_CODE
511 
517 void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
519 {
520  // Perform BV-BV overlap test
521  if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))
522  {
523  return;
524  }
525 
526  if(b0->IsLeaf())
527  {
528  if(b1->IsLeaf())
529  {
530  mNowNode0 = b0;
531  mNowNode1 = b1;
532  PrimTest(b0->GetPrimitive(), b1->GetPrimitive());
533  }
534  else
535  {
536  _Collide(b0, b1->GetNeg());
537  if(ContactFound()) return;
538  _Collide(b0, b1->GetPos());
539  }
540  }
541  else if(b1->IsLeaf())
542  {
543  _Collide(b0->GetNeg(), b1);
544  if(ContactFound()) return;
545  _Collide(b0->GetPos(), b1);
546  }
547  else
548  {
549  _Collide(b0->GetNeg(), b1->GetNeg());
550  if(ContactFound()) return;
551  _Collide(b0->GetNeg(), b1->GetPos());
552  if(ContactFound()) return;
553  _Collide(b0->GetPos(), b1->GetNeg());
554  if(ContactFound()) return;
555  _Collide(b0->GetPos(), b1->GetPos());
556  }
557 }
558 #endif
559 
561 // No-leaf trees
563 
565 
570 void AABBTreeCollider::PrimTest(udword id0, udword id1)
572 {
573  mId0 = id0;
574  mId1 = id1;
575  // Request vertices from the app
576  VertexPointers VP0;
577  VertexPointers VP1;
578  mIMesh0->GetTriangle(VP0, id0);
579  mIMesh1->GetTriangle(VP1, id1);
580 
581  // Modified by S-cubed, Inc.
582  // Transform from space 0 (old : 1) to space 1 (old : 0)
583  // CD $B$G$OJQ49$,5U$J$N$G$"$o$;$k!#(B
584  Point u0,u1,u2;
585  TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1);
586  TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1);
587  TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1);
588 
589  // Perform triangle-triangle overlap test
590  if(TriTriOverlap(u0, u1, u2,
591  *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2]))
592  {
593  // Keep track of colliding pairs
594  mPairs.Add(id0).Add(id1);
595  // Set contact status
596  mFlags |= OPC_CONTACT;
597  }
598 }
599 
601 
605 inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
607 {
608  // Request vertices from the app
609  VertexPointers VP;
610  mIMesh1->GetTriangle(VP, id1);
611 
612  // Perform triangle-triangle overlap test
613  if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
614  {
615  // Keep track of colliding pairs
616  mPairs.Add(mLeafIndex).Add(id1);
617  // Set contact status
618  mFlags |= OPC_CONTACT;
619  }
620 }
621 
623 
627 inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
629 {
630  // Request vertices from the app
631  VertexPointers VP;
632  mIMesh0->GetTriangle(VP, id0);
633 
634  // Perform triangle-triangle overlap test
635  if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
636  {
637  // Keep track of colliding pairs
638  mPairs.Add(id0).Add(mLeafIndex);
639  // Set contact status
640  mFlags |= OPC_CONTACT;
641  }
642 }
643 
645 
649 void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
651 {
652  // Perform triangle-box overlap test
653  // Modified by S-cubed, Inc.
654  //NoleafNode $B$O;HMQ$7$J$$(B
655  // if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
656 
657  // Keep same triangle, deal with first child
658  if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
659  else _CollideTriBox(b->GetPos());
660 
661  if(ContactFound()) return;
662 
663  // Keep same triangle, deal with second child
664  if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
665  else _CollideTriBox(b->GetNeg());
666 }
667 
669 
673 void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
675 {
676  // Modified by S-cubed, Inc.
677  //NoleafNode $B$O;HMQ$7$J$$(B
678  // Perform triangle-box overlap test
679  //if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
680 
681  // Keep same triangle, deal with first child
682  if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
683  else _CollideBoxTri(b->GetPos());
684 
685  if(ContactFound()) return;
686 
687  // Keep same triangle, deal with second child
688  if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
689  else _CollideBoxTri(b->GetNeg());
690 }
691 
693 #define FETCH_LEAF(prim_index, imesh, rot, trans) \
694  mLeafIndex = prim_index; \
695  /* Request vertices from the app */ \
696  VertexPointers VP; imesh->GetTriangle(VP, prim_index); \
697  /* Transform them in a common space */ \
698  TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \
699  TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \
700  TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);
701 
703 
708 void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
710 {
711  // Perform BV-BV overlap test
712  if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return;
713 
714  // Catch leaf status
715  BOOL BHasPosLeaf = b->HasPosLeaf();
716  BOOL BHasNegLeaf = b->HasNegLeaf();
717 
718  if(a->HasPosLeaf())
719  {
720  FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
721 
722  if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
723  else _CollideTriBox(b->GetPos());
724 
725  if(ContactFound()) return;
726 
727  if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
728  else _CollideTriBox(b->GetNeg());
729  }
730  else
731  {
732  if(BHasPosLeaf)
733  {
734  FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
735 
736  _CollideBoxTri(a->GetPos());
737  }
738  else _Collide(a->GetPos(), b->GetPos());
739 
740  if(ContactFound()) return;
741 
742  if(BHasNegLeaf)
743  {
744  FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
745 
746  _CollideBoxTri(a->GetPos());
747  }
748  else _Collide(a->GetPos(), b->GetNeg());
749  }
750 
751  if(ContactFound()) return;
752 
753  if(a->HasNegLeaf())
754  {
755  FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
756 
757  if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
758  else _CollideTriBox(b->GetPos());
759 
760  if(ContactFound()) return;
761 
762  if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
763  else _CollideTriBox(b->GetNeg());
764  }
765  else
766  {
767  if(BHasPosLeaf)
768  {
769  // ### That leaf has possibly already been fetched
770  FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
771 
772  _CollideBoxTri(a->GetNeg());
773  }
774  else _Collide(a->GetNeg(), b->GetPos());
775 
776  if(ContactFound()) return;
777 
778  if(BHasNegLeaf)
779  {
780  // ### That leaf has possibly already been fetched
781  FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
782 
783  _CollideBoxTri(a->GetNeg());
784  }
785  else _Collide(a->GetNeg(), b->GetNeg());
786  }
787 }
788 
790 // Quantized trees
792 
794 
803 void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb)
805 {
806  // Perform BV-BV overlap test
807  if(!BoxBoxOverlap(a, Pa, b, Pb)) return;
808 
809  if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
810 
811  if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
812  {
813  // Dequantize box
814  const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
815  const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
816  const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
817  _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);
818 
819  if(ContactFound()) return;
820 
821  // Dequantize box
822  Box = &b0->GetPos()->mAABB;
823  const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
824  const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
825  _Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
826  }
827  else
828  {
829  // Dequantize box
830  const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
831  const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
832  const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
833  _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);
834 
835  if(ContactFound()) return;
836 
837  // Dequantize box
838  Box = &b1->GetPos()->mAABB;
839  const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
840  const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
841  _Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
842  }
843 }
844 
846 // Quantized no-leaf trees
848 
850 
855 void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
857 {
858 
859  // Dequantize box
860  const QuantizedAABB* bb = &b->mAABB;
861  const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
862  const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
863 
864  // Perform triangle-box overlap test
865  // Modified by S-cubed, Inc.
866  //NoleafNode $B$O;HMQ$7$J$$(B
867  //if(!TriBoxOverlap(Pb, eb)) return;
868 
869  if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
870  else _CollideTriBox(b->GetPos());
871 
872  if(ContactFound()) return;
873 
874  if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
875  else _CollideTriBox(b->GetNeg());
876 }
877 
879 
884 void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
886 {
887  // Dequantize box
888  const QuantizedAABB* bb = &b->mAABB;
889  const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
890  const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);
891 
892 
893  // Modified by S-cubed, Inc.
894  //NoleafNode $B$O;HMQ$7$J$$(B
895  // Perform triangle-box overlap test
896  // if(!TriBoxOverlap(Pa, ea)) return;
897 
898  if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
899  else _CollideBoxTri(b->GetPos());
900 
901  if(ContactFound()) return;
902 
903  if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
904  else _CollideBoxTri(b->GetNeg());
905 }
906 
908 
913 void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
915 {
916  // Dequantize box A
917  const QuantizedAABB* ab = &a->mAABB;
918  const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
919  const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
920  // Dequantize box B
921  const QuantizedAABB* bb = &b->mAABB;
922  const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
923  const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
924 
925  // Perform BV-BV overlap test
926  if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return;
927 
928  // Catch leaf status
929  BOOL BHasPosLeaf = b->HasPosLeaf();
930  BOOL BHasNegLeaf = b->HasNegLeaf();
931 
932  if(a->HasPosLeaf())
933  {
934  FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
935 
936  if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
937  else _CollideTriBox(b->GetPos());
938 
939  if(ContactFound()) return;
940 
941  if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
942  else _CollideTriBox(b->GetNeg());
943  }
944  else
945  {
946  if(BHasPosLeaf)
947  {
948  FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
949 
950  _CollideBoxTri(a->GetPos());
951  }
952  else _Collide(a->GetPos(), b->GetPos());
953 
954  if(ContactFound()) return;
955 
956  if(BHasNegLeaf)
957  {
958  FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
959 
960  _CollideBoxTri(a->GetPos());
961  }
962  else _Collide(a->GetPos(), b->GetNeg());
963  }
964 
965  if(ContactFound()) return;
966 
967  if(a->HasNegLeaf())
968  {
969  FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
970 
971  if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
972  else _CollideTriBox(b->GetPos());
973 
974  if(ContactFound()) return;
975 
976  if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
977  else _CollideTriBox(b->GetNeg());
978  }
979  else
980  {
981  if(BHasPosLeaf)
982  {
983  // ### That leaf has possibly already been fetched
984  FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
985 
986  _CollideBoxTri(a->GetNeg());
987  }
988  else _Collide(a->GetNeg(), b->GetPos());
989 
990  if(ContactFound()) return;
991 
992  if(BHasNegLeaf)
993  {
994  // ### That leaf has possibly already been fetched
995  FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
996 
997  _CollideBoxTri(a->GetNeg());
998  }
999  else _Collide(a->GetNeg(), b->GetNeg());
1000  }
1001 }
Opcode::AABBCollisionNode::GetSize
inline_ float GetSize() const
Definition: Opcode.h:82
AABBTreeCollider::PrimTestTriIndex
inline_ void PrimTestTriIndex(udword id1)
null
#define null
our own NULL pointer
Definition: IceTypes.h:57
AABBTreeCollider::CheckTemporalCoherence
bool CheckTemporalCoherence(Pair *cache)
OPC_CONTACT
Final contact status after a collision query.
Definition: OPC_Collider.h:27
IceCore::Pair
A generic couple structure.
Definition: OPC_IceHook.h:17
inline_
#define inline_
Definition: IcePreprocessor.h:103
Opcode::VertexPointers::Vertex
const Point * Vertex[3]
Definition: Opcode.h:26
Opcode::QuantizedAABB::mExtents
uword mExtents[3]
Quantized extents.
Definition: Opcode.h:115
i
png_uint_32 i
Definition: png.h:2735
Opcode::BVTCache::Model1
const Model * Model1
Model for second object.
Definition: Opcode.h:61
AABBTreeCollider::_CollideTriBox
void _CollideTriBox(const AABBNoLeafNode *b)
b
long b
Definition: jpegint.h:371
Collider::InitQuery
virtual inline_ void InitQuery()
Definition: OPC_Collider.h:173
IceCore::Pair::id1
udword id1
Second index of the pair.
Definition: OPC_IceHook.h:23
Opcode::QuantizedAABB::mCenter
sword mCenter[3]
Quantized center.
Definition: Opcode.h:114
IceMaths::Matrix4x4::GetTrans
inline_ const HPoint & GetTrans() const
Returns the translation part of the matrix.
Definition: OPC_IceHook.h:96
Opcode::BaseModel::GetTree
inline_ const AABBOptimizedTree * GetTree() const
Definition: Opcode.h:99
FETCH_LEAF
#define FETCH_LEAF(prim_index, imesh, rot, trans)
Request triangle vertices from the app and transform them.
Definition: OPC_TreeCollider.cpp:693
BOOL
int BOOL
Another boolean type.
Definition: IceTypes.h:102
IceMaths::Matrix4x4::Identity
inline_ void Identity()
Sets the identity matrix.
Definition: OPC_IceHook.h:175
udword
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
Opcode::BaseModel::IsQuantized
inline_ BOOL IsQuantized() const
Definition: Opcode.h:132
Collider::ValidateSettings
virtual const char * ValidateSettings()=0
AABBTreeCollider::~AABBTreeCollider
virtual ~AABBTreeCollider()
j
def j(str, encoding="cp932")
Opcode::AABBQuantizedNode::GetSize
inline_ uword GetSize() const
Definition: Opcode.h:105
AABBTreeCollider::_Collide
void _Collide(const AABBCollisionNode *b0, const AABBCollisionNode *b1)
IceCore::Pair::id0
udword id0
First index of the pair.
Definition: OPC_IceHook.h:22
a
string a
OPC_BoxBoxOverlap.h
AABBTreeCollider::Collide
bool Collide(BVTCache &cache, const Matrix4x4 *world0=null, const Matrix4x4 *world1=null)
UPDATE_CACHE
#define UPDATE_CACHE
Definition: OPC_TreeCollider.cpp:330
InvertPRMatrix
ICEMATHS_API void InvertPRMatrix(Matrix4x4 &dest, const Matrix4x4 &src)
Opcode::BVTCache::Model0
const Model * Model0
Model for first object.
Definition: Opcode.h:60
Opcode::BVTCache::ResetCountDown
inline_ void ResetCountDown()
Definition: Opcode.h:53
TransformPoint
inline_ void TransformPoint(Point &dest, const Point &source, const Matrix3x3 &rot, const Point &trans)
Quickly rotates & translates a vector.
Definition: OPC_Common.h:118
OPC_TriTriOverlap.h
Opcode::BaseModel::HasLeafNodes
inline_ BOOL HasLeafNodes() const
Definition: Opcode.h:124
AABBTreeCollider::PrimTestIndexTri
inline_ void PrimTestIndexTri(udword id0)
Opcode::BaseModel::GetMeshInterface
inline_ const MeshInterface * GetMeshInterface() const
Definition: Opcode.h:156
AABBTreeCollider::_CollideBoxTri
void _CollideBoxTri(const AABBNoLeafNode *b)
AABBTreeCollider::PrimTest
void PrimTest(udword id0, udword id1)

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:40