/// <summary>
/// 6軸可動制限を作る
/// </summary>
/// <param name="jointParam">6軸ジョイントパラメータ</param>
/// <returns>6軸可動制限</returns>
private Joint6Restriction CreateRestriction(Spring6DofJointParam jointParam)
{
var movementRestriction = new Joint6MovementRestriction(jointParam.MoveLimitationMin, jointParam.MoveLimitationMax);
var rotationRestriction = new Joint6RotationRestriction(jointParam.RotationLimitationMin, jointParam.RotationLimitationMax);
return(new Joint6Restriction(movementRestriction, rotationRestriction));
}
/// <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));
}
private Joint6Stiffness CreateStiffness(Spring6DofJointParam jointParam)
{
return(new Joint6Stiffness(jointParam.SpringMoveCoefficient, jointParam.SpringRotationCoefficient));
}
