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); } }