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