/// <summary>
/// 6軸ジョイントに繋がる剛体のペアを作る
/// </summary>
/// <param name="jointParam">6軸ジョイントパラメータ</param>
/// <returns>6軸ジョイントに繋がる剛体のペア</returns>
private Joint6ConnectedBodyPair CreateConnectedBodyPair(Spring6DofJointParam jointParam)
{
var bodyA = rigidBodies[jointParam.RigidBodyAIndex];
var bodyAworld_inv = Matrix.Invert(bulletManager.GetWorld(bodyA));
var bodyB = rigidBodies[jointParam.RigidBodyBIndex];
var bodyBworld_inv = Matrix.Invert(bulletManager.GetWorld(bodyB));
var jointRotation = jointParam.Rotation;
var jointPosition = jointParam.Position;
var jointWorld = Matrix.RotationYawPitchRoll(jointRotation.Y, jointRotation.X, jointRotation.Z) * Matrix.Translation(jointPosition.X, jointPosition.Y, jointPosition.Z);
var connectedBodyA = new Joint6ConnectedBody(bodyA, jointWorld * bodyAworld_inv);
var connectedBodyB = new Joint6ConnectedBody(bodyB, jointWorld * bodyBworld_inv);
return(new Joint6ConnectedBodyPair(connectedBodyA, connectedBodyB));
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="connectedBodyA">Rigid A lead to a six-axis joint</param>
/// <param name="connectedBodyB">Rigid body B leads to 6-axis joint</param>
public Joint6ConnectedBodyPair(Joint6ConnectedBody connectedBodyA, Joint6ConnectedBody connectedBodyB)
{
this.connectedBodyA = connectedBodyA;
this.connectedBodyB = connectedBodyB;
}
