PfxInt32 pfxContactLargeTriMesh(
PfxContactCache &contacts,
const PfxLargeTriMesh *lmeshA,
const PfxTransform3 &transformA,
const PfxShape &shapeB,
const PfxTransform3 &transformB,
PfxFloat distanceThreshold)
{
PfxTransform3 transformAB;
PfxMatrix3 matrixAB;
PfxVector3 offsetAB;
// Bローカル→Aローカルへの変換
transformAB = orthoInverse(transformA) * transformB;
matrixAB = transformAB.getUpper3x3();
offsetAB = transformAB.getTranslation();
// -----------------------------------------------------
// LargeTriMeshに含まれるTriMeshのAABBと凸体のAABBを判定し、
// 交差するものを個別に衝突判定する。※LargeMesh座標系
PfxVector3 shapeHalf(0.0f);
PfxVector3 shapeCenter = offsetAB;
switch(shapeB.getType()) {
case kPfxShapeSphere:
shapeHalf = PfxVector3(shapeB.getSphere().m_radius);
break;
case kPfxShapeCapsule:
{
PfxCapsule capsule = shapeB.getCapsule();
shapeHalf = absPerElem(matrixAB) * PfxVector3(capsule.m_halfLen+capsule.m_radius,capsule.m_radius,capsule.m_radius);
}
break;
case kPfxShapeCylinder:
{
PfxCylinder cylinder = shapeB.getCylinder();
shapeHalf = absPerElem(matrixAB) * PfxVector3(cylinder.m_halfLen,cylinder.m_radius,cylinder.m_radius);
}
break;
case kPfxShapeBox:
shapeHalf = absPerElem(matrixAB) * shapeB.getBox().m_half;
break;
case kPfxShapeConvexMesh:
shapeHalf = absPerElem(matrixAB) * shapeB.getConvexMesh()->m_half;
break;
default:
break;
}
// -----------------------------------------------------
// アイランドとの衝突判定
PfxVecInt3 aabbMinL,aabbMaxL;
lmeshA->getLocalPosition((shapeCenter-shapeHalf),(shapeCenter+shapeHalf),aabbMinL,aabbMaxL);
PfxUInt32 numIslands = lmeshA->m_numIslands;
{
for(PfxUInt32 i=0;i<numIslands;i++) {
// AABBチェック
PfxAabb16 aabbB = lmeshA->m_aabbList[i];
if(aabbMaxL.getX() < pfxGetXMin(aabbB) || aabbMinL.getX() > pfxGetXMax(aabbB)) continue;
if(aabbMaxL.getY() < pfxGetYMin(aabbB) || aabbMinL.getY() > pfxGetYMax(aabbB)) continue;
if(aabbMaxL.getZ() < pfxGetZMin(aabbB) || aabbMinL.getZ() > pfxGetZMax(aabbB)) continue;
PfxTriMesh *island = &lmeshA->m_islands[i];
// 衝突判定
PfxContactCache localContacts;
switch(shapeB.getType()) {
case kPfxShapeSphere:
pfxContactTriMeshSphere(localContacts,island,transformA,shapeB.getSphere(),transformB,distanceThreshold);
break;
case kPfxShapeCapsule:
pfxContactTriMeshCapsule(localContacts,island,transformA,shapeB.getCapsule(),transformB,distanceThreshold);
break;
case kPfxShapeBox:
pfxContactTriMeshBox(localContacts,island,transformA,shapeB.getBox(),transformB,distanceThreshold);
break;
case kPfxShapeCylinder:
pfxContactTriMeshCylinder(localContacts,island,transformA,shapeB.getCylinder(),transformB,distanceThreshold);
break;
case kPfxShapeConvexMesh:
pfxContactTriMeshConvex(localContacts,island,transformA,*shapeB.getConvexMesh(),transformB,distanceThreshold);
break;
default:
break;
}
// 衝突点を追加
for(int j=0;j<localContacts.getNumContacts();j++) {
PfxSubData subData = localContacts.getSubData(j);
subData.setIslandId(i);
contacts.addContactPoint(
localContacts.getDistance(j),
localContacts.getNormal(j),
localContacts.getLocalPointA(j),
localContacts.getLocalPointB(j),
subData);
}
}
}
return contacts.getNumContacts();
}
PfxInt32 pfxContactTriMeshSphere(
PfxContactCache &contacts,
const PfxTriMesh *meshA,
const PfxTransform3 &transformA,
const PfxSphere &sphereB,
const PfxTransform3 &transformB,
PfxFloat distanceThreshold)
{
(void) distanceThreshold;
PfxTransform3 transformAB,transformBA;
PfxMatrix3 matrixBA;
PfxVector3 offsetBA;
// Bローカル→Aローカルへの変換
transformAB = orthoInverse(transformA) * transformB;
// Aローカル→Bローカルへの変換
transformBA = orthoInverse(transformAB);
matrixBA = transformBA.getUpper3x3();
offsetBA = transformBA.getTranslation();
//-------------------------------------------
// 判定する面を絞り込む
PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = {0};
PfxUInt32 numSelFacets = 0;
PfxVector3 aabbB(sphereB.m_radius);
numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
if(numSelFacets == 0) {
return 0;
}
//-----------------------------------------------
// 判定
PfxContactCache localContacts;
// TriangleMeshの面->sphereの判定
// ※TriangleMesh座標系
{
for(PfxUInt32 f = 0; f < numSelFacets; f++ ) {
const PfxFacet &facet = meshA->m_facets[selFacets[f]];
const PfxVector3 facetNormal = pfxReadVector3(facet.m_normal);
const PfxVector3 facetPnts[3] = {
meshA->m_verts[facet.m_vertIds[0]],
meshA->m_verts[facet.m_vertIds[1]],
meshA->m_verts[facet.m_vertIds[2]],
};
const PfxEdge *edge[3] = {
&meshA->m_edges[facet.m_edgeIds[0]],
&meshA->m_edges[facet.m_edgeIds[1]],
&meshA->m_edges[facet.m_edgeIds[2]],
};
PfxVector3 sepAxis,pntA,pntB;
PfxUInt32 edgeChk =
((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |
((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |
((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);
pfxContactTriangleSphere(localContacts,selFacets[f],
facetNormal,facetPnts[0],facetPnts[1],facetPnts[2],
facet.m_thickness,
0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),
0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),
0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),
edgeChk,
sphereB.m_radius,transformAB.getTranslation());
}
}
for(int i=0;i<localContacts.getNumContacts();i++) {
PfxSubData subData = localContacts.getSubData(i);
const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
PfxTriangle triangleA(
meshA->m_verts[facet.m_vertIds[0]],
meshA->m_verts[facet.m_vertIds[1]],
meshA->m_verts[facet.m_vertIds[2]]);
PfxFloat s=0.0f,t=0.0f;
pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);
subData.m_type = PfxSubData::MESH_INFO;
subData.setFacetLocalS(s);
subData.setFacetLocalT(t);
contacts.addContactPoint(
localContacts.getDistance(i),
transformA.getUpper3x3() * localContacts.getNormal(i),
localContacts.getLocalPointA(i),
transformBA * localContacts.getLocalPointB(i),
subData);
}
return contacts.getNumContacts();
}
PfxInt32 pfxDetectCollision(PfxDetectCollisionParam ¶m)
{
PfxInt32 ret = pfxCheckParamOfDetectCollision(param);
if(ret != SCE_PFX_OK)
return ret;
SCE_PFX_PUSH_MARKER("pfxDetectCollision");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxCollidable *offsetCollidables = param.offsetCollidables;
PfxUInt32 numRigidBodies = param.numRigidBodies;
for(PfxUInt32 i=0;i<numContactPairs;i++) {
const PfxBroadphasePair &pair = contactPairs[i];
if(!pfxCheckCollidableInCollision(pair)) {
continue;
}
PfxUInt32 iContact = pfxGetContactId(pair);
PfxUInt32 iA = pfxGetObjectIdA(pair);
PfxUInt32 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[iContact];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxCollidable &collA = offsetCollidables[iA];
PfxCollidable &collB = offsetCollidables[iB];
PfxTransform3 tA0(stateA.getOrientation(), stateA.getPosition());
PfxTransform3 tB0(stateB.getOrientation(), stateB.getPosition());
PfxContactCache contactCache;
PfxShapeIterator itrShapeA(collA);
for(PfxUInt32 j=0;j<collA.getNumShapes();j++,++itrShapeA) {
const PfxShape &shapeA = *itrShapeA;
PfxTransform3 offsetTrA = shapeA.getOffsetTransform();
PfxTransform3 worldTrA = tA0 * offsetTrA;
PfxShapeIterator itrShapeB(collB);
for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB) {
const PfxShape &shapeB = *itrShapeB;
PfxTransform3 offsetTrB = shapeB.getOffsetTransform();
PfxTransform3 worldTrB = tB0 * offsetTrB;
if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) &&
(shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) ) {
pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(
contactCache,
shapeA,offsetTrA,worldTrA,j,
shapeB,offsetTrB,worldTrB,k,
SCE_PFX_CONTACT_THRESHOLD);
}
}
}
for(int j=0;j<contactCache.getNumContacts();j++) {
const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);
contact.addContactPoint(
cp.m_distance,
cp.m_normal,
cp.m_localPointA,
cp.m_localPointB,
cp.m_subData
);
}
}
SCE_PFX_POP_MARKER();
(void) numRigidBodies;
return SCE_PFX_OK;
}