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