void pfxSetupBallJoint(
PfxJoint &joint,
const PfxRigidState &stateA,
const PfxRigidState &stateB,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB,
PfxFloat timeStep
)
{
PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
PfxVector3 vAB = vA-vB;
PfxVector3 distance = (stateA.getPosition() + rA) - (stateB.getPosition() + rB);
PfxMatrix3 worldFrameA,worldFrameB;
worldFrameA = PfxMatrix3(solverBodyA.m_orientation) * joint.m_frameA;
worldFrameB = PfxMatrix3(solverBodyB.m_orientation) * joint.m_frameB;
// Linear Constraint
PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(solverBodyA.m_massInv + solverBodyB.m_massInv)) -
crossMatrix(rA) * solverBodyA.m_inertiaInv * crossMatrix(rA) -
crossMatrix(rB) * solverBodyB.m_inertiaInv * crossMatrix(rB);
for(int c=0;c<3;c++) {
PfxJointConstraint &jointConstraint = joint.m_constraints[c];
PfxConstraintRow &constraint = jointConstraint.m_constraintRow;
PfxVector3 normal = worldFrameA[c];
PfxFloat posErr = dot(distance,-normal);
PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
PfxFloat upperLimit = jointConstraint.m_maxImpulse;
PfxFloat velocityAmp = 1.0f;
pfxCalcLinearLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);
PfxFloat denom = dot(K*normal,normal);
constraint.m_rhs = -velocityAmp*dot(vAB,normal);
constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep;
constraint.m_rhs *= jointConstraint.m_weight/denom;
constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom;
constraint.m_lowerLimit = lowerLimit;
constraint.m_upperLimit = upperLimit;
pfxStoreVector3(normal,constraint.m_normal);
}
}
static
void createIsland(PfxTriMesh &island,const PfxArray<PfxMcFacetPtr> &facets)
{
if(facets.empty()) return;
island.m_numFacets = facets.size();
PfxUInt32 vertsFlag[(0xff*SCE_PFX_NUMMESHFACETS*3+31)/32];
memset(vertsFlag,0,sizeof(PfxUInt32)*((0xff*SCE_PFX_NUMMESHFACETS*3+31)/32));
PfxArray<PfxMcEdgeEntry*> edgeHead(facets.size()*3);
PfxArray<PfxMcEdgeEntry> edgeList(facets.size()*3);
PfxMcEdgeEntry* nl = NULL;
edgeHead.assign(facets.size()*3,nl);
edgeList.assign(facets.size()*3,PfxMcEdgeEntry());
int vcnt = 0;
int ecnt = 0;
for(PfxUInt32 f=0;f<facets.size();f++) {
PfxMcFacet &iFacet = *facets[f];
PfxMcEdge *iEdge[3] = {
iFacet.e[0],
iFacet.e[1],
iFacet.e[2],
};
PfxFacet &oFacet = island.m_facets[f];
oFacet.m_half[0] = oFacet.m_half[1] = oFacet.m_half[2] = 0.0f;
oFacet.m_center[0] = oFacet.m_center[1] = oFacet.m_center[2] = 0.0f;
pfxStoreVector3(iFacet.n,oFacet.m_normal);
oFacet.m_thickness = iFacet.thickness;
// Vertex
for(int v=0;v<3;v++) {
PfxMcVert *vert = facets[f]->v[v];
PfxUInt32 idx = vert->i;
PfxUInt32 mask = 1 << (idx & 31);
if((vertsFlag[idx>>5] & mask) == 0) {
SCE_PFX_ASSERT(vcnt<SCE_PFX_NUMMESHVERTICES);
vertsFlag[idx>>5] |= mask;
island.m_verts[vcnt] = vert->coord;
vert->flag = vcnt;// 新しいインデックス
vcnt++;
}
oFacet.m_vertIds[v] = (PfxUInt8)vert->flag;
}
// Edge
for(int v=0;v<3;v++) {
PfxUInt8 viMin = SCE_PFX_MIN(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
PfxUInt8 viMax = SCE_PFX_MAX(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
int key = ((0x8da6b343*viMin+0xd8163841*viMax)%(island.m_numFacets*3));
for(PfxMcEdgeEntry *e=edgeHead[key];;e=e->next) {
if(!e) {
edgeList[ecnt].vertId[0] = viMin;
edgeList[ecnt].vertId[1] = viMax;
edgeList[ecnt].facetId[0] = f;
edgeList[ecnt].numFacets = 1;
edgeList[ecnt].edgeNum[0] = v;
edgeList[ecnt].edgeId = ecnt;
edgeList[ecnt].dir = normalize(island.m_verts[viMax]-island.m_verts[viMin]);
edgeList[ecnt].next = edgeHead[key];
edgeHead[key] = &edgeList[ecnt];
PfxEdge edge;
edge.m_angleType = iEdge[v]->angleType;
// 厚み角の設定 0～πを0～255の整数値に変換して格納
edge.m_tilt = (PfxUInt8)((iEdge[v]->angle/(0.5f*SCE_PFX_PI))*255.0f);
edge.m_vertId[0] = viMin;
edge.m_vertId[1] = viMax;
oFacet.m_edgeIds[v] = ecnt;
island.m_edges[ecnt] = edge;
SCE_PFX_ASSERT(ecnt <= SCE_PFX_NUMMESHEDGES);
ecnt++;
break;
}
if(e->vertId[0] == viMin && e->vertId[1] == viMax) {
SCE_PFX_ASSERT(e->numFacets==1);
e->facetId[1] = f;
e->edgeNum[1] = v;
e->numFacets = 2;
oFacet.m_edgeIds[v] = e->edgeId;
break;
}
}
}
}
island.m_numEdges = ecnt;
island.m_numVerts = vcnt;
island.updateAABB();
}
void pfxSetupContactConstraint(
PfxConstraintRow &constraintResponse,
PfxConstraintRow &constraintFriction1,
PfxConstraintRow &constraintFriction2,
PfxFloat penetrationDepth,
PfxFloat restitution,
PfxFloat friction,
const PfxVector3 &contactNormal,
const PfxVector3 &contactPointA,
const PfxVector3 &contactPointB,
const PfxRigidState &stateA,
const PfxRigidState &stateB,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB,
PfxFloat separateBias,
PfxFloat timeStep
)
{
(void)friction;
PfxVector3 rA = rotate(solverBodyA.m_orientation,contactPointA);
PfxVector3 rB = rotate(solverBodyB.m_orientation,contactPointB);
PfxFloat massInvA = solverBodyA.m_massInv;
PfxFloat massInvB = solverBodyB.m_massInv;
PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {
massInvB = 0.0f;
inertiaInvB = PfxMatrix3(0.0f);
}
if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {
massInvA = 0.0f;
inertiaInvA = PfxMatrix3(0.0f);
}
PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(massInvA + massInvB)) -
crossMatrix(rA) * inertiaInvA * crossMatrix(rA) -
crossMatrix(rB) * inertiaInvB * crossMatrix(rB);
PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
PfxVector3 vAB = vA-vB;
PfxVector3 tangent1,tangent2;
pfxGetPlaneSpace(contactNormal,tangent1,tangent2);
// Contact Constraint
{
PfxVector3 normal = contactNormal;
PfxFloat denom = dot(K*normal,normal);
constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error
constraintResponse.m_rhs -= (separateBias * SCE_PFX_MIN(0.0f,penetrationDepth+SCE_PFX_CONTACT_SLOP)) / timeStep; // position error
constraintResponse.m_rhs /= denom;
constraintResponse.m_jacDiagInv = 1.0f/denom;
constraintResponse.m_lowerLimit = 0.0f;
constraintResponse.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintResponse.m_normal);
}
// Friction Constraint 1
{
PfxVector3 normal = tangent1;
PfxFloat denom = dot(K*normal,normal);
constraintFriction1.m_jacDiagInv = 1.0f/denom;
constraintFriction1.m_rhs = -dot(vAB,normal);
constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;
constraintFriction1.m_lowerLimit = 0.0f;
constraintFriction1.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintFriction1.m_normal);
}
// Friction Constraint 2
{
PfxVector3 normal = tangent2;
PfxFloat denom = dot(K*normal,normal);
constraintFriction2.m_jacDiagInv = 1.0f/denom;
constraintFriction2.m_rhs = -dot(vAB,normal);
constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;
constraintFriction2.m_lowerLimit = 0.0f;
constraintFriction2.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintFriction2.m_normal);
}
}
