int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip)
{
dIASSERT( skip >= (int)sizeof( dContactGeom ) );
dIASSERT( o1->type == dCylinderClass );
dIASSERT( o2->type == dTriMeshClass );
dIASSERT ((flags & NUMC_MASK) >= 1);
// Main data holder
sData cData;
// Assign ODE stuff
cData.gCylinder = o1;
cData.gTrimesh = (dxTriMesh*)o2;
cData.iFlags = flags;
cData.iSkip = skip;
cData.gContact = contact;
cData.nContacts = 0;
_InitCylinderTrimeshData(cData);
OBBCollider& Collider = cData.gTrimesh->_OBBCollider;
Point cCenter((float)cData.vCylinderPos[0],(float)cData.vCylinderPos[1],(float)cData.vCylinderPos[2]);
Point cExtents((float)cData.fCylinderRadius,(float)cData.fCylinderRadius,(float)cData.fCylinderRadius);
cExtents[nCYLINDER_AXIS] = (float)(cData.fCylinderSize * REAL(0.5));
Matrix3x3 obbRot;
// It is a potential issue to explicitly cast to float
// if custom width floating point type is introduced in OPCODE.
// It is necessary to make a typedef and cast to it
// (e.g. typedef float opc_float;)
// However I'm not sure in what header it should be added.
obbRot[0][0] = (float)cData.mCylinderRot[0];
obbRot[1][0] = (float)cData.mCylinderRot[1];
obbRot[2][0] = (float)cData.mCylinderRot[2];
obbRot[0][1] = (float)cData.mCylinderRot[4];
obbRot[1][1] = (float)cData.mCylinderRot[5];
obbRot[2][1] = (float)cData.mCylinderRot[6];
obbRot[0][2] = (float)cData.mCylinderRot[8];
obbRot[1][2] = (float)cData.mCylinderRot[9];
obbRot[2][2] = (float)cData.mCylinderRot[10];
OBB obbCapsule(cCenter,cExtents,obbRot);
Matrix4x4 CapsuleMatrix;
MakeMatrix(cData.vCylinderPos, cData.mCylinderRot, CapsuleMatrix);
Matrix4x4 MeshMatrix;
MakeMatrix(cData.vTrimeshPos, cData.mTrimeshRot, MeshMatrix);
// TC results
if (cData.gTrimesh->doBoxTC)
{
dxTriMesh::BoxTC* BoxTC = 0;
for (int i = 0; i < cData.gTrimesh->BoxTCCache.size(); i++)
{
if (cData.gTrimesh->BoxTCCache[i].Geom == cData.gCylinder)
{
BoxTC = &cData.gTrimesh->BoxTCCache[i];
break;
}
}
if (!BoxTC)
{
cData.gTrimesh->BoxTCCache.push(dxTriMesh::BoxTC());
BoxTC = &cData.gTrimesh->BoxTCCache[cData.gTrimesh->BoxTCCache.size() - 1];
BoxTC->Geom = cData.gCylinder;
BoxTC->FatCoeff = REAL(1.0);
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*BoxTC, obbCapsule, cData.gTrimesh->Data->BVTree, null, &MeshMatrix);
}
else
{
Collider.SetTemporalCoherence(false);
Collider.Collide(dxTriMesh::defaultBoxCache, obbCapsule, cData.gTrimesh->Data->BVTree, null,&MeshMatrix);
}
// Retrieve data
int TriCount = Collider.GetNbTouchedPrimitives();
const int* Triangles = (const int*)Collider.GetTouchedPrimitives();
if (TriCount != 0)
{
if (cData.gTrimesh->ArrayCallback != null)
{
cData.gTrimesh->ArrayCallback(cData.gTrimesh, cData.gCylinder, Triangles, TriCount);
}
// allocate buffer for local contacts on stack
cData.gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.iFlags & NUMC_MASK));
int ctContacts0 = 0;
// loop through all intersecting triangles
for (int i = 0; i < TriCount; i++)
{
const int Triint = Triangles[i];
if (!Callback(cData.gTrimesh, cData.gCylinder, Triint)) continue;
dVector3 dv[3];
FetchTriangle(cData.gTrimesh, Triint, cData.vTrimeshPos, cData.mTrimeshRot, dv);
// test this triangle
TestOneTriangleVsCylinder(cData , dv[0],dv[1],dv[2], false);
// fill-in tri index for generated contacts
for (; ctContacts0<cData.nContacts; ctContacts0++)
cData.gLocalContacts[ctContacts0].triIndex = Triint;
// Putting "break" at the end of loop prevents unnecessary checks on first pass and "continue"
if(cData.nContacts >= (cData.iFlags & NUMC_MASK))
{
break;
}
}
}
return _ProcessLocalContacts(cData);
}
static void dQueryCCTLPotentialCollisionTriangles(OBBCollider &Collider,
const sTrimeshCapsuleColliderData &cData, dxTriMesh *TriMesh, dxGeom *Capsule,
OBBCache &BoxCache)
{
// It is a potential issue to explicitly cast to float
// if custom width floating point type is introduced in OPCODE.
// It is necessary to make a typedef and cast to it
// (e.g. typedef float opc_float;)
// However I'm not sure in what header it should be added.
const dVector3 &vCapsulePosition = cData.m_vCapsulePosition;
Point cCenter(/*(float)*/ vCapsulePosition[0], /*(float)*/ vCapsulePosition[1], /*(float)*/ vCapsulePosition[2]);
Point cExtents(/*(float)*/ cData.m_vCapsuleRadius, /*(float)*/ cData.m_vCapsuleRadius,/*(float)*/ cData.m_fCapsuleSize/2);
Matrix3x3 obbRot;
const dMatrix3 &mCapsuleRotation = cData.m_mCapsuleRotation;
obbRot[0][0] = /*(float)*/ mCapsuleRotation[0];
obbRot[1][0] = /*(float)*/ mCapsuleRotation[1];
obbRot[2][0] = /*(float)*/ mCapsuleRotation[2];
obbRot[0][1] = /*(float)*/ mCapsuleRotation[4];
obbRot[1][1] = /*(float)*/ mCapsuleRotation[5];
obbRot[2][1] = /*(float)*/ mCapsuleRotation[6];
obbRot[0][2] = /*(float)*/ mCapsuleRotation[8];
obbRot[1][2] = /*(float)*/ mCapsuleRotation[9];
obbRot[2][2] = /*(float)*/ mCapsuleRotation[10];
OBB obbCapsule(cCenter,cExtents,obbRot);
Matrix4x4 CapsuleMatrix;
MakeMatrix(vCapsulePosition, mCapsuleRotation, CapsuleMatrix);
Matrix4x4 MeshMatrix;
MakeMatrix(cData.m_mTriMeshPos, cData.m_mTriMeshRot, MeshMatrix);
// TC results
if (TriMesh->doBoxTC) {
dxTriMesh::BoxTC* BoxTC = 0;
for (int i = 0; i < TriMesh->BoxTCCache.size(); i++){
if (TriMesh->BoxTCCache[i].Geom == Capsule){
BoxTC = &TriMesh->BoxTCCache[i];
break;
}
}
if (!BoxTC){
TriMesh->BoxTCCache.push(dxTriMesh::BoxTC());
BoxTC = &TriMesh->BoxTCCache[TriMesh->BoxTCCache.size() - 1];
BoxTC->Geom = Capsule;
BoxTC->FatCoeff = 1.0f;
}
// Intersect
Collider.SetTemporalCoherence(true);
Collider.Collide(*BoxTC, obbCapsule, TriMesh->Data->BVTree, null, &MeshMatrix);
}
else {
Collider.SetTemporalCoherence(false);
Collider.Collide(BoxCache, obbCapsule, TriMesh->Data->BVTree, null,&MeshMatrix);
}
}
