static SCE_PFX_FORCE_INLINE
bool pfxContactTriangleConvex(PfxContactCache &contacts,PfxUInt32 facetId,
const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
PfxUInt32 edgeChk,
const PfxConvexMesh &convex)
{
PfxVector3 facetPnts[6] = {
p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
};
PfxPoint3 pA(0.0f),pB(0.0f);
PfxVector3 nml(0.0f);
PfxGjkSolver gjk;
gjk.setup((void*)facetPnts,(void*)&convex,pfxGetSupportVertexTriangleWithThickness,pfxGetSupportVertexConvex);
PfxFloat d = gjk.collide(nml,pA,pB,PfxTransform3::identity(),PfxTransform3::identity(),SCE_PFX_FLT_MAX);
if(d >= 0.0f) return false;
PfxVector3 pointsOnTriangle = PfxVector3(pA);
PfxVector3 pointsOnConvex = PfxVector3(pB);
PfxVector3 axis = nml;
// 面上の最近接点が凸エッジ上でない場合は法線を変える
if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
axis=-normal;
}
PfxSubData subData;
subData.setFacetId(facetId);
contacts.addContactPoint(-length(pointsOnTriangle-pointsOnConvex),axis,pA,pB,subData);
return true;
}
static SCE_PFX_FORCE_INLINE
bool pfxContactTriangleSphere(PfxContactCache &contacts,PfxUInt32 facetId,
const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,
const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,
PfxUInt32 edgeChk,
PfxFloat sphereRadius,const PfxVector3 &spherePos)
{
PfxVector3 facetPnts[3] = {
p0,p1,p2,
};
// 早期判定
{
PfxPlane planeA(normal,p0);
PfxFloat len1 = planeA.onPlane(spherePos);
if(len1 >= sphereRadius || len1 < -thickness-sphereRadius) return false;
}
// 球と面の最近接点を計算
{
PfxTriangle triangleA(p0,p1,p2);
PfxVector3 pntA;
// pfxClosestPointTriangle(spherePos,triangleA,pntA);
bool insideTriangle = false;
while(1) {
PfxVector3 ab = p1 - p0;
PfxVector3 ac = p2 - p0;
PfxVector3 ap = spherePos - p0;
PfxFloat d1 = dot(ab, ap);
PfxFloat d2 = dot(ac, ap);
if(d1 <= 0.0f && d2 <= 0.0f) {
pntA = p0;
break;
}
PfxVector3 bp = spherePos - p1;
PfxFloat d3 = dot(ab, bp);
PfxFloat d4 = dot(ac, bp);
if (d3 >= 0.0f && d4 <= d3) {
pntA = p1;
break;
}
PfxFloat vc = d1*d4 - d3*d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {
PfxFloat v = d1 / (d1 - d3);
pntA = p0 + v * ab;
break;
}
PfxVector3 cp = spherePos - p2;
PfxFloat d5 = dot(ab, cp);
PfxFloat d6 = dot(ac, cp);
if (d6 >= 0.0f && d5 <= d6) {
pntA = p2;
break;
}
PfxFloat vb = d5*d2 - d1*d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {
PfxFloat w = d2 / (d2 - d6);
pntA = p0 + w * ac;
break;
}
PfxFloat va = d3*d6 - d5*d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {
PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
pntA = p1 + w * (p2 - p1);
break;
}
PfxFloat den = 1.0f / (va + vb + vc);
PfxFloat v = vb * den;
PfxFloat w = vc * den;
pntA = p0 + ab * v + ac * w;
insideTriangle = true;
break;
}
PfxVector3 distVec = pntA - spherePos;
PfxFloat l = length(distVec);
if(!insideTriangle && l >= sphereRadius) return false;
// 分離軸
PfxVector3 sepAxis = (l < 0.00001f || insideTriangle) ? -normal : distVec / l;
// 球上の衝突点
PfxVector3 pointsOnSphere = spherePos + sphereRadius * sepAxis;
PfxVector3 pointsOnTriangle = pntA;
// 面上の最近接点が凸エッジ上でない場合は法線を変える
if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||
((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||
((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {
sepAxis=-normal;
}
PfxSubData subData;
subData.setFacetId(facetId);
contacts.addContactPoint(-length(pointsOnSphere-pointsOnTriangle),sepAxis,PfxPoint3(pointsOnTriangle),PfxPoint3(pointsOnSphere),subData);
}
return true;
}
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 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 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;
}
