/// <summary>
/// Between rigid and rigid six-axis spring add a constraint
/// </summary>
/// <param name="connectedBodyPair">A pair of rigid links</param>
/// <param name="restriction">6-axis motion limits</param>
/// <param name="stiffness">6-axis spring.</param>
public void Add6DofSpringConstraint(Joint6ConnectedBodyPair connectedBodyPair, Joint6Restriction restriction, Joint6Stiffness stiffness)
{
var bodyA = connectedBodyPair.connectedBodyA.rigidBody;
var bodyB = connectedBodyPair.connectedBodyB.rigidBody;
var frameInA = connectedBodyPair.connectedBodyA.world;
var frameInB = connectedBodyPair.connectedBodyB.world;
var constraint = new Generic6DofSpringConstraint(bodyA, bodyB, frameInA, frameInB, true); // 第五引数の効果は謎。どちらでも同じ様に見える……。
var c_p1 = restriction.movementRestriction.c_p1;
var c_p2 = restriction.movementRestriction.c_p2;
var c_r1 = restriction.rotationRestriction.c_r1;
var c_r2 = restriction.rotationRestriction.c_r2;
constraint.LinearLowerLimit = new Vector3(c_p1.X, c_p1.Y, c_p1.Z); // 型はベクトルだがベクトル量ではないのでZは反転しない。
constraint.LinearUpperLimit = new Vector3(c_p2.X, c_p2.Y, c_p2.Z);
constraint.AngularLowerLimit = new Vector3(c_r1.X, c_r1.Y, c_r1.Z);
constraint.AngularUpperLimit = new Vector3(c_r2.X, c_r2.Y, c_r2.Z);
SetStiffness(stiffness.translation.X, 0, constraint);
SetStiffness(stiffness.translation.Y, 1, constraint);
SetStiffness(stiffness.translation.Z, 2, constraint);
SetStiffness(stiffness.rotation.X, 3, constraint);
SetStiffness(stiffness.rotation.Y, 4, constraint);
SetStiffness(stiffness.rotation.Z, 5, constraint);
this.dynamicsWorld.AddConstraint(constraint);
}
/// <summary>
/// 剛体と剛体の間に6軸バネ拘束を追加
/// </summary>
/// <param name="connectedBodyPair">繋ぐ剛体のペア</param>
/// <param name="restriction">6軸可動制限</param>
/// <param name="stiffness">6軸バネ</param>
public void Add6DofSpringConstraint(Joint6ConnectedBodyPair connectedBodyPair, Joint6Restriction restriction, Joint6Stiffness stiffness)
{
constraintFactory.Add6DofSpringConstraint(connectedBodyPair, restriction, stiffness);
}
