public static void Start()
{
RuntimeObject.Init();
RigidBody.Init();
CollisionBody.Init();
CollisionSensor.Init();
TransportSurface.Init();
CollisionMaterial.Init();
HingeJoint.Init();
SlidingJoint.Init();
CylindricalJoint.Init();
FixedJoint.Init();
BallJoint.Init();
AngularLimit.Init();
LinearLimit.Init();
AngularSpring.Init();
LinearSpring.Init();
SpeedControl.Init();
PositionControl.Init();
BreakingConstraint.Init();
GearCoupling.Init();
CamCoupling.Init();
ElecCamCoupling.Init();
PreventCollision.Init();
ChangeMaterial.Init();
ComponentPart.Init();
SourceBehavior.Init();
SinkBehavior.Init();
GraphControl.Init();
ExternalConnection.Init();
SignalAdapter.Init();
Signal.Init();
ProxyObject.Init();
RuntimeParameters.Init();
}
private static void AddJoint(Ragdoll ragdoll, Skeleton skeleton, AvatarBone parentBone, AvatarBone childBone, float errorReduction, float softness)
{
int parentIndex = (int)parentBone;
int childIndex = (int)childBone;
// To define AnchorPositionALocal/AnchorPositionBLocal:
// To get the AnchorPositionALocal we apply jointPosesAbsolute[indexA].Inverse to
// convert the joint pose from model space into the joints space of parentBone. Then we apply
// ragdoll.BodyOffsets[boneAIndex].Inverse to convert from joint space to body space. The result is
// the joint position of B in body space of A.
// To get AnchorPositionBLocal, we only have to apply the inverse offset.
BallJoint joint = new BallJoint
{
BodyA = ragdoll.Bodies[parentIndex],
BodyB = ragdoll.Bodies[childIndex],
CollisionEnabled = false,
AnchorPositionALocal = (ragdoll.BodyOffsets[parentIndex].Inverse * skeleton.GetBindPoseAbsoluteInverse(parentIndex) * skeleton.GetBindPoseAbsoluteInverse(childIndex).Inverse).Translation,
AnchorPositionBLocal = ragdoll.BodyOffsets[childIndex].Inverse.Position,
ErrorReduction = errorReduction,
Softness = softness,
};
ragdoll.Joints[childIndex] = joint;
}
/// <summary>
/// Adds a BallJoint between the specified bones.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="ragdoll">The ragdoll.</param>
/// <param name="parent">The parent.</param>
/// <param name="child">The child.</param>
private static void AddJoint(SkeletonPose skeletonPose, Ragdoll ragdoll, int parent, int child)
{
// Get bodies and offsets for the bones.
var skeleton = skeletonPose.Skeleton;
var childBody = ragdoll.Bodies[child];
var childOffset = ragdoll.BodyOffsets[child];
var parentBody = ragdoll.Bodies[parent];
var parentOffset = ragdoll.BodyOffsets[parent];
// Get bind poses of the bones in model space.
var parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(parent).Inverse;
var childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(child).Inverse;
// The child pose relative to the parent bone.
var bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute;
// Add BallJoint that connects the two bones. The position of the joint is the
// origin of the child bone.
BallJoint joint = new BallJoint
{
BodyA = parentBody,
BodyB = childBody,
CollisionEnabled = false,
AnchorPositionALocal = (parentOffset.Inverse * bindPoseRelative).Position,
AnchorPositionBLocal = childOffset.Inverse.Position,
ErrorReduction = 0.2f,
Softness = 0.0001f,
};
ragdoll.Joints.Add(joint);
}
/// <summary>
/// Visualizes the <see cref="AngularLimit"/> of a <see cref="BallJoint"/>.
/// </summary>
/// <param name="debugRenderer">The debug renderer.</param>
/// <param name="joint">The joint.</param>
/// <param name="limit">The limit.</param>
/// <param name="scale">
/// A scale factor that determines the size of the drawn elements.
/// </param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="debugRenderer" />, <paramref name="joint" />, or <paramref name="limit" />
/// is <see langword="null" />.
/// </exception>
public static void DrawAngularLimit(this DebugRenderer debugRenderer, BallJoint joint, AngularLimit limit, float scale, bool drawOverScene)
{
if (debugRenderer == null)
{
throw new ArgumentNullException("debugRenderer");
}
if (joint == null)
{
throw new ArgumentNullException("joint");
}
if (limit == null)
{
throw new ArgumentNullException("limit");
}
Vector3 jointPosition = joint.BodyA.Pose.ToWorldPosition(joint.AnchorPositionALocal);
// A transformation that converts from constraint anchor space to world space.
Pose constraintToWorld = limit.BodyA.Pose * new Pose(limit.AnchorOrientationALocal);
// Draw an arc for each rotation axis.
DrawArc(debugRenderer, constraintToWorld, jointPosition, Vector3.UnitX, Vector3.UnitY, limit.Minimum.X, limit.Maximum.X, scale, Color.Red, drawOverScene);
DrawArc(debugRenderer, constraintToWorld, jointPosition, Vector3.UnitY, Vector3.UnitX, limit.Minimum.Y, limit.Maximum.Y, scale, Color.Green, drawOverScene);
DrawArc(debugRenderer, constraintToWorld, jointPosition, Vector3.UnitZ, Vector3.UnitX, limit.Minimum.Z, limit.Maximum.Z, scale, Color.Blue, drawOverScene);
}
/// <summary>
/// Visualizes the <see cref="TwistSwingLimit"/> of a <see cref="BallJoint"/>.
/// </summary>
/// <param name="debugRenderer">The debug renderer.</param>
/// <param name="joint">The joint.</param>
/// <param name="limit">The limit.</param>
/// <param name="scale">
/// A scale factor that determines the size of the drawn elements.
/// </param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <exception cref="ArgumentNullException">
/// <paramref name="debugRenderer" />, <paramref name="joint" />, or <paramref name="limit" />
/// is <see langword="null" />.
/// </exception>
public static void DrawTwistSwingLimit(this DebugRenderer debugRenderer, BallJoint joint, TwistSwingLimit limit, float scale, bool drawOverScene)
{
if (debugRenderer == null)
{
throw new ArgumentNullException("debugRenderer");
}
if (joint == null)
{
throw new ArgumentNullException("joint");
}
if (limit == null)
{
throw new ArgumentNullException("limit");
}
// ----- Draw swing cone.
// The tip of the swing cone:
Vector3 coneTip = joint.BodyA.Pose.ToWorldPosition(joint.AnchorPositionALocal);
// The first point on the swing cone:
var previousConePoint = limit.GetPointOnCone(0, coneTip, scale);
// Draw swing cone.
const int numberOfSegments = 24;
const float segmentAngle = ConstantsF.TwoPi / numberOfSegments;
Color color = Color.Violet;
for (int i = 0; i < numberOfSegments; i++)
{
var conePoint = limit.GetPointOnCone((i + 1) * segmentAngle, coneTip, scale);
// Line from cone tip to cone base.
debugRenderer.DrawLine(coneTip, conePoint, color, drawOverScene);
// Line on the cone base.
debugRenderer.DrawLine(previousConePoint, conePoint, color, drawOverScene);
previousConePoint = conePoint;
}
// ----- Draw twist axis.
// The x-axis is the twist direction.
Vector3 twistAxis = Vector3.UnitX;
// The twist axis relative to body B.
Vector3 twistAxisDirectionBLocal = limit.AnchorOrientationBLocal * twistAxis;
// The twist axis relative to world space.
Vector3 twistAxisDirection = limit.BodyB.Pose.ToWorldDirection(twistAxisDirectionBLocal);
// (A similar computation is used in DrawArc() below.)
// Line in twist direction.
debugRenderer.DrawLine(coneTip, coneTip + twistAxisDirection * scale, Color.Red, drawOverScene);
// A transformation that converts from constraint anchor space to world space.
Pose constraintToWorld = limit.BodyA.Pose * new Pose(limit.AnchorOrientationALocal);
// Draw an arc that visualizes the twist limits.
DrawArc(debugRenderer, constraintToWorld, coneTip, Vector3.UnitX, Vector3.UnitY, limit.Minimum.X, limit.Maximum.X, scale, Color.Red, drawOverScene);
}
/// <summary>
/// Removes all dynamic rigid bodies from the simulation.
/// </summary>
private void ClearScene()
{
// Remove all rigid bodies, except static bodies.
var staticBodies = Simulation.RigidBodies
.Where(body => body.MotionType == MotionType.Static)
.ToArray();
Simulation.RigidBodies.Clear();
Simulation.RigidBodies.AddRange(staticBodies);
Simulation.Constraints.Clear();
_spring = null;
}
/// <summary>
/// Visualizes the constraints of the ragdoll (for debugging).
/// </summary>
/// <param name="debugRenderer">The debug renderer.</param>
/// <param name="ragdoll">The ragdoll.</param>
/// <param name="scale">
/// A scale factor that determines the size of the drawn elements.
/// </param>
/// <param name="drawOverScene">
/// If set to <see langword="true"/> the object is drawn over the graphics scene (depth-test
/// disabled).
/// </param>
/// <remarks>
/// Currently, only <see cref="TwistSwingLimit" />s and <see cref="AngularLimit" />s are
/// supported.
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="debugRenderer" /> or <paramref name="ragdoll" /> is <see langword="null" />.
/// </exception>
public static void DrawConstraints(this DebugRenderer debugRenderer, Ragdoll ragdoll, float scale, bool drawOverScene)
{
if (debugRenderer == null)
{
throw new ArgumentNullException("debugRenderer");
}
if (ragdoll == null)
{
throw new ArgumentNullException("ragdoll");
}
// Render information for each limit.
foreach (Constraint limit in ragdoll.Limits)
{
// Get the ball joint constraint that connects the two bodies of the limit.
BallJoint joint = null;
foreach (Constraint constraint in ragdoll.Joints)
{
if (constraint.BodyA == limit.BodyA && constraint.BodyB == limit.BodyB ||
constraint.BodyA == limit.BodyB && constraint.BodyB == limit.BodyA)
{
joint = constraint as BallJoint;
break;
}
}
// Skip this limit if no joint was found.
if (joint == null)
{
continue;
}
TwistSwingLimit twistSwingLimit = limit as TwistSwingLimit;
if (twistSwingLimit != null)
{
DrawTwistSwingLimit(debugRenderer, joint, twistSwingLimit, scale, drawOverScene);
continue;
}
AngularLimit angularLimit = limit as AngularLimit;
if (angularLimit != null)
{
DrawAngularLimit(debugRenderer, joint, angularLimit, scale, drawOverScene);
continue;
}
}
}
//due to the menu system several other operations must take place prior
//to the begining of this, to ensure that all other options are compleate
//This is only called after a cerain number of frames.
void delayedSetup()
{
valueProduce = timeline.transform.GetChild(0).gameObject;
TLC = valueProduce.GetComponent <TimeLineControler>();
baseRot = transform.rotation.eulerAngles;
if (invertVal)
{
baseRot = -baseRot;
}
if (fudgeRot180)
{
fudge = 180;
}
BallJ = transform.parent.GetComponent <BallJoint>();
if (BallJ)
{
pairentisBall = true;
}
}
private void InitJoints()
{
baseJoint = new BallJoint()
{
Name = "base",
RotZ = .060f
};
var j1 = new BallJoint()
{
Name = "j1", RotZ = 0f
};
var j2 = new BallJoint()
{
Name = "j2", RotY = 0f
};
var j3 = new BallJoint()
{
Name = "j3", RotX = 0f
};
baseJoint.AddChild(j1);
j1.AddChild(j2);
j2.AddChild(j3);
}
public static SkeletalJoint_Base Create(CustomRigidJoint rigidJoint, CustomRigidGroup parent)
{
if (RotationalJoint.IsRotationalJoint(rigidJoint))
{
return(new RotationalJoint(parent, rigidJoint));
}
if (LinearJoint.IsLinearJoint(rigidJoint))
{
return(new LinearJoint(parent, rigidJoint));
}
if (CylindricalJoint.IsCylindricalJoint(rigidJoint))
{
return(new CylindricalJoint(parent, rigidJoint));
}
if (PlanarJoint.IsPlanarJoint(rigidJoint))
{
return(new PlanarJoint(parent, rigidJoint));
}
if (BallJoint.IsBallJoint(rigidJoint))
{
return(new BallJoint(parent, rigidJoint));
}
return(null);
}
internal DigitalRuneSphericalJoint(SphericalJointDescriptor descriptor)
{
WrappedSphericalJoint = new BallJoint();
#region set RigidBodies
if (!(descriptor.RigidBodyA is RigidBody))
{
throw new ArgumentException(String.Format("The type of the property 'RigidBodyA' must be '{0}'.", typeof(RigidBody)));
}
WrappedSphericalJoint.BodyA = ((RigidBody)descriptor.RigidBodyA).WrappedRigidBody;
_rigidBodyA = descriptor.RigidBodyA;
if (!(descriptor.RigidBodyB is RigidBody))
{
throw new ArgumentException("The type of the property 'RigidBodyB' must be 'System.Physics.DigitalRune.RigidBody'.");
}
WrappedSphericalJoint.BodyB = ((RigidBody)descriptor.RigidBodyB).WrappedRigidBody;
_rigidBodyB = descriptor.RigidBodyB;
#endregion
WrappedSphericalJoint.AnchorPositionALocal = descriptor.AnchorPositionALocal.ToDigitalRune();
WrappedSphericalJoint.AnchorPositionBLocal = descriptor.AnchorPositionBLocal.ToDigitalRune();
Descriptor = descriptor;
}
public ConstraintSample1(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// Tip: It is best to initialize bodies in a position where the constraints are
// satisfied - but even if they are initialized in other poses, the simulation
// will try to move them to the correct positions.
// ----- FixedJoint
// Create two boxes and connect them with a FixedJoint.
RigidBody box0 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3(-5, 3, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box0);
RigidBody box1 = new RigidBody(new BoxShape(1.2f, 1.2f, 1.2f))
{
Pose = new Pose(new Vector3(-5, 3 - 1.2f, 0))
};
Simulation.RigidBodies.Add(box1);
FixedJoint fixedJoint = new FixedJoint
{
BodyA = box0,
// The attachment point on the first box is at the bottom of the box.
AnchorPoseALocal = new Pose(new Vector3(0, -0.5f, 0)),
BodyB = box1,
// The attachment point on the second box is at the top of the box.
AnchorPoseBLocal = new Pose(new Vector3(0, 0.6f, 0)),
// Disable collision between the connected bodies.
CollisionEnabled = false,
};
Simulation.Constraints.Add(fixedJoint);
// ----- BallJoint
// Create two boxes and connect a corner of the second box with a ball-and-socked joint
// to the first box.
RigidBody box2 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3(-3, 3, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box2);
RigidBody box3 = new RigidBody(new BoxShape(1.2f, 1.2f, 1.2f))
{
Pose = new Pose(new Vector3(-3, 3, 0))
};
Simulation.RigidBodies.Add(box3);
BallJoint ballJoint = new BallJoint
{
BodyA = box2,
// The attachment point on the first box is at the box bottom.
AnchorPositionALocal = new Vector3(0, -0.5f, 0),
BodyB = box3,
// The attachment point on the second box is a corner of the box.
AnchorPositionBLocal = new Vector3(0.6f, 0.6f, 0.6f),
};
Simulation.Constraints.Add(ballJoint);
// ----- PrismaticJoint
// A prismatic joint is like a slider (without rotation around the slider axis).
RigidBody box4 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3(-1, 3, 0)),
};
Simulation.RigidBodies.Add(box4);
RigidBody box5 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3(-1, 3 - 0.5f, 0))
};
Simulation.RigidBodies.Add(box5);
PrismaticJoint prismaticJoint = new PrismaticJoint
{
BodyA = box4,
// The attachment point on the first box is in the center of the box.
// The slider joint allows linear movement along the first constraint axis.
// --> To define the constraint anchor orientation:
// The columns are the axes. We set the local -y axis in the first column. This is the
// slider axis. The other two columns are two orthonormal axes.
// (All three columns are orthonormal and form a valid rotation matrix.)
AnchorPoseALocal = new Pose(new Vector3(0, 0, 0), new Matrix(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
BodyB = box5,
// The attachment point on the second box is at the top of the box.
AnchorPoseBLocal = new Pose(new Vector3(0, 0.75f, 0), new Matrix(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
// The slider axis is -y. We limit the up movement, so that the anchor point on the second
// body can slide up to the anchor point on the first body, but not higher.
Minimum = 0,
// The second body can slide down until the anchor points have a max. distance of 0.5.
Maximum = 0.5f,
};
Simulation.Constraints.Add(prismaticJoint);
// ----- CylindricalJoint
// A cylindrical joint is a slider that allows rotation around the slider axis.
RigidBody box6 = new RigidBody(new BoxShape(1, 1, 1))
{
Pose = new Pose(new Vector3(1, 3, 0)),
};
Simulation.RigidBodies.Add(box6);
RigidBody box7 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3(1, 3 - 0.5f, 0))
};
Simulation.RigidBodies.Add(box7);
CylindricalJoint cylindricalJoint = new CylindricalJoint
{
BodyA = box6,
AnchorPoseALocal = new Pose(new Vector3(0, 0, 0), new Matrix(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
BodyB = box7,
AnchorPoseBLocal = new Pose(new Vector3(0, 0.75f, 0), new Matrix(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
// The linear movement limits on the slider axis.
LinearMinimum = 0,
LinearMaximum = 0.5f,
// The rotation limits around the slider axis (in radians). Here, we allow free
// rotations.
AngularMinimum = float.NegativeInfinity,
AngularMaximum = float.PositiveInfinity,
};
Simulation.Constraints.Add(cylindricalJoint);
// ----- HingeJoint
// Hinge joints allow rotations around one axis. They can be used to model swinging doors
// or rotating wheels.
RigidBody cylinder0 = new RigidBody(new CylinderShape(0.1f, 2f))
{
Pose = new Pose(new Vector3(3, 1, 0)),
MotionType = MotionType.Static
};
Simulation.RigidBodies.Add(cylinder0);
RigidBody box8 = new RigidBody(new BoxShape(1f, 1.8f, 0.1f))
{
Pose = new Pose(new Vector3(3 + 0.5f, 1, 0))
};
Simulation.RigidBodies.Add(box8);
HingeJoint hingeJoint = new HingeJoint
{
BodyA = cylinder0,
AnchorPoseALocal = new Pose(new Vector3(0, 0, 0), new Matrix(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
BodyB = box8,
AnchorPoseBLocal = new Pose(new Vector3(-0.5f, 0, 0), new Matrix(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
// The rotation limits around the hinge axis (in radians).
Minimum = -ConstantsF.PiOver2,
Maximum = ConstantsF.PiOver2,
};
Simulation.Constraints.Add(hingeJoint);
}
private void InitializeMarionetteConstraints()
{
// Create constraints that pull important body parts to Kinect joint positions.
// The Update() method below will update the BallJoint.AnchorPositionALocal vectors.
// Limit the maximal forces that these joints can apply. We do not want these joints to be
// so strong that they can violate the ragdoll joint and limit constraints. Increasing
// this force makes the ragdoll more responsive but can also violate ragdoll constraints
// (e.g. by stretching the limbs).
const float maxForce = 1000;
var ragdollSkeleton = _meshNode.SkeletonPose.Skeleton;
_headSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("Head")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_headSpring);
_elbowLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Forearm")],
MaxForce = maxForce / 2, // Elbow springs are weaker because the correct
// hand position is more important and the hand
// constraint should therefore be stronger.
};
// This constraint should be attached at the elbow position and not at the center of the forearm:
var elbowLeftJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("L_Forearm")).Translation;
_elbowLeftSpring.AnchorPositionBLocal = _elbowLeftSpring.BodyB.Pose.ToLocalPosition(elbowLeftJointPosition);
Simulation.Constraints.Add(_elbowLeftSpring);
_handLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Hand")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_handLeftSpring);
_elbowRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Forearm")],
MaxForce = maxForce / 2,
};
// This constraint should be attached at the elbow position and not at the center of the forearm:
var elbowRightJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("R_Forearm")).Translation;
_elbowRightSpring.AnchorPositionBLocal = _elbowRightSpring.BodyB.Pose.ToLocalPosition(elbowRightJointPosition);
Simulation.Constraints.Add(_elbowRightSpring);
_handRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Hand")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_handRightSpring);
_kneeLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Knee2")],
MaxForce = maxForce,
};
// This constraint should be attached at the knee position and not at the center of the lower leg:
var kneeLeftJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("L_Knee2")).Translation;
_kneeLeftSpring.AnchorPositionBLocal = _kneeLeftSpring.BodyB.Pose.ToLocalPosition(kneeLeftJointPosition);
Simulation.Constraints.Add(_kneeLeftSpring);
_ankleLeftSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("L_Ankle1")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_ankleLeftSpring);
_kneeRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Knee")],
MaxForce = maxForce,
};
// This constraint should be attached at the knee position and not at the center of the lower leg:
var kneeRightJointPosition = _meshNode.SkeletonPose.GetBonePoseAbsolute(ragdollSkeleton.GetIndex("R_Knee")).Translation;
_kneeRightSpring.AnchorPositionBLocal = _kneeRightSpring.BodyB.Pose.ToLocalPosition(kneeRightJointPosition);
Simulation.Constraints.Add(_kneeRightSpring);
_ankleRightSpring = new BallJoint
{
BodyA = Simulation.World,
BodyB = _ragdoll.Bodies[ragdollSkeleton.GetIndex("R_Ankle")],
MaxForce = maxForce,
};
Simulation.Constraints.Add(_ankleRightSpring);
}
public IKPhysicsSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
GraphicsScreen.DrawReticle = true;
// Add game objects which allows to grab rigid bodies.
_grabObject = new GrabObject(Services);
GameObjectService.Objects.Add(_grabObject);
// Add Dude model.
var modelNode = ContentManager.Load <ModelNode>("Dude/Dude");
_meshNode = modelNode.GetSubtree().OfType <MeshNode>().First().Clone();
_meshNode.PoseLocal = new Pose(new Vector3F(0, 0, 0));
SampleHelper.EnablePerPixelLighting(_meshNode);
GraphicsScreen.Scene.Children.Add(_meshNode);
// Create a ragdoll for the Dude model.
_ragdoll = new Ragdoll();
DudeRagdollCreator.Create(_meshNode.SkeletonPose, _ragdoll, Simulation, 0.571f);
// Set the initial world space pose of the whole ragdoll. And copy the bone poses of the
// current skeleton pose.
_ragdoll.Pose = _meshNode.PoseWorld;
_ragdoll.UpdateBodiesFromSkeleton(_meshNode.SkeletonPose);
// Enable constraints (joints and limits, no motors)
_ragdoll.EnableJoints();
_ragdoll.EnableLimits();
_ragdoll.DisableMotors();
foreach (var body in _ragdoll.Bodies)
{
if (body != null)
{
// Disable rigid body sleeping. (If we leave it enabled, the simulation might
// disable slow bodies before they reach their IK goal.)
body.CanSleep = false;
// Disable collisions response.
body.CollisionResponseEnabled = false;
}
}
// Add rigid bodies and the constraints of the ragdoll to the simulation.
_ragdoll.AddToSimulation(Simulation);
// Disable all force effects (default gravity and damping).
Simulation.ForceEffects.Clear();
// Create constraints which hold selected bodies at their current position
// relative to the world.
// To constrain the position + orientation, we use a FixedJoint.
foreach (var boneName in new[] { "Pelvis" })
{
var ragdollBody = _ragdoll.Bodies[_meshNode.SkeletonPose.Skeleton.GetIndex(boneName)];
var ikJoint = new FixedJoint
{
AnchorPoseALocal = ragdollBody.Pose,
BodyA = Simulation.World,
AnchorPoseBLocal = Pose.Identity,
BodyB = ragdollBody,
CollisionEnabled = false,
MaxForce = 1000,
};
_ikJoints.Add(ikJoint);
Simulation.Constraints.Add(ikJoint);
}
// To constrain only the position, we use a BallJoint.
foreach (var boneName in new[] { "L_Hand", "R_Hand", "L_Ankle1", "R_Ankle" })
{
var ragdollBody = _ragdoll.Bodies[_meshNode.SkeletonPose.Skeleton.GetIndex(boneName)];
var ikJoint = new BallJoint
{
AnchorPositionALocal = ragdollBody.Pose.Position,
BodyA = Simulation.World,
AnchorPositionBLocal = Vector3F.Zero,
BodyB = ragdollBody,
CollisionEnabled = false,
MaxForce = 1000,
};
_ikJoints.Add(ikJoint);
Simulation.Constraints.Add(ikJoint);
}
}
/// <summary>
/// Allows the user to drag a rigid body by using touch.
/// </summary>
private void DragBodies()
{
// Here is how it works:
// We first make a hit-test using a ray to check whether the user touches a rigid body.
// If there is a hit we create a spring (using a ball-socket joint) and connect the rigid
// body to the touch location. Every time the user moves her finger we update the position
// of the spring and the spring pulls the rigid body towards the finger.
// We use raw touch points to select and drag a rigid body.
TouchCollection touches = InputService.TouchCollection;
if (touches.Count == 0)
{
// No touches detected.
if (_spring != null)
{
// There is an active spring, so the user is currently dragging a rigid body.
// Release the body by removing the spring.
_spring.Simulation.Constraints.Remove(_spring);
_spring = null;
}
}
else
{
// Touch detected.
TouchLocation touchLocation = touches[0];
// Convert the touch location from screen coordinates to world coordinates.
var cameraNode = GraphicsScreen.CameraNode;
Vector3 pScreen = new Vector3(touchLocation.Position.X, touchLocation.Position.Y, 0);
Vector3 pWorld = GraphicsService.GraphicsDevice.Viewport.Unproject(
pScreen,
cameraNode.Camera.Projection,
(Matrix)cameraNode.View,
Matrix.Identity);
// pWorld is point on the near clip plane of the camera.
// Set the origin and direction of the ray for hit-testing.
Vector3F rayOrigin = _cameraPosition;
Vector3F rayDirection = ((Vector3F)pWorld - _cameraPosition).Normalized;
if (touchLocation.State == TouchLocationState.Pressed)
{
// Let's create a ray and see if we hit a rigid body.
// (Create the ray shape and the required collision object only once.)
if (_rayShape == null)
{
_rayShape = new RayShape {
StopsAtFirstHit = true
};
_rayCollisionObject = new CollisionObject(new GeometricObject(_rayShape));
}
// Set the origin and direction of the ray.
_rayShape.Origin = rayOrigin;
_rayShape.Direction = rayDirection.Normalized;
// Make a hit test using the collision detection and get the first contact found.
var contactSet = Simulation.CollisionDomain
.GetContacts(_rayCollisionObject)
.FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// Get the point where the ray hits the rigid body.
Contact contact = contactSet[0];
// The contact sets contains two objects ("ObjectA" and "ObjectB").
// One is the ray the other is the object that was hit by the ray.
var hitCollisionObject = (contactSet.ObjectA == _rayCollisionObject)
? contactSet.ObjectB
: contactSet.ObjectA;
// Check whether the object is a dynamic rigid body.
var hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
{
// Remove the old joint, in case a rigid body is already grabbed.
if (_spring != null && _spring.Simulation != null)
{
_spring.Simulation.Constraints.Remove(_spring);
}
// The penetration depth tells us the distance from the ray origin to the rigid body
// in view direction.
_springAnchorDistanceFromCamera = contact.PenetrationDepth;
// Get the position where the ray hits the other object.
// (The position is defined in the local space of the object.)
Vector3F hitPositionLocal = (contactSet.ObjectA == _rayCollisionObject)
? contact.PositionBLocal
: contact.PositionALocal;
// Attach the rigid body at the touch location using a ball-socket joint.
// (Note: We could also use a FixedJoint, if we don't want any rotations.)
_spring = new BallJoint
{
BodyA = hitBody,
AnchorPositionALocal = hitPositionLocal,
// We need to attach the grabbed object to a second body. In this case we just want to
// anchor the object at a specific point in the world. To achieve this we can use the
// special rigid body "World", which is defined in the simulation.
BodyB = Simulation.World,
AnchorPositionBLocal = rayOrigin + rayDirection * _springAnchorDistanceFromCamera,
// Some constraint adjustments.
ErrorReduction = 0.3f,
// We set a softness > 0. This makes the joint "soft" and it will act like
// damped spring.
Softness = 0.00001f,
// We limit the maximal force. This reduces the ability of this joint to violate
// other constraints.
MaxForce = 1e6f
};
// Add the spring to the simulation.
Simulation.Constraints.Add(_spring);
}
}
}
else if (touchLocation.State == TouchLocationState.Moved)
{
if (_spring != null)
{
// User has grabbed something.
// Update the position of the object by updating the anchor position of the ball-socket
// joint.
_spring.AnchorPositionBLocal = rayOrigin + rayDirection * _springAnchorDistanceFromCamera;
// Reduce the angular velocity by a certain factor. (This acts like a damping because we
// do not want the object to rotate like crazy.)
_spring.BodyA.AngularVelocity *= 0.9f;
}
}
}
}
public void Connect(string strName, out BallJoint item)
{
item = RuntimeObject.FromPtr(ExGetProperty(this.m_pSelf, strName, 4, 0x61a8)) as BallJoint;
}
// OnUpdate() is called once per frame.
protected override void OnUpdate(TimeSpan deltaTime)
{
if (_spring != null
&& !_inputService.IsDown(MouseButtons.Left)
&& !_inputService.IsDown(Buttons.LeftTrigger, LogicalPlayerIndex.One))
{
// The user has released the object.
_simulation.Constraints.Remove(_spring);
_spring = null;
}
if (!_inputService.IsMouseOrTouchHandled
&& !_inputService.IsGamePadHandled(LogicalPlayerIndex.Any)
&& (_inputService.IsPressed(MouseButtons.Left, false)
|| _inputService.IsPressed(Buttons.LeftTrigger, false, LogicalPlayerIndex.One)))
{
// The user has pressed the grab button and the input was not already handled
// by another game object.
// Remove the old joint, in case anything is grabbed.
if (_spring != null)
{
_simulation.Constraints.Remove(_spring);
_spring = null;
}
// The spring is attached at the position that is targeted with the cross-hair.
// We can perform a ray hit-test to find the position. The ray starts at the camera
// position and shoots forward (-z direction).
var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
var cameraNode = cameraGameObject.CameraNode;
Vector3F cameraPosition = cameraNode.PoseWorld.Position;
Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(Vector3F.Forward);
// Create a ray for picking.
RayShape ray = new RayShape(cameraPosition, cameraDirection, 1000);
// The ray should stop at the first hit. We only want the first object.
ray.StopsAtFirstHit = true;
// The collision detection requires a CollisionObject.
CollisionObject rayCollisionObject = new CollisionObject(new GeometricObject(ray, Pose.Identity));
// Assign the collision object to collision group 2. (In SampleGame.cs a
// collision filter based on collision groups was set. Objects for hit-testing
// are in group 2.)
rayCollisionObject.CollisionGroup = 2;
// Get the first object that has contact with the ray.
ContactSet contactSet = _simulation.CollisionDomain.GetContacts(rayCollisionObject).FirstOrDefault();
if (contactSet != null && contactSet.Count > 0)
{
// The ray has hit something.
// The contact set contains all detected contacts between the ray and the rigid body.
// Get the first contact in the contact set. (A ray hit usually contains exactly 1 contact.)
Contact contact = contactSet[0];
// The contact set contains the object pair of the collision. One object is the ray.
// The other is the object we want to grab.
CollisionObject hitCollisionObject = (contactSet.ObjectA == rayCollisionObject) ? contactSet.ObjectB : contactSet.ObjectA;
// Check whether a dynamic rigid body was hit.
RigidBody hitBody = hitCollisionObject.GeometricObject as RigidBody;
if (hitBody != null && hitBody.MotionType == MotionType.Dynamic)
{
// Attach the rigid body at the cursor position using a ball-socket joint.
// (Note: We could also use a FixedJoint, if we don't want any rotations.)
// The penetration depth tells us the distance from the ray origin to the rigid body.
_springAttachmentDistanceFromObserver = contact.PenetrationDepth;
// Get the position where the ray hits the other object.
// (The position is defined in the local space of the object.)
Vector3F hitPositionLocal = (contactSet.ObjectA == rayCollisionObject) ? contact.PositionBLocal : contact.PositionALocal;
_spring = new BallJoint
{
BodyA = hitBody,
AnchorPositionALocal = hitPositionLocal,
// We need to attach the grabbed object to a second body. In this case we just want to
// anchor the object at a specific point in the world. To achieve this we can use the
// special rigid body "World", which is defined in the simulation.
BodyB = _simulation.World,
// AnchorPositionBLocal is set below.
// Some constraint adjustments.
ErrorReduction = 0.3f,
// We set a softness > 0. This makes the joint "soft" and it will act like
// damped spring.
Softness = 0.00001f,
// We limit the maximal force. This reduces the ability of this joint to violate
// other constraints.
MaxForce = 1e6f
};
// Add the spring to the simulation.
_simulation.Constraints.Add(_spring);
}
}
}
if (_spring != null)
{
// User has grabbed something.
// Update the position of the object by updating the anchor position of
// the ball-socket joint.
var cameraGameObject = (CameraObject)_gameObjectService.Objects["Camera"];
var cameraNode = cameraGameObject.CameraNode;
Vector3F cameraPosition = cameraNode.PoseWorld.Position;
Vector3F cameraDirection = cameraNode.PoseWorld.ToWorldDirection(-Vector3F.UnitZ);
_spring.AnchorPositionBLocal = cameraPosition + cameraDirection * _springAttachmentDistanceFromObserver;
// Reduce the angular velocity by a certain factor. (This acts like a damping because we
// do not want the object to rotate like crazy.)
_spring.BodyA.AngularVelocity *= 0.9f;
}
}
public void Connect(string strName, out BallJoint item)
{
ExAddProperty(this.m_pSelf, strName, 4, 0x61a8);
item = null;
}
// OnUnload() is called when the GameObject is removed from the IGameObjectService.
protected override void OnUnload()
{
_simulation.Constraints.Remove(_spring);
_spring = null;
}
public void CollisinFiltering()
{
var simulation = new Simulation();
var filter = (ICollisionFilter)simulation.CollisionDomain.CollisionDetection.CollisionFilter;
var bodyA = new RigidBody(new SphereShape(1));
simulation.RigidBodies.Add(bodyA);
var bodyB = new RigidBody(new SphereShape(1));
simulation.RigidBodies.Add(bodyB);
var bodyC = new RigidBody(new SphereShape(1));
simulation.RigidBodies.Add(bodyC);
var pairAB = new Pair<CollisionObject>(bodyA.CollisionObject, bodyB.CollisionObject);
var pairAC = new Pair<CollisionObject>(bodyA.CollisionObject, bodyC.CollisionObject);
var pairBC = new Pair<CollisionObject>(bodyB.CollisionObject, bodyC.CollisionObject);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
var constraintA = new BallJoint() { BodyA = bodyB, BodyB = bodyC, CollisionEnabled = false };
var constraintB = new BallJoint() { BodyA = bodyB, BodyB = bodyC };
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
simulation.Constraints.Add(constraintB);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
simulation.Constraints.Add(constraintA);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Remove(constraintB);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Remove(constraintA);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
simulation.Constraints.Add(constraintA);
simulation.Constraints.Add(constraintB);
constraintB.CollisionEnabled = false;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
constraintA.CollisionEnabled = true;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
constraintB.CollisionEnabled = true;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
constraintA.CollisionEnabled = false;
constraintB.CollisionEnabled = false;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Remove(constraintA);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Clear();
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
}
void CreateMainBody()
{
DestroyMainBody();
CreatePhysicsModel();
float height;
float walkUpHeight;
float fromPositionToFloorDistance;
Type.GetBodyFormInfo( crouching, out height, out walkUpHeight, out fromPositionToFloorDistance );
{
Body body = PhysicsModel.CreateBody();
mainBody = body;
body.Name = "main";
body.Position = Position;
body.Rotation = Rotation;
body.Sleepiness = 0;
body.AngularDamping = 10;
//body.CenterOfMassPosition = new Vec3( 0, 0, -height / 4 );
body.MassMethod = Body.MassMethods.Manually;
body.Mass = Type.Mass;
//body.InertiaTensorFactor = new Vec3( 100, 100, 100 );
body.PhysX_SolverPositionIterations = body.PhysX_SolverPositionIterations * 2;
body.PhysX_SolverVelocityIterations = body.PhysX_SolverVelocityIterations * 2;
float length = height - Type.Radius * 2;
if( length < 0 )
{
Log.Error( "Character: OnPostCreate: height - Type.Radius * 2 < 0." );
return;
}
//create main capsule
{
CapsuleShape shape = body.CreateCapsuleShape();
shape.Length = length;
float offset = fromPositionToFloorDistance - height / 2;
shape.Position = new Vec3( 0, 0, -offset );
shape.Radius = Type.Radius;
shape.ContactGroup = (int)ContactGroup.Dynamic;
shape.StaticFriction = 0;
shape.DynamicFriction = 0;
shape.Restitution = 0;
shape.Hardness = 0;
shape.SpecialLiquidDensity = 1500;
}
//{
// //ODE. create two capsules
// xx
// //float offset = fromPositionToFloorDistance - height / 2;
// float length = height - Type.Radius * 2 - walkUpHeight;
// if( length < 0 )
// {
// Log.Error( "Character: CreateMainBody: height - Type.Radius * 2 - walkUpHeight < 0." );
// return;
// }
// //create main capsule
// {
// CapsuleShape shape = body.CreateCapsuleShape();
// shape.Length = length;
// shape.Radius = Type.Radius;
// shape.ContactGroup = (int)ContactGroup.Dynamic;
// shape.StaticFriction = 0;
// shape.DynamicFriction = 0;
// shape.Restitution = 0;
// shape.Hardness = 0;
// shape.SpecialLiquidDensity = 1500;
// }
// //create bottom capsule
// {
// CapsuleShape shape = body.CreateCapsuleShape();
// shape.Length = height - Type.BottomRadius * 2;
// shape.Radius = Type.BottomRadius;
// shape.Position = new Vec3( 0, 0, ( height - walkUpHeight ) / 2 - height / 2 );
// shape.ContactGroup = (int)ContactGroup.Dynamic;
// shape.StaticFriction = 0;
// shape.DynamicFriction = 0;
// shape.Restitution = 0;
// shape.Hardness = 0;
// shape.SpecialLiquidDensity = 1500;
// }
//}
}
//create joint to fix rotation of the main body.
//PhysX only
if( PhysicsWorld.Instance.IsPhysX() )
{
//create not contactable Kinematic body to attach joint
Body body = PhysicsModel.CreateBody();
fixRotationJointBody = body;
body.PhysX_Kinematic = true;
//body.Static = true;
body.Name = "static";
body.Position = Position;
SphereShape shape = body.CreateSphereShape();
shape.Radius = Type.Radius / 5;
shape.ContactGroup = (int)ContactGroup.NoContact;
//create joint
fixRotationJoint = (BallJoint)PhysicsModel.CreateJoint( Joint.Type.Ball, mainBody, fixRotationJointBody );
fixRotationJoint.ContactsEnabled = false;
fixRotationJoint.Anchor = Position;
fixRotationJoint.PhysX_MotionLockedAxisX = false;
fixRotationJoint.PhysX_MotionLockedAxisY = false;
fixRotationJoint.PhysX_MotionLockedAxisZ = false;
fixRotationJoint.Axis1.LimitsEnabled = true;
fixRotationJoint.Axis1.LimitLow = 0;
fixRotationJoint.Axis1.LimitHigh = 0;
fixRotationJoint.Axis1.Direction = Vec3.XAxis;
fixRotationJoint.Axis2.LimitsEnabled = true;
fixRotationJoint.Axis2.LimitLow = 0;
fixRotationJoint.Axis2.LimitHigh = 0;
fixRotationJoint.Axis2.Direction = Vec3.YAxis;
fixRotationJoint.Axis3.Direction = Vec3.ZAxis;
}
PhysicsModel.PushToWorld();
DisableControlPhysicsModelPushedToWorldFlag = true;
}
public void CollisinFiltering()
{
var simulation = new Simulation();
var filter = (ICollisionFilter)simulation.CollisionDomain.CollisionDetection.CollisionFilter;
var bodyA = new RigidBody(new SphereShape(1));
simulation.RigidBodies.Add(bodyA);
var bodyB = new RigidBody(new SphereShape(1));
simulation.RigidBodies.Add(bodyB);
var bodyC = new RigidBody(new SphereShape(1));
simulation.RigidBodies.Add(bodyC);
var pairAB = new Pair <CollisionObject>(bodyA.CollisionObject, bodyB.CollisionObject);
var pairAC = new Pair <CollisionObject>(bodyA.CollisionObject, bodyC.CollisionObject);
var pairBC = new Pair <CollisionObject>(bodyB.CollisionObject, bodyC.CollisionObject);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
var constraintA = new BallJoint()
{
BodyA = bodyB, BodyB = bodyC, CollisionEnabled = false
};
var constraintB = new BallJoint()
{
BodyA = bodyB, BodyB = bodyC
};
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
simulation.Constraints.Add(constraintB);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
simulation.Constraints.Add(constraintA);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Remove(constraintB);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Remove(constraintA);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
simulation.Constraints.Add(constraintA);
simulation.Constraints.Add(constraintB);
constraintB.CollisionEnabled = false;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
constraintA.CollisionEnabled = true;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
constraintB.CollisionEnabled = true;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
constraintA.CollisionEnabled = false;
constraintB.CollisionEnabled = false;
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Remove(constraintA);
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsFalse(filter.Filter(pairBC));
simulation.Constraints.Clear();
Assert.IsTrue(filter.Filter(pairAB));
Assert.IsTrue(filter.Filter(pairAC));
Assert.IsTrue(filter.Filter(pairBC));
}
public ConstraintSample3(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- HingeJoint with AngularVelocityMotor
// A board is fixed on a pole like a wind wheel.
// An AngularVelocityMotor creates a rotation with constant angular velocity around the
// hinge axis.
RigidBody box0 = new RigidBody(new BoxShape(0.1f, 2f, 0.1f))
{
Pose = new Pose(new Vector3F(-2, 1, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box0);
RigidBody box1 = new RigidBody(new BoxShape(0.1f, 0.4f, 1f))
{
Pose = new Pose(new Vector3F(-2 + 0.05f, 1.8f, 0))
};
Simulation.RigidBodies.Add(box1);
HingeJoint hingeJoint = new HingeJoint
{
BodyA = box0,
BodyB = box1,
AnchorPoseALocal = new Pose(new Vector3F(0.05f, 0.8f, 0)),
AnchorPoseBLocal = new Pose(new Vector3F(-0.05f, 0, 0)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(hingeJoint);
AngularVelocityMotor angularVelocityMotor = new AngularVelocityMotor
{
BodyA = box0,
// The rotation axis is the local x axis of BodyA.
AxisALocal = Vector3F.UnitX,
BodyB = box1,
TargetVelocity = 10,
// The motor power is limit, so that the rotation can be stopped by other objects blocking
// the movement.
MaxForce = 10000,
// The HingeJoint controls all other axes. So this motor must only act on the hinge
// axis.
UseSingleAxisMode = true,
CollisionEnabled = false,
};
Simulation.Constraints.Add(angularVelocityMotor);
// ----- A PrismaticJoint with a LinearVelocityMotor.
RigidBody box2 = new RigidBody(new BoxShape(0.7f, 0.7f, 0.7f))
{
Pose = new Pose(new Vector3F(0, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box2);
RigidBody box3 = new RigidBody(new BoxShape(0.5f, 1.5f, 0.5f))
{
Pose = new Pose(new Vector3F(0, 1, 0))
};
Simulation.RigidBodies.Add(box3);
_prismaticJoint = new PrismaticJoint
{
BodyA = box2,
BodyB = box3,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
AnchorPoseBLocal = new Pose(new Vector3F(0, 0, 0), new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1)),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_prismaticJoint);
_linearVelocityMotor = new LinearVelocityMotor
{
BodyA = box2,
AxisALocal = -Vector3F.UnitY,
BodyB = box3,
TargetVelocity = 1,
CollisionEnabled = false,
MaxForce = 10000,
UseSingleAxisMode = true,
};
Simulation.Constraints.Add(_linearVelocityMotor);
// ----- A BallJoint with a TwistSwingLimit and a QuaternionMotor
// The ball joint connects a cylinder to a static box.
// The twist-swing limits rotational movement.
// The quaternion motor acts like a spring that controls the angle of joint.
RigidBody box4 = new RigidBody(new BoxShape(0.5f, 0.5f, 0.5f))
{
Pose = new Pose(new Vector3F(2, 2, 0)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(box4);
RigidBody cylinder0 = new RigidBody(new CylinderShape(0.1f, 0.75f))
{
Pose = new Pose(new Vector3F(2, 2 - 0.75f / 2 - 0.25f, 0))
};
Simulation.RigidBodies.Add(cylinder0);
_ballJoint = new BallJoint
{
BodyA = box4,
BodyB = cylinder0,
AnchorPositionALocal = new Vector3F(0, -0.25f, 0),
AnchorPositionBLocal = new Vector3F(0, 0.75f / 2, 0),
CollisionEnabled = false,
};
Simulation.Constraints.Add(_ballJoint);
_twistSwingLimit = new TwistSwingLimit
{
BodyA = box4,
// The first column is the twist axis (-y). The other two columns are the swing axes.
AnchorOrientationALocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
BodyB = cylinder0,
AnchorOrientationBLocal = new Matrix33F(0, 1, 0,
-1, 0, 0,
0, 0, 1),
CollisionEnabled = false,
// The twist is limited to +/- 10°. The swing limits are +/- 40° and +/- 60°. This creates
// a deformed cone that limits the swing movements (see visualization).
Minimum = new Vector3F(-MathHelper.ToRadians(10), -MathHelper.ToRadians(40), -MathHelper.ToRadians(60)),
Maximum = new Vector3F(MathHelper.ToRadians(10), MathHelper.ToRadians(40), MathHelper.ToRadians(60)),
};
Simulation.Constraints.Add(_twistSwingLimit);
QuaternionMotor quaternionMotor = new QuaternionMotor
{
BodyA = box4,
AnchorOrientationALocal = Matrix33F.Identity,
BodyB = cylinder0,
AnchorOrientationBLocal = Matrix33F.Identity,
CollisionEnabled = false,
// The QuaternionMotor controls the orientation of the second body relative to the first
// body. Here, we define that the cylinder should swing 30° away from the default
// orientation.
TargetOrientation = QuaternionF.CreateRotationZ(MathHelper.ToRadians(30)),
// Position and orientation motors are similar to damped-springs. We can control
// the stiffness and damping of the spring. (It is also possible to set the SpringConstant
// to 0 if the QuaternionMotor should only act as a rotational damping.)
SpringConstant = 100,
DampingConstant = 20,
};
Simulation.Constraints.Add(quaternionMotor);
}
public void AddRagdoll(float scale, Vector3F ragdollPosition)
{
// Ragdolls are usually used in games to create realistic death animations of
// characters. The character is usually rendered using a skinned triangle mesh.
// But in the physics simulation the body parts of the character are represented
// using simple shapes, such as spheres, capsules, boxes, or convex polyhedra,
// which are connected with joints.
// The physics simulations computes how these parts collide and fall. The positions
// and orientations are then read back each frame to update the animation of the
// triangle mesh.
// In this example the ragdoll is built from spheres, capsules and boxes. The
// rigid bodies are created in code. In practice, ragdolls should be built using
// external tools, such as a 3D modeller or a game editor.
#region ----- Create rigid bodies for the most relevant body parts -----
BoxShape pelvisShape = new BoxShape(0.3f * scale, 0.22f * scale, 0.20f * scale);
MassFrame pelvisMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, Density, 0.01f, 3);
RigidBody pelvis = new RigidBody(pelvisShape, pelvisMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.03f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(pelvis);
BoxShape torsoShape = new BoxShape(0.35f * scale, 0.22f * scale, 0.44f * scale);
MassFrame torsoMass = MassFrame.FromShapeAndDensity(torsoShape, Vector3F.One, Density, 0.01f, 3);
RigidBody torso = new RigidBody(torsoShape, torsoMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.4f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(torso);
SphereShape headShape = new SphereShape(0.13f * scale);
MassFrame headMass = MassFrame.FromShapeAndDensity(headShape, Vector3F.One, Density, 0.01f, 3);
RigidBody head = new RigidBody(headShape, headMass, null)
{
Pose = new Pose(new Vector3F(0f * scale, 0f, -0.776f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(head);
CapsuleShape upperArmShape = new CapsuleShape(0.08f * scale, 0.3f * scale);
MassFrame upperArmMass = MassFrame.FromShapeAndDensity(upperArmShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(-0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftUpperArm);
RigidBody rightUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightUpperArm);
CapsuleShape lowerArmShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerArmMass = MassFrame.FromShapeAndDensity(lowerArmShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(-0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftLowerArm);
RigidBody rightLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightLowerArm);
CapsuleShape upperLegShape = new CapsuleShape(0.09f * scale, 0.5f * scale);
MassFrame upperLegMass = MassFrame.FromShapeAndDensity(upperLegShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(-0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftUpperLeg);
RigidBody rightUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightUpperLeg);
CapsuleShape lowerLegShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerLegMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(leftLowerLeg);
RigidBody rightLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
Simulation.RigidBodies.Add(rightLowerLeg);
BoxShape footShape = new BoxShape(0.12f * scale, 0.28f * scale, 0.07f * scale);
MassFrame footMass = MassFrame.FromShapeAndDensity(footShape, Vector3F.One, Density, 0.01f, 3);
RigidBody leftFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(leftFoot);
RigidBody rightFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
Simulation.RigidBodies.Add(rightFoot);
#endregion
#region ----- Add joints between body parts -----
Vector3F pelvisJointPosition = new Vector3F(0f, 0.026f * scale, -0.115f * scale) + ragdollPosition;
HingeJoint pelvisJoint = new HingeJoint
{
BodyA = torso,
BodyB = pelvis,
AnchorPoseALocal = new Pose(torso.Pose.ToLocalPosition(pelvisJointPosition)),
AnchorPoseBLocal = new Pose(pelvis.Pose.ToLocalPosition(pelvisJointPosition)),
Minimum = -0.5f,
Maximum = 1.1f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(pelvisJoint);
Vector3F neckJointPosition = new Vector3F(0f, 0.026f * scale, -0.690f * scale) + ragdollPosition;
HingeJoint neckJoint = new HingeJoint
{
BodyA = head,
BodyB = torso,
AnchorPoseALocal = new Pose(head.Pose.ToLocalPosition(neckJointPosition)),
AnchorPoseBLocal = new Pose(torso.Pose.ToLocalPosition(neckJointPosition)),
Minimum = -1f,
Maximum = 1f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(neckJoint);
Vector3F leftShoulderJointPosition = new Vector3F(-0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F leftShoulderJointAxis = new Vector3F(0, -1, -1).Normalized;
Matrix33F leftShoulderJointOrientation = new Matrix33F();
leftShoulderJointOrientation.SetColumn(0, leftShoulderJointAxis);
leftShoulderJointOrientation.SetColumn(1, leftShoulderJointAxis.Orthonormal1);
leftShoulderJointOrientation.SetColumn(2, leftShoulderJointAxis.Orthonormal2);
BallJoint leftShoulderJoint = new BallJoint
{
BodyA = leftUpperArm,
BodyB = torso,
AnchorPositionALocal = leftUpperArm.Pose.ToLocalPosition(leftShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(leftShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftShoulderJoint);
Vector3F rightShoulderJointPosition = new Vector3F(0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F rightShoulderJointAxis = new Vector3F(0, 1, 1).Normalized;
Matrix33F rightShoulderJointOrientation = new Matrix33F();
rightShoulderJointOrientation.SetColumn(0, rightShoulderJointAxis);
rightShoulderJointOrientation.SetColumn(1, rightShoulderJointAxis.Orthonormal1);
rightShoulderJointOrientation.SetColumn(2, rightShoulderJointAxis.Orthonormal2);
BallJoint rightShoulderJoint = new BallJoint
{
BodyA = rightUpperArm,
BodyB = torso,
AnchorPositionALocal = rightUpperArm.Pose.ToLocalPosition(rightShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(rightShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightShoulderJoint);
Vector3F leftElbowJointPosition = new Vector3F(-0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
Matrix33F elbowAxisOrientation = new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0);
HingeJoint leftElbowJoint = new HingeJoint
{
BodyA = leftLowerArm,
BodyB = leftUpperArm,
AnchorPoseALocal = new Pose(leftLowerArm.Pose.ToLocalPosition(leftElbowJointPosition), leftLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(leftUpperArm.Pose.ToLocalPosition(leftElbowJointPosition), leftUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = -2,
Maximum = 0,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftElbowJoint);
Vector3F rightElbowJointPosition = new Vector3F(0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
HingeJoint rightElbowJoint = new HingeJoint
{
BodyA = rightLowerArm,
BodyB = rightUpperArm,
AnchorPoseALocal = new Pose(rightLowerArm.Pose.ToLocalPosition(rightElbowJointPosition), rightLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(rightUpperArm.Pose.ToLocalPosition(rightElbowJointPosition), rightUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = 0,
Maximum = 2,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightElbowJoint);
Vector3F leftHipJointPosition = new Vector3F(-0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint leftHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(leftHipJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftHipJointPosition), leftUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftHipJoint);
Vector3F rightHipJointPosition = new Vector3F(0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint rightHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(rightHipJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightHipJointPosition), rightUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightHipJoint);
Vector3F leftKneeJointPosition = new Vector3F(-0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint leftKneeJoint = new HingeJoint
{
BodyA = leftLowerLeg,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftKneeJoint);
Vector3F rightKneeJointPosition = new Vector3F(0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint rightKneeJoint = new HingeJoint
{
BodyA = rightLowerLeg,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightKneeJoint);
Vector3F leftAnkleJointPosition = new Vector3F(-0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint leftAnkleJoint = new HingeJoint
{
BodyA = leftFoot,
BodyB = leftLowerLeg,
AnchorPoseALocal = new Pose(leftFoot.Pose.ToLocalPosition(leftAnkleJointPosition)),
AnchorPoseBLocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftAnkleJointPosition), leftLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(leftAnkleJoint);
Vector3F rightAnkleJointPosition = new Vector3F(0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint rightAnkleJoint = new HingeJoint
{
BodyA = rightFoot,
BodyB = rightLowerLeg,
AnchorPoseALocal = new Pose(rightFoot.Pose.ToLocalPosition(rightAnkleJointPosition)),
AnchorPoseBLocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightAnkleJointPosition), rightLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = JointErrorReduction,
Softness = JointSoftness,
MaxForce = JointMaxForce,
};
Simulation.Constraints.Add(rightAnkleJoint);
#endregion
#region ----- Add damping to improve stability -----
if (DampingEnabled)
{
// Damping removes jiggling and improves stability.
AddDamping(pelvis, torso);
AddDamping(torso, head);
AddDamping(torso, leftUpperArm);
AddDamping(leftUpperArm, leftLowerArm);
AddDamping(torso, rightUpperArm);
AddDamping(rightUpperArm, rightLowerArm);
AddDamping(pelvis, leftUpperLeg);
AddDamping(pelvis, rightUpperLeg);
AddDamping(leftUpperLeg, leftLowerLeg);
AddDamping(rightUpperLeg, rightLowerLeg);
AddDamping(leftLowerLeg, leftFoot);
AddDamping(rightLowerLeg, rightFoot);
}
#endregion
}
void DestroyMainBody()
{
mainBody = null;
fixRotationJointBody = null;
fixRotationJoint = null;
DestroyPhysicsModel();
}
