コード例 #1
0
void pfxSetupSwingTwistJoint(
	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 angAB = stateA.getAngularVelocity() - stateB.getAngularVelocity();

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

	PfxVector3 axis[3];
	PfxFloat angle[3];
	pfxCalcJointAngleSwingTwist(worldFrameA,worldFrameB,angle,axis);

	// Angular Constraint
	for(int c=3;c<6;c++) {
		PfxJointConstraint &jointConstraint = joint.m_constraints[c];
		PfxConstraintRow &constraint = jointConstraint.m_constraintRow;

		PfxVector3 normal = axis[c-3];

		PfxFloat posErr = angle[c-3];
		PfxFloat lowerLimit = -jointConstraint.m_maxImpulse;
		PfxFloat upperLimit =  jointConstraint.m_maxImpulse;
		PfxFloat velocityAmp = 1.0f;
		
		pfxCalcAngularLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit);

		PfxFloat denom = dot((solverBodyA.m_inertiaInv+solverBodyB.m_inertiaInv)*normal,normal);
		
		constraint.m_rhs = -velocityAmp*dot(angAB,normal); // velocity error
		constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep; // position error
		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);
	}
}