From c01469dddabe404506ef3a64542e8423f9e11f2c Mon Sep 17 00:00:00 2001 From: Aki Date: Sun, 30 Jan 2022 17:44:05 +0100 Subject: Converted Opcode and Ice into unix newlines format --- Opcode/OPC_TreeCollider.cpp | 1886 +++++++++++++++++++++---------------------- 1 file changed, 943 insertions(+), 943 deletions(-) (limited to 'Opcode/OPC_TreeCollider.cpp') diff --git a/Opcode/OPC_TreeCollider.cpp b/Opcode/OPC_TreeCollider.cpp index 24b9e2a..490f5e2 100644 --- a/Opcode/OPC_TreeCollider.cpp +++ b/Opcode/OPC_TreeCollider.cpp @@ -1,943 +1,943 @@ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/* - * OPCODE - Optimized Collision Detection - * Copyright (C) 2001 Pierre Terdiman - * Homepage: http://www.codercorner.com/Opcode.htm - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Contains code for a tree collider. - * \file OPC_TreeCollider.cpp - * \author Pierre Terdiman - * \date March, 20, 2001 - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Contains an AABB tree collider. - * This class performs a collision test between two AABB trees. - * - * \class AABBTreeCollider - * \author Pierre Terdiman - * \version 1.3 - * \date March, 20, 2001 -*/ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Precompiled Header -#include "StdAfx.h" - -using namespace Opcode; - -#include "OPC_BoxBoxOverlap.h" -#include "OPC_TriBoxOverlap.h" -#include "OPC_TriTriOverlap.h" - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Constructor. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -AABBTreeCollider::AABBTreeCollider() : - mNbBVBVTests (0), - mNbPrimPrimTests (0), - mNbBVPrimTests (0), - mFullBoxBoxTest (true), - mFullPrimBoxTest (true), - mIMesh0 (null), - mIMesh1 (null) -{ -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Destructor. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -AABBTreeCollider::~AABBTreeCollider() -{ -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Validates current settings. You should call this method after all the settings and callbacks have been defined. - * \return null if everything is ok, else a string describing the problem - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -const char* AABBTreeCollider::ValidateSettings() -{ - if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; - return null; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Generic collision query for generic OPCODE models. After the call, access the results with: - * - GetContactStatus() - * - GetNbPairs() - * - GetPairs() - * - * \param cache [in] collision cache for model pointers and a colliding pair of primitives - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \return true if success - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1) -{ - // Checkings - if(!cache.Model0 || !cache.Model1) return false; - if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false; - if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false; - - /* - - Rules: - - perform hull test - - when hulls collide, disable hull test - - if meshes overlap, reset countdown - - if countdown reaches 0, enable hull test - - */ - -#ifdef __MESHMERIZER_H__ - // Handle hulls - if(cache.HullTest) - { - if(cache.Model0->GetHull() && cache.Model1->GetHull()) - { - struct Local - { - static IcePoint* SVCallback(const IcePoint& sv, udword& previndex, udword user_data) - { - CollisionHull* Hull = (CollisionHull*)user_data; - previndex = Hull->ComputeSupportingVertex(sv, previndex); - return (IcePoint*)&Hull->GetVerts()[previndex]; - } - }; - - bool Collide; - - if(0) - { - static GJKEngine GJK; - static bool GJKInitDone=false; - if(!GJKInitDone) - { - GJK.Enable(GJK_BACKUP_PROCEDURE); - GJK.Enable(GJK_DEGENERATE); - GJK.Enable(GJK_HILLCLIMBING); - GJKInitDone = true; - } - GJK.SetCallbackObj0(Local::SVCallback); - GJK.SetCallbackObj1(Local::SVCallback); - GJK.SetUserData0(udword(cache.Model0->GetHull())); - GJK.SetUserData1(udword(cache.Model1->GetHull())); - Collide = GJK.Collide(*world0, *world1, &cache.SepVector); - } - else - { - static SVEngine SVE; - SVE.SetCallbackObj0(Local::SVCallback); - SVE.SetCallbackObj1(Local::SVCallback); - SVE.SetUserData0(udword(cache.Model0->GetHull())); - SVE.SetUserData1(udword(cache.Model1->GetHull())); - Collide = SVE.Collide(*world0, *world1, &cache.SepVector); - } - - if(!Collide) - { - // Reset stats & contact status - mFlags &= ~OPC_CONTACT; - mNbBVBVTests = 0; - mNbPrimPrimTests = 0; - mNbBVPrimTests = 0; - mPairs.Reset(); - return true; - } - } - } - - // Here, hulls collide - cache.HullTest = false; -#endif // __MESHMERIZER_H__ - - // Checkings - if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false; - - // Simple double-dispatch - bool Status; - if(!cache.Model0->HasLeafNodes()) - { - if(cache.Model0->IsQuantized()) - { - const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree(); - const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - else - { - const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree(); - const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - } - else - { - if(cache.Model0->IsQuantized()) - { - const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree(); - const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - else - { - const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree(); - const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree(); - Status = Collide(T0, T1, world0, world1, &cache); - } - } - -#ifdef __MESHMERIZER_H__ - if(Status) - { - // Reset counter as long as overlap occurs - if(GetContactStatus()) cache.ResetCountDown(); - - // Enable hull test again when counter reaches zero - cache.CountDown--; - if(!cache.CountDown) - { - cache.ResetCountDown(); - cache.HullTest = true; - } - } -#endif - return Status; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Initializes a collision query : - * - reset stats & contact status - * - setup matrices - * - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1) -{ - // Reset stats & contact status - Collider::InitQuery(); - mNbBVBVTests = 0; - mNbPrimPrimTests = 0; - mNbBVPrimTests = 0; - mPairs.Reset(); - - // Setup matrices - Matrix4x4 InvWorld0, InvWorld1; - if(world0) InvertPRMatrix(InvWorld0, *world0); - else InvWorld0.Identity(); - - if(world1) InvertPRMatrix(InvWorld1, *world1); - else InvWorld1.Identity(); - - Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1; - Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0; - - mR0to1 = World0to1; World0to1.GetTrans(mT0to1); - mR1to0 = World1to0; World1to0.GetTrans(mT1to0); - - // Precompute absolute 1-to-0 rotation matrix - for(udword i=0;i<3;i++) - { - for(udword j=0;j<3;j++) - { - // Epsilon value prevents floating-IcePoint inaccuracies (strategy borrowed from RAPID) - mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]); - } - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Takes advantage of temporal coherence. - * \param cache [in] cache for a pair of previously colliding primitives - * \return true if we can return immediately - * \warning only works for "First Contact" mode - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache) -{ - // Checkings - if(!cache) return false; - - // Test previously colliding primitives first - if(TemporalCoherenceEnabled() && FirstContactEnabled()) - { - PrimTest(cache->id0, cache->id1); - if(GetContactStatus()) return true; - } - return false; -} - -#define UPDATE_CACHE \ - if(cache && GetContactStatus()) \ - { \ - cache->id0 = mPairs.GetEntry(0); \ - cache->id1 = mPairs.GetEntry(1); \ - } - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Collision query for normal AABB trees. - * \param tree0 [in] AABB tree from first object - * \param tree1 [in] AABB tree from second object - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \param cache [in/out] cache for a pair of previously colliding primitives - * \return true if success - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) -{ - // Init collision query - InitQuery(world0, world1); - - // Check previous state - if(CheckTemporalCoherence(cache)) return true; - - // Perform collision query - _Collide(tree0->GetNodes(), tree1->GetNodes()); - - UPDATE_CACHE - - return true; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Collision query for no-leaf AABB trees. - * \param tree0 [in] AABB tree from first object - * \param tree1 [in] AABB tree from second object - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \param cache [in/out] cache for a pair of previously colliding primitives - * \return true if success - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) -{ - // Init collision query - InitQuery(world0, world1); - - // Check previous state - if(CheckTemporalCoherence(cache)) return true; - - // Perform collision query - _Collide(tree0->GetNodes(), tree1->GetNodes()); - - UPDATE_CACHE - - return true; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Collision query for quantized AABB trees. - * \param tree0 [in] AABB tree from first object - * \param tree1 [in] AABB tree from second object - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \param cache [in/out] cache for a pair of previously colliding primitives - * \return true if success - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) -{ - // Init collision query - InitQuery(world0, world1); - - // Check previous state - if(CheckTemporalCoherence(cache)) return true; - - // Setup dequantization coeffs - mCenterCoeff0 = tree0->mCenterCoeff; - mExtentsCoeff0 = tree0->mExtentsCoeff; - mCenterCoeff1 = tree1->mCenterCoeff; - mExtentsCoeff1 = tree1->mExtentsCoeff; - - // Dequantize box A - const AABBQuantizedNode* N0 = tree0->GetNodes(); - const IcePoint a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z); - const IcePoint Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z); - // Dequantize box B - const AABBQuantizedNode* N1 = tree1->GetNodes(); - const IcePoint b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z); - const IcePoint Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z); - - // Perform collision query - _Collide(N0, N1, a, Pa, b, Pb); - - UPDATE_CACHE - - return true; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Collision query for quantized no-leaf AABB trees. - * \param tree0 [in] AABB tree from first object - * \param tree1 [in] AABB tree from second object - * \param world0 [in] world matrix for first object - * \param world1 [in] world matrix for second object - * \param cache [in/out] cache for a pair of previously colliding primitives - * \return true if success - * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) -{ - // Init collision query - InitQuery(world0, world1); - - // Check previous state - if(CheckTemporalCoherence(cache)) return true; - - // Setup dequantization coeffs - mCenterCoeff0 = tree0->mCenterCoeff; - mExtentsCoeff0 = tree0->mExtentsCoeff; - mCenterCoeff1 = tree1->mCenterCoeff; - mExtentsCoeff1 = tree1->mExtentsCoeff; - - // Perform collision query - _Collide(tree0->GetNodes(), tree1->GetNodes()); - - UPDATE_CACHE - - return true; -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Standard trees -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -// The normal AABB tree can use 2 different descent rules (with different performances) -//#define ORIGINAL_CODE //!< UNC-like descent rules -#define ALTERNATIVE_CODE //!< Alternative descent rules - -#ifdef ORIGINAL_CODE -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision query for normal AABB trees. - * \param b0 [in] collision node from first tree - * \param b1 [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) -{ - // Perform BV-BV overlap test - if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return; - - if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } - - if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) - { - _Collide(b0->GetNeg(), b1); - if(ContactFound()) return; - _Collide(b0->GetPos(), b1); - } - else - { - _Collide(b0, b1->GetNeg()); - if(ContactFound()) return; - _Collide(b0, b1->GetPos()); - } -} -#endif - -#ifdef ALTERNATIVE_CODE -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision query for normal AABB trees. - * \param b0 [in] collision node from first tree - * \param b1 [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) -{ - // Perform BV-BV overlap test - if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) - { - return; - } - - if(b0->IsLeaf()) - { - if(b1->IsLeaf()) - { - PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); - } - else - { - _Collide(b0, b1->GetNeg()); - if(ContactFound()) return; - _Collide(b0, b1->GetPos()); - } - } - else if(b1->IsLeaf()) - { - _Collide(b0->GetNeg(), b1); - if(ContactFound()) return; - _Collide(b0->GetPos(), b1); - } - else - { - _Collide(b0->GetNeg(), b1->GetNeg()); - if(ContactFound()) return; - _Collide(b0->GetNeg(), b1->GetPos()); - if(ContactFound()) return; - _Collide(b0->GetPos(), b1->GetNeg()); - if(ContactFound()) return; - _Collide(b0->GetPos(), b1->GetPos()); - } -} -#endif - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// No-leaf trees -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Leaf-leaf test for two primitive indices. - * \param id0 [in] index from first leaf-triangle - * \param id1 [in] index from second leaf-triangle - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::PrimTest(udword id0, udword id1) -{ - // Request vertices from the app - VertexPointers VP0; - VertexPointers VP1; - mIMesh0->GetTriangle(VP0, id0); - mIMesh1->GetTriangle(VP1, id1); - - // Transform from space 1 to space 0 - IcePoint u0,u1,u2; - TransformPoint(u0, *VP1.Vertex[0], mR1to0, mT1to0); - TransformPoint(u1, *VP1.Vertex[1], mR1to0, mT1to0); - TransformPoint(u2, *VP1.Vertex[2], mR1to0, mT1to0); - - // Perform triangle-triangle overlap test - if(TriTriOverlap(*VP0.Vertex[0], *VP0.Vertex[1], *VP0.Vertex[2], u0, u1, u2)) - { - // Keep track of colliding pairs - mPairs.Add(id0).Add(id1); - // Set contact status - mFlags |= OPC_CONTACT; - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B. - * \param id1 [in] leaf-triangle index from tree B - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1) -{ - // Request vertices from the app - VertexPointers VP; - mIMesh1->GetTriangle(VP, id1); - - // Perform triangle-triangle overlap test - if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) - { - // Keep track of colliding pairs - mPairs.Add(mLeafIndex).Add(id1); - // Set contact status - mFlags |= OPC_CONTACT; - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A. - * \param id0 [in] leaf-triangle index from tree A - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0) -{ - // Request vertices from the app - VertexPointers VP; - mIMesh0->GetTriangle(VP, id0); - - // Perform triangle-triangle overlap test - if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) - { - // Keep track of colliding pairs - mPairs.Add(id0).Add(mLeafIndex); - // Set contact status - mFlags |= OPC_CONTACT; - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision of a leaf node from A and a branch from B. - * \param b [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b) -{ - // Perform triangle-box overlap test - if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; - - // Keep same triangle, deal with first child - if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - // Keep same triangle, deal with second child - if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision of a leaf node from B and a branch from A. - * \param b [in] collision node from first tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b) -{ - // Perform triangle-box overlap test - if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; - - // Keep same triangle, deal with first child - if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); - else _CollideBoxTri(b->GetPos()); - - if(ContactFound()) return; - - // Keep same triangle, deal with second child - if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); - else _CollideBoxTri(b->GetNeg()); -} - -//! Request triangle vertices from the app and transform them -#define FETCH_LEAF(prim_index, imesh, rot, trans) \ - mLeafIndex = prim_index; \ - /* Request vertices from the app */ \ - VertexPointers VP; imesh->GetTriangle(VP, prim_index); \ - /* Transform them in a common space */ \ - TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \ - TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \ - TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans); - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision query for no-leaf AABB trees. - * \param a [in] collision node from first tree - * \param b [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b) -{ - // Perform BV-BV overlap test - if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return; - - // Catch leaf status - BOOL BHasPosLeaf = b->HasPosLeaf(); - BOOL BHasNegLeaf = b->HasNegLeaf(); - - if(a->HasPosLeaf()) - { - FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) - - if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); - } - else - { - if(BHasPosLeaf) - { - FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetPos()); - } - else _Collide(a->GetPos(), b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) - { - FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetPos()); - } - else _Collide(a->GetPos(), b->GetNeg()); - } - - if(ContactFound()) return; - - if(a->HasNegLeaf()) - { - FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) - - if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); - } - else - { - if(BHasPosLeaf) - { - // ### That leaf has possibly already been fetched - FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetNeg()); - } - else _Collide(a->GetNeg(), b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) - { - // ### That leaf has possibly already been fetched - FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetNeg()); - } - else _Collide(a->GetNeg(), b->GetNeg()); - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Quantized trees -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision query for quantized AABB trees. - * \param b0 [in] collision node from first tree - * \param b1 [in] collision node from second tree - * \param a [in] extent from box A - * \param Pa [in] center from box A - * \param b [in] extent from box B - * \param Pb [in] center from box B - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const IcePoint& a, const IcePoint& Pa, const IcePoint& b, const IcePoint& Pb) -{ - // Perform BV-BV overlap test - if(!BoxBoxOverlap(a, Pa, b, Pb)) return; - - if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } - - if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) - { - // Dequantize box - const QuantizedAABB* Box = &b0->GetNeg()->mAABB; - const IcePoint negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); - const IcePoint nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); - _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb); - - if(ContactFound()) return; - - // Dequantize box - Box = &b0->GetPos()->mAABB; - const IcePoint posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); - const IcePoint posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); - _Collide(b0->GetPos(), b1, posa, posPa, b, Pb); - } - else - { - // Dequantize box - const QuantizedAABB* Box = &b1->GetNeg()->mAABB; - const IcePoint negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); - const IcePoint negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); - _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb); - - if(ContactFound()) return; - - // Dequantize box - Box = &b1->GetPos()->mAABB; - const IcePoint posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); - const IcePoint posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); - _Collide(b0, b1->GetPos(), a, Pa, posb, posPb); - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -// Quantized no-leaf trees -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision of a leaf node from A and a quantized branch from B. - * \param leaf [in] leaf triangle from first tree - * \param b [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b) -{ - // Dequantize box - const QuantizedAABB* bb = &b->mAABB; - const IcePoint Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); - const IcePoint eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); - - // Perform triangle-box overlap test - if(!TriBoxOverlap(Pb, eb)) return; - - if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision of a leaf node from B and a quantized branch from A. - * \param b [in] collision node from first tree - * \param leaf [in] leaf triangle from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b) -{ - // Dequantize box - const QuantizedAABB* bb = &b->mAABB; - const IcePoint Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z); - const IcePoint ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z); - - // Perform triangle-box overlap test - if(!TriBoxOverlap(Pa, ea)) return; - - if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); - else _CollideBoxTri(b->GetPos()); - - if(ContactFound()) return; - - if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); - else _CollideBoxTri(b->GetNeg()); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/** - * Recursive collision query for quantized no-leaf AABB trees. - * \param a [in] collision node from first tree - * \param b [in] collision node from second tree - */ -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b) -{ - // Dequantize box A - const QuantizedAABB* ab = &a->mAABB; - const IcePoint Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z); - const IcePoint ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z); - // Dequantize box B - const QuantizedAABB* bb = &b->mAABB; - const IcePoint Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); - const IcePoint eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); - - // Perform BV-BV overlap test - if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return; - - // Catch leaf status - BOOL BHasPosLeaf = b->HasPosLeaf(); - BOOL BHasNegLeaf = b->HasNegLeaf(); - - if(a->HasPosLeaf()) - { - FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) - - if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); - } - else - { - if(BHasPosLeaf) - { - FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetPos()); - } - else _Collide(a->GetPos(), b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) - { - FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetPos()); - } - else _Collide(a->GetPos(), b->GetNeg()); - } - - if(ContactFound()) return; - - if(a->HasNegLeaf()) - { - FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) - - if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); - else _CollideTriBox(b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); - else _CollideTriBox(b->GetNeg()); - } - else - { - if(BHasPosLeaf) - { - // ### That leaf has possibly already been fetched - FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetNeg()); - } - else _Collide(a->GetNeg(), b->GetPos()); - - if(ContactFound()) return; - - if(BHasNegLeaf) - { - // ### That leaf has possibly already been fetched - FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) - - _CollideBoxTri(a->GetNeg()); - } - else _Collide(a->GetNeg(), b->GetNeg()); - } -} +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* + * OPCODE - Optimized Collision Detection + * Copyright (C) 2001 Pierre Terdiman + * Homepage: http://www.codercorner.com/Opcode.htm + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Contains code for a tree collider. + * \file OPC_TreeCollider.cpp + * \author Pierre Terdiman + * \date March, 20, 2001 + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Contains an AABB tree collider. + * This class performs a collision test between two AABB trees. + * + * \class AABBTreeCollider + * \author Pierre Terdiman + * \version 1.3 + * \date March, 20, 2001 +*/ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Precompiled Header +#include "StdAfx.h" + +using namespace Opcode; + +#include "OPC_BoxBoxOverlap.h" +#include "OPC_TriBoxOverlap.h" +#include "OPC_TriTriOverlap.h" + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Constructor. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +AABBTreeCollider::AABBTreeCollider() : + mNbBVBVTests (0), + mNbPrimPrimTests (0), + mNbBVPrimTests (0), + mFullBoxBoxTest (true), + mFullPrimBoxTest (true), + mIMesh0 (null), + mIMesh1 (null) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Destructor. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +AABBTreeCollider::~AABBTreeCollider() +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Validates current settings. You should call this method after all the settings and callbacks have been defined. + * \return null if everything is ok, else a string describing the problem + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +const char* AABBTreeCollider::ValidateSettings() +{ + if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; + return null; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Generic collision query for generic OPCODE models. After the call, access the results with: + * - GetContactStatus() + * - GetNbPairs() + * - GetPairs() + * + * \param cache [in] collision cache for model pointers and a colliding pair of primitives + * \param world0 [in] world matrix for first object + * \param world1 [in] world matrix for second object + * \return true if success + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1) +{ + // Checkings + if(!cache.Model0 || !cache.Model1) return false; + if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false; + if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false; + + /* + + Rules: + - perform hull test + - when hulls collide, disable hull test + - if meshes overlap, reset countdown + - if countdown reaches 0, enable hull test + + */ + +#ifdef __MESHMERIZER_H__ + // Handle hulls + if(cache.HullTest) + { + if(cache.Model0->GetHull() && cache.Model1->GetHull()) + { + struct Local + { + static IcePoint* SVCallback(const IcePoint& sv, udword& previndex, udword user_data) + { + CollisionHull* Hull = (CollisionHull*)user_data; + previndex = Hull->ComputeSupportingVertex(sv, previndex); + return (IcePoint*)&Hull->GetVerts()[previndex]; + } + }; + + bool Collide; + + if(0) + { + static GJKEngine GJK; + static bool GJKInitDone=false; + if(!GJKInitDone) + { + GJK.Enable(GJK_BACKUP_PROCEDURE); + GJK.Enable(GJK_DEGENERATE); + GJK.Enable(GJK_HILLCLIMBING); + GJKInitDone = true; + } + GJK.SetCallbackObj0(Local::SVCallback); + GJK.SetCallbackObj1(Local::SVCallback); + GJK.SetUserData0(udword(cache.Model0->GetHull())); + GJK.SetUserData1(udword(cache.Model1->GetHull())); + Collide = GJK.Collide(*world0, *world1, &cache.SepVector); + } + else + { + static SVEngine SVE; + SVE.SetCallbackObj0(Local::SVCallback); + SVE.SetCallbackObj1(Local::SVCallback); + SVE.SetUserData0(udword(cache.Model0->GetHull())); + SVE.SetUserData1(udword(cache.Model1->GetHull())); + Collide = SVE.Collide(*world0, *world1, &cache.SepVector); + } + + if(!Collide) + { + // Reset stats & contact status + mFlags &= ~OPC_CONTACT; + mNbBVBVTests = 0; + mNbPrimPrimTests = 0; + mNbBVPrimTests = 0; + mPairs.Reset(); + return true; + } + } + } + + // Here, hulls collide + cache.HullTest = false; +#endif // __MESHMERIZER_H__ + + // Checkings + if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false; + + // Simple double-dispatch + bool Status; + if(!cache.Model0->HasLeafNodes()) + { + if(cache.Model0->IsQuantized()) + { + const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree(); + const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree(); + Status = Collide(T0, T1, world0, world1, &cache); + } + else + { + const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree(); + const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree(); + Status = Collide(T0, T1, world0, world1, &cache); + } + } + else + { + if(cache.Model0->IsQuantized()) + { + const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree(); + const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree(); + Status = Collide(T0, T1, world0, world1, &cache); + } + else + { + const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree(); + const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree(); + Status = Collide(T0, T1, world0, world1, &cache); + } + } + +#ifdef __MESHMERIZER_H__ + if(Status) + { + // Reset counter as long as overlap occurs + if(GetContactStatus()) cache.ResetCountDown(); + + // Enable hull test again when counter reaches zero + cache.CountDown--; + if(!cache.CountDown) + { + cache.ResetCountDown(); + cache.HullTest = true; + } + } +#endif + return Status; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Initializes a collision query : + * - reset stats & contact status + * - setup matrices + * + * \param world0 [in] world matrix for first object + * \param world1 [in] world matrix for second object + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1) +{ + // Reset stats & contact status + Collider::InitQuery(); + mNbBVBVTests = 0; + mNbPrimPrimTests = 0; + mNbBVPrimTests = 0; + mPairs.Reset(); + + // Setup matrices + Matrix4x4 InvWorld0, InvWorld1; + if(world0) InvertPRMatrix(InvWorld0, *world0); + else InvWorld0.Identity(); + + if(world1) InvertPRMatrix(InvWorld1, *world1); + else InvWorld1.Identity(); + + Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1; + Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0; + + mR0to1 = World0to1; World0to1.GetTrans(mT0to1); + mR1to0 = World1to0; World1to0.GetTrans(mT1to0); + + // Precompute absolute 1-to-0 rotation matrix + for(udword i=0;i<3;i++) + { + for(udword j=0;j<3;j++) + { + // Epsilon value prevents floating-IcePoint inaccuracies (strategy borrowed from RAPID) + mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]); + } + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Takes advantage of temporal coherence. + * \param cache [in] cache for a pair of previously colliding primitives + * \return true if we can return immediately + * \warning only works for "First Contact" mode + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache) +{ + // Checkings + if(!cache) return false; + + // Test previously colliding primitives first + if(TemporalCoherenceEnabled() && FirstContactEnabled()) + { + PrimTest(cache->id0, cache->id1); + if(GetContactStatus()) return true; + } + return false; +} + +#define UPDATE_CACHE \ + if(cache && GetContactStatus()) \ + { \ + cache->id0 = mPairs.GetEntry(0); \ + cache->id1 = mPairs.GetEntry(1); \ + } + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Collision query for normal AABB trees. + * \param tree0 [in] AABB tree from first object + * \param tree1 [in] AABB tree from second object + * \param world0 [in] world matrix for first object + * \param world1 [in] world matrix for second object + * \param cache [in/out] cache for a pair of previously colliding primitives + * \return true if success + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) +{ + // Init collision query + InitQuery(world0, world1); + + // Check previous state + if(CheckTemporalCoherence(cache)) return true; + + // Perform collision query + _Collide(tree0->GetNodes(), tree1->GetNodes()); + + UPDATE_CACHE + + return true; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Collision query for no-leaf AABB trees. + * \param tree0 [in] AABB tree from first object + * \param tree1 [in] AABB tree from second object + * \param world0 [in] world matrix for first object + * \param world1 [in] world matrix for second object + * \param cache [in/out] cache for a pair of previously colliding primitives + * \return true if success + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) +{ + // Init collision query + InitQuery(world0, world1); + + // Check previous state + if(CheckTemporalCoherence(cache)) return true; + + // Perform collision query + _Collide(tree0->GetNodes(), tree1->GetNodes()); + + UPDATE_CACHE + + return true; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Collision query for quantized AABB trees. + * \param tree0 [in] AABB tree from first object + * \param tree1 [in] AABB tree from second object + * \param world0 [in] world matrix for first object + * \param world1 [in] world matrix for second object + * \param cache [in/out] cache for a pair of previously colliding primitives + * \return true if success + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) +{ + // Init collision query + InitQuery(world0, world1); + + // Check previous state + if(CheckTemporalCoherence(cache)) return true; + + // Setup dequantization coeffs + mCenterCoeff0 = tree0->mCenterCoeff; + mExtentsCoeff0 = tree0->mExtentsCoeff; + mCenterCoeff1 = tree1->mCenterCoeff; + mExtentsCoeff1 = tree1->mExtentsCoeff; + + // Dequantize box A + const AABBQuantizedNode* N0 = tree0->GetNodes(); + const IcePoint a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z); + const IcePoint Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z); + // Dequantize box B + const AABBQuantizedNode* N1 = tree1->GetNodes(); + const IcePoint b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z); + const IcePoint Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z); + + // Perform collision query + _Collide(N0, N1, a, Pa, b, Pb); + + UPDATE_CACHE + + return true; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Collision query for quantized no-leaf AABB trees. + * \param tree0 [in] AABB tree from first object + * \param tree1 [in] AABB tree from second object + * \param world0 [in] world matrix for first object + * \param world1 [in] world matrix for second object + * \param cache [in/out] cache for a pair of previously colliding primitives + * \return true if success + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache) +{ + // Init collision query + InitQuery(world0, world1); + + // Check previous state + if(CheckTemporalCoherence(cache)) return true; + + // Setup dequantization coeffs + mCenterCoeff0 = tree0->mCenterCoeff; + mExtentsCoeff0 = tree0->mExtentsCoeff; + mCenterCoeff1 = tree1->mCenterCoeff; + mExtentsCoeff1 = tree1->mExtentsCoeff; + + // Perform collision query + _Collide(tree0->GetNodes(), tree1->GetNodes()); + + UPDATE_CACHE + + return true; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Standard trees +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +// The normal AABB tree can use 2 different descent rules (with different performances) +//#define ORIGINAL_CODE //!< UNC-like descent rules +#define ALTERNATIVE_CODE //!< Alternative descent rules + +#ifdef ORIGINAL_CODE +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for normal AABB trees. + * \param b0 [in] collision node from first tree + * \param b1 [in] collision node from second tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) +{ + // Perform BV-BV overlap test + if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return; + + if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } + + if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) + { + _Collide(b0->GetNeg(), b1); + if(ContactFound()) return; + _Collide(b0->GetPos(), b1); + } + else + { + _Collide(b0, b1->GetNeg()); + if(ContactFound()) return; + _Collide(b0, b1->GetPos()); + } +} +#endif + +#ifdef ALTERNATIVE_CODE +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for normal AABB trees. + * \param b0 [in] collision node from first tree + * \param b1 [in] collision node from second tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1) +{ + // Perform BV-BV overlap test + if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) + { + return; + } + + if(b0->IsLeaf()) + { + if(b1->IsLeaf()) + { + PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); + } + else + { + _Collide(b0, b1->GetNeg()); + if(ContactFound()) return; + _Collide(b0, b1->GetPos()); + } + } + else if(b1->IsLeaf()) + { + _Collide(b0->GetNeg(), b1); + if(ContactFound()) return; + _Collide(b0->GetPos(), b1); + } + else + { + _Collide(b0->GetNeg(), b1->GetNeg()); + if(ContactFound()) return; + _Collide(b0->GetNeg(), b1->GetPos()); + if(ContactFound()) return; + _Collide(b0->GetPos(), b1->GetNeg()); + if(ContactFound()) return; + _Collide(b0->GetPos(), b1->GetPos()); + } +} +#endif + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// No-leaf trees +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Leaf-leaf test for two primitive indices. + * \param id0 [in] index from first leaf-triangle + * \param id1 [in] index from second leaf-triangle + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::PrimTest(udword id0, udword id1) +{ + // Request vertices from the app + VertexPointers VP0; + VertexPointers VP1; + mIMesh0->GetTriangle(VP0, id0); + mIMesh1->GetTriangle(VP1, id1); + + // Transform from space 1 to space 0 + IcePoint u0,u1,u2; + TransformPoint(u0, *VP1.Vertex[0], mR1to0, mT1to0); + TransformPoint(u1, *VP1.Vertex[1], mR1to0, mT1to0); + TransformPoint(u2, *VP1.Vertex[2], mR1to0, mT1to0); + + // Perform triangle-triangle overlap test + if(TriTriOverlap(*VP0.Vertex[0], *VP0.Vertex[1], *VP0.Vertex[2], u0, u1, u2)) + { + // Keep track of colliding pairs + mPairs.Add(id0).Add(id1); + // Set contact status + mFlags |= OPC_CONTACT; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Leaf-leaf test for a previously fetched triangle from tree A (in B's space) and a new leaf from B. + * \param id1 [in] leaf-triangle index from tree B + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1) +{ + // Request vertices from the app + VertexPointers VP; + mIMesh1->GetTriangle(VP, id1); + + // Perform triangle-triangle overlap test + if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) + { + // Keep track of colliding pairs + mPairs.Add(mLeafIndex).Add(id1); + // Set contact status + mFlags |= OPC_CONTACT; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Leaf-leaf test for a previously fetched triangle from tree B (in A's space) and a new leaf from A. + * \param id0 [in] leaf-triangle index from tree A + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0) +{ + // Request vertices from the app + VertexPointers VP; + mIMesh0->GetTriangle(VP, id0); + + // Perform triangle-triangle overlap test + if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) + { + // Keep track of colliding pairs + mPairs.Add(id0).Add(mLeafIndex); + // Set contact status + mFlags |= OPC_CONTACT; + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision of a leaf node from A and a branch from B. + * \param b [in] collision node from second tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b) +{ + // Perform triangle-box overlap test + if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; + + // Keep same triangle, deal with first child + if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); + else _CollideTriBox(b->GetPos()); + + if(ContactFound()) return; + + // Keep same triangle, deal with second child + if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); + else _CollideTriBox(b->GetNeg()); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision of a leaf node from B and a branch from A. + * \param b [in] collision node from first tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b) +{ + // Perform triangle-box overlap test + if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return; + + // Keep same triangle, deal with first child + if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); + else _CollideBoxTri(b->GetPos()); + + if(ContactFound()) return; + + // Keep same triangle, deal with second child + if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); + else _CollideBoxTri(b->GetNeg()); +} + +//! Request triangle vertices from the app and transform them +#define FETCH_LEAF(prim_index, imesh, rot, trans) \ + mLeafIndex = prim_index; \ + /* Request vertices from the app */ \ + VertexPointers VP; imesh->GetTriangle(VP, prim_index); \ + /* Transform them in a common space */ \ + TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \ + TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \ + TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans); + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for no-leaf AABB trees. + * \param a [in] collision node from first tree + * \param b [in] collision node from second tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b) +{ + // Perform BV-BV overlap test + if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return; + + // Catch leaf status + BOOL BHasPosLeaf = b->HasPosLeaf(); + BOOL BHasNegLeaf = b->HasNegLeaf(); + + if(a->HasPosLeaf()) + { + FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) + + if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); + else _CollideTriBox(b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); + else _CollideTriBox(b->GetNeg()); + } + else + { + if(BHasPosLeaf) + { + FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetPos()); + } + else _Collide(a->GetPos(), b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) + { + FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetPos()); + } + else _Collide(a->GetPos(), b->GetNeg()); + } + + if(ContactFound()) return; + + if(a->HasNegLeaf()) + { + FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) + + if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); + else _CollideTriBox(b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); + else _CollideTriBox(b->GetNeg()); + } + else + { + if(BHasPosLeaf) + { + // ### That leaf has possibly already been fetched + FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetNeg()); + } + else _Collide(a->GetNeg(), b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) + { + // ### That leaf has possibly already been fetched + FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetNeg()); + } + else _Collide(a->GetNeg(), b->GetNeg()); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Quantized trees +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for quantized AABB trees. + * \param b0 [in] collision node from first tree + * \param b1 [in] collision node from second tree + * \param a [in] extent from box A + * \param Pa [in] center from box A + * \param b [in] extent from box B + * \param Pb [in] center from box B + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const IcePoint& a, const IcePoint& Pa, const IcePoint& b, const IcePoint& Pb) +{ + // Perform BV-BV overlap test + if(!BoxBoxOverlap(a, Pa, b, Pb)) return; + + if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; } + + if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize()))) + { + // Dequantize box + const QuantizedAABB* Box = &b0->GetNeg()->mAABB; + const IcePoint negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); + const IcePoint nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); + _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb); + + if(ContactFound()) return; + + // Dequantize box + Box = &b0->GetPos()->mAABB; + const IcePoint posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z); + const IcePoint posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z); + _Collide(b0->GetPos(), b1, posa, posPa, b, Pb); + } + else + { + // Dequantize box + const QuantizedAABB* Box = &b1->GetNeg()->mAABB; + const IcePoint negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); + const IcePoint negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); + _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb); + + if(ContactFound()) return; + + // Dequantize box + Box = &b1->GetPos()->mAABB; + const IcePoint posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z); + const IcePoint posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z); + _Collide(b0, b1->GetPos(), a, Pa, posb, posPb); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Quantized no-leaf trees +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision of a leaf node from A and a quantized branch from B. + * \param leaf [in] leaf triangle from first tree + * \param b [in] collision node from second tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b) +{ + // Dequantize box + const QuantizedAABB* bb = &b->mAABB; + const IcePoint Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); + const IcePoint eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); + + // Perform triangle-box overlap test + if(!TriBoxOverlap(Pb, eb)) return; + + if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive()); + else _CollideTriBox(b->GetPos()); + + if(ContactFound()) return; + + if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive()); + else _CollideTriBox(b->GetNeg()); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision of a leaf node from B and a quantized branch from A. + * \param b [in] collision node from first tree + * \param leaf [in] leaf triangle from second tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b) +{ + // Dequantize box + const QuantizedAABB* bb = &b->mAABB; + const IcePoint Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z); + const IcePoint ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z); + + // Perform triangle-box overlap test + if(!TriBoxOverlap(Pa, ea)) return; + + if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive()); + else _CollideBoxTri(b->GetPos()); + + if(ContactFound()) return; + + if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive()); + else _CollideBoxTri(b->GetNeg()); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for quantized no-leaf AABB trees. + * \param a [in] collision node from first tree + * \param b [in] collision node from second tree + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b) +{ + // Dequantize box A + const QuantizedAABB* ab = &a->mAABB; + const IcePoint Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z); + const IcePoint ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z); + // Dequantize box B + const QuantizedAABB* bb = &b->mAABB; + const IcePoint Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z); + const IcePoint eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z); + + // Perform BV-BV overlap test + if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return; + + // Catch leaf status + BOOL BHasPosLeaf = b->HasPosLeaf(); + BOOL BHasNegLeaf = b->HasNegLeaf(); + + if(a->HasPosLeaf()) + { + FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1) + + if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); + else _CollideTriBox(b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); + else _CollideTriBox(b->GetNeg()); + } + else + { + if(BHasPosLeaf) + { + FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetPos()); + } + else _Collide(a->GetPos(), b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) + { + FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetPos()); + } + else _Collide(a->GetPos(), b->GetNeg()); + } + + if(ContactFound()) return; + + if(a->HasNegLeaf()) + { + FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1) + + if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive()); + else _CollideTriBox(b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive()); + else _CollideTriBox(b->GetNeg()); + } + else + { + if(BHasPosLeaf) + { + // ### That leaf has possibly already been fetched + FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetNeg()); + } + else _Collide(a->GetNeg(), b->GetPos()); + + if(ContactFound()) return; + + if(BHasNegLeaf) + { + // ### That leaf has possibly already been fetched + FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0) + + _CollideBoxTri(a->GetNeg()); + } + else _Collide(a->GetNeg(), b->GetNeg()); + } +} -- cgit v1.1