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