diff options
Diffstat (limited to 'libs/ode-0.16.1/OPCODE/OPC_OBBCollider.cpp')
| -rw-r--r-- | libs/ode-0.16.1/OPCODE/OPC_OBBCollider.cpp | 767 |
1 files changed, 767 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/OPCODE/OPC_OBBCollider.cpp b/libs/ode-0.16.1/OPCODE/OPC_OBBCollider.cpp new file mode 100644 index 0000000..730c7cc --- /dev/null +++ b/libs/ode-0.16.1/OPCODE/OPC_OBBCollider.cpp @@ -0,0 +1,767 @@ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/* + * OPCODE - Optimized Collision Detection + * Copyright (C) 2001 Pierre Terdiman + * Homepage: http://www.codercorner.com/Opcode.htm + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Contains code for an OBB collider. + * \file OPC_OBBCollider.cpp + * \author Pierre Terdiman + * \date January, 1st, 2002 + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Contains an OBB-vs-tree collider. + * + * \class OBBCollider + * \author Pierre Terdiman + * \version 1.3 + * \date January, 1st, 2002 +*/ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// Precompiled Header +#include "Stdafx.h" + +using namespace Opcode; + +#include "OPC_BoxBoxOverlap.h" +#include "OPC_TriBoxOverlap.h" + +#define SET_CONTACT(prim_index, flag) \ + /* Set contact status */ \ + mFlags |= flag; \ + mTouchedPrimitives->Add(udword(prim_index)); + +//! OBB-triangle test +#define OBB_PRIM(prim_index, flag) \ + /* Request vertices from the app */ \ + VertexPointers VP; ConversionArea VC; mIMesh->GetTriangle(VP, prim_index, VC); \ + /* Transform them in a common space */ \ + TransformPoint(mLeafVerts[0], *VP.Vertex[0], mRModelToBox, mTModelToBox); \ + TransformPoint(mLeafVerts[1], *VP.Vertex[1], mRModelToBox, mTModelToBox); \ + TransformPoint(mLeafVerts[2], *VP.Vertex[2], mRModelToBox, mTModelToBox); \ + /* Perform triangle-box overlap test */ \ + if(TriBoxOverlap()) \ + { \ + SET_CONTACT(prim_index, flag) \ + } + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Constructor. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +OBBCollider::OBBCollider() : mFullBoxBoxTest(true) +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Destructor. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +OBBCollider::~OBBCollider() +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * 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* OBBCollider::ValidateSettings() +{ + if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!"; + + return VolumeCollider::ValidateSettings(); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Generic collision query for generic OPCODE models. After the call, access the results: + * - with GetContactStatus() + * - with GetNbTouchedPrimitives() + * - with GetTouchedPrimitives() + * + * \param cache [in/out] a box cache + * \param box [in] collision OBB in local space + * \param model [in] Opcode model to collide with + * \param worldb [in] OBB's world matrix, or null + * \param worldm [in] model's world matrix, or null + * \return true if success + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool OBBCollider::Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb, const Matrix4x4* worldm) +{ + // Checkings + if(!Setup(&model)) return false; + + // Init collision query + if(InitQuery(cache, box, worldb, worldm)) return true; + + if(!model.HasLeafNodes()) + { + if(model.IsQuantized()) + { + const AABBQuantizedNoLeafTree* Tree = static_cast<const AABBQuantizedNoLeafTree *>(model.GetTree()); + + // Setup dequantization coeffs + mCenterCoeff = Tree->mCenterCoeff; + mExtentsCoeff = Tree->mExtentsCoeff; + + // Perform collision query + if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); + else _Collide(Tree->GetNodes()); + } + else + { + const AABBNoLeafTree* Tree = static_cast<const AABBNoLeafTree *>(model.GetTree()); + + // Perform collision query + if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); + else _Collide(Tree->GetNodes()); + } + } + else + { + if(model.IsQuantized()) + { + const AABBQuantizedTree* Tree = static_cast<const AABBQuantizedTree *>(model.GetTree()); + + // Setup dequantization coeffs + mCenterCoeff = Tree->mCenterCoeff; + mExtentsCoeff = Tree->mExtentsCoeff; + + // Perform collision query + if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); + else _Collide(Tree->GetNodes()); + } + else + { + const AABBCollisionTree* Tree = static_cast<const AABBCollisionTree *>(model.GetTree()); + + // Perform collision query + if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes()); + else _Collide(Tree->GetNodes()); + } + } + + return true; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Initializes a collision query : + * - reset stats & contact status + * - setup matrices + * - check temporal coherence + * + * \param cache [in/out] a box cache + * \param box [in] obb in local space + * \param worldb [in] obb's world matrix, or null + * \param worldm [in] model's world matrix, or null + * \return TRUE if we can return immediately + * \warning SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +BOOL OBBCollider::InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb, const Matrix4x4* worldm) +{ + // 1) Call the base method + VolumeCollider::InitQuery(); + + // 2) Compute obb in world space + mBoxExtents = box.mExtents; + + Matrix4x4 WorldB; + + if(worldb) + { + WorldB = Matrix4x4( box.mRot * Matrix3x3(*worldb) ); + WorldB.SetTrans(box.mCenter * *worldb); + } + else + { + WorldB = box.mRot; + WorldB.SetTrans(box.mCenter); + } + + // Setup matrices + Matrix4x4 InvWorldB; + InvertPRMatrix(InvWorldB, WorldB); + + if(worldm) + { + Matrix4x4 InvWorldM; + InvertPRMatrix(InvWorldM, *worldm); + + Matrix4x4 WorldBtoM = WorldB * InvWorldM; + Matrix4x4 WorldMtoB = *worldm * InvWorldB; + + mRModelToBox = WorldMtoB; WorldMtoB.GetTrans(mTModelToBox); + mRBoxToModel = WorldBtoM; WorldBtoM.GetTrans(mTBoxToModel); + } + else + { + mRModelToBox = InvWorldB; InvWorldB.GetTrans(mTModelToBox); + mRBoxToModel = WorldB; WorldB.GetTrans(mTBoxToModel); + } + + // 3) Setup destination pointer + mTouchedPrimitives = &cache.TouchedPrimitives; + + // 4) Special case: 1-triangle meshes [Opcode 1.3] + if(mCurrentModel && mCurrentModel->HasSingleNode()) + { + if(!SkipPrimitiveTests()) + { + // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0. + mTouchedPrimitives->Reset(); + + // Perform overlap test between the unique triangle and the box (and set contact status if needed) + OBB_PRIM(udword(0), OPC_CONTACT) + + // Return immediately regardless of status + return TRUE; + } + } + + // 5) Check temporal coherence: + if(TemporalCoherenceEnabled()) + { + // Here we use temporal coherence + // => check results from previous frame before performing the collision query + if(FirstContactEnabled()) + { + // We're only interested in the first contact found => test the unique previously touched face + if(mTouchedPrimitives->GetNbEntries()) + { + // Get index of previously touched face = the first entry in the array + udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0); + + // Then reset the array: + // - if the overlap test below is successful, the index we'll get added back anyway + // - if it isn't, then the array should be reset anyway for the normal query + mTouchedPrimitives->Reset(); + + // Perform overlap test between the cached triangle and the box (and set contact status if needed) + OBB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT) + + // Return immediately if possible + if(GetContactStatus()) return TRUE; + } + // else no face has been touched during previous query + // => we'll have to perform a normal query + } + else + { + // ### rewrite this + OBB TestBox(mTBoxToModel, mBoxExtents, mRBoxToModel); + + // We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious): + if(IsCacheValid(cache) && TestBox.IsInside(cache.FatBox)) + { + // - if N is included in P, return previous list + // => we simply leave the list (mTouchedFaces) unchanged + + // Set contact status if needed + if(mTouchedPrimitives->GetNbEntries()) mFlags |= OPC_TEMPORAL_CONTACT; + + // In any case we don't need to do a query + return TRUE; + } + else + { + // - else do the query using a fat N + + // Reset cache since we'll about to perform a real query + mTouchedPrimitives->Reset(); + + // Make a fat box so that coherence will work for subsequent frames + TestBox.mExtents *= cache.FatCoeff; + mBoxExtents *= cache.FatCoeff; + + // Update cache with query data (signature for cached faces) + cache.FatBox = TestBox; + } + } + } + else + { + // Here we don't use temporal coherence => do a normal query + mTouchedPrimitives->Reset(); + } + + // Now we can precompute box-box data + + // Precompute absolute box-to-model rotation matrix + for(udword i=0;i<3;i++) + { + for(udword j=0;j<3;j++) + { + // Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID) + mAR.m[i][j] = 1e-6f + fabsf(mRBoxToModel.m[i][j]); + } + } + + // Precompute bounds for box-in-box test + mB0 = mBoxExtents - mTModelToBox; + mB1 = - mBoxExtents - mTModelToBox; + + // Precompute box-box data - Courtesy of Erwin de Vries + mBBx1 = mBoxExtents.x*mAR.m[0][0] + mBoxExtents.y*mAR.m[1][0] + mBoxExtents.z*mAR.m[2][0]; + mBBy1 = mBoxExtents.x*mAR.m[0][1] + mBoxExtents.y*mAR.m[1][1] + mBoxExtents.z*mAR.m[2][1]; + mBBz1 = mBoxExtents.x*mAR.m[0][2] + mBoxExtents.y*mAR.m[1][2] + mBoxExtents.z*mAR.m[2][2]; + + mBB_1 = mBoxExtents.y*mAR.m[2][0] + mBoxExtents.z*mAR.m[1][0]; + mBB_2 = mBoxExtents.x*mAR.m[2][0] + mBoxExtents.z*mAR.m[0][0]; + mBB_3 = mBoxExtents.x*mAR.m[1][0] + mBoxExtents.y*mAR.m[0][0]; + mBB_4 = mBoxExtents.y*mAR.m[2][1] + mBoxExtents.z*mAR.m[1][1]; + mBB_5 = mBoxExtents.x*mAR.m[2][1] + mBoxExtents.z*mAR.m[0][1]; + mBB_6 = mBoxExtents.x*mAR.m[1][1] + mBoxExtents.y*mAR.m[0][1]; + mBB_7 = mBoxExtents.y*mAR.m[2][2] + mBoxExtents.z*mAR.m[1][2]; + mBB_8 = mBoxExtents.x*mAR.m[2][2] + mBoxExtents.z*mAR.m[0][2]; + mBB_9 = mBoxExtents.x*mAR.m[1][2] + mBoxExtents.y*mAR.m[0][2]; + + return FALSE; +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Checks the OBB completely contains the box. In which case we can end the query sooner. + * \param bc [in] box center + * \param be [in] box extents + * \return true if the OBB contains the whole box + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +inline_ BOOL OBBCollider::OBBContainsBox(const Point& bc, const Point& be) +{ + // I assume if all 8 box vertices are inside the OBB, so does the whole box. + // Sounds ok but maybe there's a better way? +/* +#define TEST_PT(a,b,c) \ + p.x=a; p.y=b; p.z=c; p+=bc; \ + f = p.x * mRModelToBox.m[0][0] + p.y * mRModelToBox.m[1][0] + p.z * mRModelToBox.m[2][0]; if(f>mB0.x || f<mB1.x) return FALSE;\ + f = p.x * mRModelToBox.m[0][1] + p.y * mRModelToBox.m[1][1] + p.z * mRModelToBox.m[2][1]; if(f>mB0.y || f<mB1.y) return FALSE;\ + f = p.x * mRModelToBox.m[0][2] + p.y * mRModelToBox.m[1][2] + p.z * mRModelToBox.m[2][2]; if(f>mB0.z || f<mB1.z) return FALSE; + + Point p; + float f; + + TEST_PT(be.x, be.y, be.z) + TEST_PT(-be.x, be.y, be.z) + TEST_PT(be.x, -be.y, be.z) + TEST_PT(-be.x, -be.y, be.z) + TEST_PT(be.x, be.y, -be.z) + TEST_PT(-be.x, be.y, -be.z) + TEST_PT(be.x, -be.y, -be.z) + TEST_PT(-be.x, -be.y, -be.z) + + return TRUE; +*/ + + // Yes there is: + // - compute model-box's AABB in OBB space + // - test AABB-in-AABB + float NCx = bc.x * mRModelToBox.m[0][0] + bc.y * mRModelToBox.m[1][0] + bc.z * mRModelToBox.m[2][0]; + float NEx = fabsf(mRModelToBox.m[0][0] * be.x) + fabsf(mRModelToBox.m[1][0] * be.y) + fabsf(mRModelToBox.m[2][0] * be.z); + + if(mB0.x < NCx+NEx) return FALSE; + if(mB1.x > NCx-NEx) return FALSE; + + float NCy = bc.x * mRModelToBox.m[0][1] + bc.y * mRModelToBox.m[1][1] + bc.z * mRModelToBox.m[2][1]; + float NEy = fabsf(mRModelToBox.m[0][1] * be.x) + fabsf(mRModelToBox.m[1][1] * be.y) + fabsf(mRModelToBox.m[2][1] * be.z); + + if(mB0.y < NCy+NEy) return FALSE; + if(mB1.y > NCy-NEy) return FALSE; + + float NCz = bc.x * mRModelToBox.m[0][2] + bc.y * mRModelToBox.m[1][2] + bc.z * mRModelToBox.m[2][2]; + float NEz = fabsf(mRModelToBox.m[0][2] * be.x) + fabsf(mRModelToBox.m[1][2] * be.y) + fabsf(mRModelToBox.m[2][2] * be.z); + + if(mB0.z < NCz+NEz) return FALSE; + if(mB1.z > NCz-NEz) return FALSE; + + return TRUE; +} + +#define TEST_BOX_IN_OBB(center, extents) \ + if(OBBContainsBox(center, extents)) \ + { \ + /* Set contact status */ \ + mFlags |= OPC_CONTACT; \ + _Dump(node); \ + return; \ + } + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for normal AABB trees. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_Collide(const AABBCollisionNode* node) +{ + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; + + TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) + + if(node->IsLeaf()) + { + OBB_PRIM(node->GetPrimitive(), OPC_CONTACT) + } + else + { + _Collide(node->GetPos()); + + if(ContactFound()) return; + + _Collide(node->GetNeg()); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for normal AABB trees, without primitive tests. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node) +{ + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; + + TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) + + if(node->IsLeaf()) + { + SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) + } + else + { + _CollideNoPrimitiveTest(node->GetPos()); + + if(ContactFound()) return; + + _CollideNoPrimitiveTest(node->GetNeg()); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for quantized AABB trees. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_Collide(const AABBQuantizedNode* node) +{ + // Dequantize box + const QuantizedAABB& Box = node->mAABB; + const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); + const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); + + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(Extents, Center)) return; + + TEST_BOX_IN_OBB(Center, Extents) + + if(node->IsLeaf()) + { + OBB_PRIM(node->GetPrimitive(), OPC_CONTACT) + } + else + { + _Collide(node->GetPos()); + + if(ContactFound()) return; + + _Collide(node->GetNeg()); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for quantized AABB trees, without primitive tests. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node) +{ + // Dequantize box + const QuantizedAABB& Box = node->mAABB; + const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); + const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); + + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(Extents, Center)) return; + + TEST_BOX_IN_OBB(Center, Extents) + + if(node->IsLeaf()) + { + SET_CONTACT(node->GetPrimitive(), OPC_CONTACT) + } + else + { + _CollideNoPrimitiveTest(node->GetPos()); + + if(ContactFound()) return; + + _CollideNoPrimitiveTest(node->GetNeg()); + } +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for no-leaf AABB trees. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_Collide(const AABBNoLeafNode* node) +{ + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; + + TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) + + if(node->HasPosLeaf()) { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } + else _Collide(node->GetPos()); + + if(ContactFound()) return; + + if(node->HasNegLeaf()) { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } + else _Collide(node->GetNeg()); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for no-leaf AABB trees, without primitive tests. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node) +{ + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter)) return; + + TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents) + + if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } + else _CollideNoPrimitiveTest(node->GetPos()); + + if(ContactFound()) return; + + if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } + else _CollideNoPrimitiveTest(node->GetNeg()); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for quantized no-leaf AABB trees. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_Collide(const AABBQuantizedNoLeafNode* node) +{ + // Dequantize box + const QuantizedAABB& Box = node->mAABB; + const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); + const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); + + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(Extents, Center)) return; + + TEST_BOX_IN_OBB(Center, Extents) + + if(node->HasPosLeaf()) { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) } + else _Collide(node->GetPos()); + + if(ContactFound()) return; + + if(node->HasNegLeaf()) { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) } + else _Collide(node->GetNeg()); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Recursive collision query for quantized no-leaf AABB trees, without primitive tests. + * \param node [in] current collision node + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node) +{ + // Dequantize box + const QuantizedAABB& Box = node->mAABB; + const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z); + const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z); + + // Perform OBB-AABB overlap test + if(!BoxBoxOverlap(Extents, Center)) return; + + TEST_BOX_IN_OBB(Center, Extents) + + if(node->HasPosLeaf()) { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) } + else _CollideNoPrimitiveTest(node->GetPos()); + + if(ContactFound()) return; + + if(node->HasNegLeaf()) { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) } + else _CollideNoPrimitiveTest(node->GetNeg()); +} + + + + + + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Constructor. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +HybridOBBCollider::HybridOBBCollider() +{ +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** + * Destructor. + */ +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +HybridOBBCollider::~HybridOBBCollider() +{ +} + +bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb, const Matrix4x4* worldm) +{ + // We don't want primitive tests here! + mFlags |= OPC_NO_PRIMITIVE_TESTS; + + // Checkings + if(!Setup(&model)) return false; + + // Init collision query + if(InitQuery(cache, box, worldb, worldm)) return true; + + // Special case for 1-leaf trees + if(mCurrentModel && mCurrentModel->HasSingleNode()) + { + // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles + udword Nb = mIMesh->GetNbTriangles(); + + // Loop through all triangles + for(udword i=0;i<Nb;i++) + { + OBB_PRIM(i, OPC_CONTACT) + } + return true; + } + + // Override destination array since we're only going to get leaf boxes here + mTouchedBoxes.Reset(); + mTouchedPrimitives = &mTouchedBoxes; + + // Now, do the actual query against leaf boxes + if(!model.HasLeafNodes()) + { + if(model.IsQuantized()) + { + const AABBQuantizedNoLeafTree* Tree = static_cast<const AABBQuantizedNoLeafTree *>(model.GetTree()); + + // Setup dequantization coeffs + mCenterCoeff = Tree->mCenterCoeff; + mExtentsCoeff = Tree->mExtentsCoeff; + + // Perform collision query - we don't want primitive tests here! + _CollideNoPrimitiveTest(Tree->GetNodes()); + } + else + { + const AABBNoLeafTree* Tree = static_cast<const AABBNoLeafTree *>(model.GetTree()); + + // Perform collision query - we don't want primitive tests here! + _CollideNoPrimitiveTest(Tree->GetNodes()); + } + } + else + { + if(model.IsQuantized()) + { + const AABBQuantizedTree* Tree = static_cast<const AABBQuantizedTree *>(model.GetTree()); + + // Setup dequantization coeffs + mCenterCoeff = Tree->mCenterCoeff; + mExtentsCoeff = Tree->mExtentsCoeff; + + // Perform collision query - we don't want primitive tests here! + _CollideNoPrimitiveTest(Tree->GetNodes()); + } + else + { + const AABBCollisionTree* Tree = static_cast<const AABBCollisionTree *>(model.GetTree()); + + // Perform collision query - we don't want primitive tests here! + _CollideNoPrimitiveTest(Tree->GetNodes()); + } + } + + // We only have a list of boxes so far + if(GetContactStatus()) + { + // Reset contact status, since it currently only reflects collisions with leaf boxes + Collider::InitQuery(); + + // Change dest container so that we can use built-in overlap tests and get collided primitives + cache.TouchedPrimitives.Reset(); + mTouchedPrimitives = &cache.TouchedPrimitives; + + // Read touched leaf boxes + udword Nb = mTouchedBoxes.GetNbEntries(); + const udword* Touched = mTouchedBoxes.GetEntries(); + + const LeafTriangles* LT = model.GetLeafTriangles(); + const udword* Indices = model.GetIndices(); + + // Loop through touched leaves + while(Nb--) + { + const LeafTriangles& CurrentLeaf = LT[*Touched++]; + + // Each leaf box has a set of triangles + udword NbTris = CurrentLeaf.GetNbTriangles(); + if(Indices) + { + const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()]; + + // Loop through triangles and test each of them + while(NbTris--) + { + udword TriangleIndex = *T++; + OBB_PRIM(TriangleIndex, OPC_CONTACT) + } + } + else + { + udword BaseIndex = CurrentLeaf.GetTriangleIndex(); + + // Loop through triangles and test each of them + while(NbTris--) + { + udword TriangleIndex = BaseIndex++; + OBB_PRIM(TriangleIndex, OPC_CONTACT) + } + } + } + } + + return true; +} |