OpenSimMirror/libraries/ode-0.9/OPCODE/OPC_SphereCollider.cpp

740 lines
28 KiB
C++

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* OPCODE - Optimized Collision Detection
* Copyright (C) 2001 Pierre Terdiman
* Homepage: http://www.codercorner.com/Opcode.htm
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains code for a sphere collider.
* \file OPC_SphereCollider.cpp
* \author Pierre Terdiman
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Contains a sphere-vs-tree collider.
* This class performs a collision test between a sphere and an AABB tree. You can use this to do a standard player vs world collision,
* in a Nettle/Telemachos way. It doesn't suffer from all reported bugs in those two classic codes - the "new" one by Paul Nettle is a
* debuggued version I think. Collision response can be driven by reported collision data - it works extremely well for me. In sake of
* efficiency, all meshes (that is, all AABB trees) should of course also be kept in an extra hierarchical structure (octree, whatever).
*
* \class SphereCollider
* \author Pierre Terdiman
* \version 1.3
* \date June, 2, 2001
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Precompiled Header
#include "Stdafx.h"
using namespace Opcode;
#include "OPC_SphereAABBOverlap.h"
#include "OPC_SphereTriOverlap.h"
#define SET_CONTACT(prim_index, flag) \
/* Set contact status */ \
mFlags |= flag; \
mTouchedPrimitives->Add(udword(prim_index));
//! Sphere-triangle overlap test
#define SPHERE_PRIM(prim_index, flag) \
/* Request vertices from the app */ \
VertexPointers VP; mIMesh->GetTriangle(VP, prim_index); \
\
/* Perform sphere-tri overlap test */ \
if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2])) \
{ \
SET_CONTACT(prim_index, flag) \
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SphereCollider::SphereCollider()
{
mCenter.Zero();
mRadius2 = 0.0f;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SphereCollider::~SphereCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Generic collision query for generic OPCODE models. After the call, access the results:
* - with GetContactStatus()
* - with GetNbTouchedPrimitives()
* - with GetTouchedPrimitives()
*
* \param cache [in/out] a sphere cache
* \param sphere [in] collision sphere in local space
* \param model [in] Opcode model to collide with
* \param worlds [in] sphere'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 SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
{
// Checkings
if(!Setup(&model)) return false;
// Init collision query
if(InitQuery(cache, sphere, worlds, 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++)
{
SPHERE_PRIM(i, OPC_CONTACT)
}
return true;
}
if(!model.HasLeafNodes())
{
if(model.IsQuantized())
{
const AABBQuantizedNoLeafTree* Tree = (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 = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query
if(SkipPrimitiveTests()) _CollideNoPrimitiveTest(Tree->GetNodes());
else _Collide(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (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 = (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 sphere cache
* \param sphere [in] sphere in local space
* \param worlds [in] sphere'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 SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm)
{
// 1) Call the base method
VolumeCollider::InitQuery();
// 2) Compute sphere in model space:
// - Precompute R^2
mRadius2 = sphere.mRadius * sphere.mRadius;
// - Compute center position
mCenter = sphere.mCenter;
// -> to world space
if(worlds) mCenter *= *worlds;
// -> to model space
if(worldm)
{
// Invert model matrix
Matrix4x4 InvWorldM;
InvertPRMatrix(InvWorldM, *worldm);
mCenter *= InvWorldM;
}
// 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 sphere (and set contact status if needed)
SPHERE_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 sphere (and set contact status if needed)
SPHERE_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
{
// We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious):
float r = sqrtf(cache.FatRadius2) - sphere.mRadius;
if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r)
{
// - 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 sphere so that coherence will work for subsequent frames
mRadius2 *= cache.FatCoeff;
// mRadius2 = (sphere.mRadius * cache.FatCoeff)*(sphere.mRadius * cache.FatCoeff);
// Update cache with query data (signature for cached faces)
cache.Center = mCenter;
cache.FatRadius2 = mRadius2;
}
}
}
else
{
// Here we don't use temporal coherence => do a normal query
mTouchedPrimitives->Reset();
}
return FALSE;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Collision query for vanilla AABB trees.
* \param cache [in/out] a sphere cache
* \param sphere [in] collision sphere in world space
* \param tree [in] AABB tree
* \return true if success
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree)
{
// This is typically called for a scene tree, full of -AABBs-, not full of triangles.
// So we don't really have "primitives" to deal with. Hence it doesn't work with
// "FirstContact" + "TemporalCoherence".
ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
// Checkings
if(!tree) return false;
// Init collision query
if(InitQuery(cache, sphere)) return true;
// Perform collision query
_Collide(tree);
return true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Checks the sphere 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 sphere contains the whole box
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline_ BOOL SphereCollider::SphereContainsBox(const Point& bc, const Point& be)
{
// I assume if all 8 box vertices are inside the sphere, so does the whole box.
// Sounds ok but maybe there's a better way?
Point p;
p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x+be.x; p.y=bc.y-be.y; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
p.x=bc.x-be.x; if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
return TRUE;
}
#define TEST_BOX_IN_SPHERE(center, extents) \
if(SphereContainsBox(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 SphereCollider::_Collide(const AABBCollisionNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->IsLeaf())
{
SPHERE_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 SphereCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(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 SphereCollider::_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 Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(Center, Extents)
if(node->IsLeaf())
{
SPHERE_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 SphereCollider::_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 Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(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 SphereCollider::_Collide(const AABBNoLeafNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SPHERE_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 SphereCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
{
// Perform Sphere-AABB overlap test
if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents)) return;
TEST_BOX_IN_SPHERE(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 SphereCollider::_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 Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(Center, Extents)
if(node->HasPosLeaf()) { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
else _Collide(node->GetPos());
if(ContactFound()) return;
if(node->HasNegLeaf()) { SPHERE_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 SphereCollider::_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 Sphere-AABB overlap test
if(!SphereAABBOverlap(Center, Extents)) return;
TEST_BOX_IN_SPHERE(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());
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Recursive collision query for vanilla AABB trees.
* \param node [in] current collision node
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void SphereCollider::_Collide(const AABBTreeNode* node)
{
// Perform Sphere-AABB overlap test
Point Center, Extents;
node->GetAABB()->GetCenter(Center);
node->GetAABB()->GetExtents(Extents);
if(!SphereAABBOverlap(Center, Extents)) return;
if(node->IsLeaf() || SphereContainsBox(Center, Extents))
{
mFlags |= OPC_CONTACT;
mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
}
else
{
_Collide(node->GetPos());
_Collide(node->GetNeg());
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Constructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridSphereCollider::HybridSphereCollider()
{
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
* Destructor.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
HybridSphereCollider::~HybridSphereCollider()
{
}
bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, 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, sphere, worlds, 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++)
{
SPHERE_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 = (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 = (const AABBNoLeafTree*)model.GetTree();
// Perform collision query - we don't want primitive tests here!
_CollideNoPrimitiveTest(Tree->GetNodes());
}
}
else
{
if(model.IsQuantized())
{
const AABBQuantizedTree* Tree = (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 = (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++;
SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
}
}
else
{
udword BaseIndex = CurrentLeaf.GetTriangleIndex();
// Loop through triangles and test each of them
while(NbTris--)
{
udword TriangleIndex = BaseIndex++;
SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
}
}
}
}
return true;
}