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); }
private static void DrawTwistSwingLimit(BallJoint joint, TwistSwingLimit limit) { // ----- Draw swing cone. // The tip of the swing cone: Vector3F coneTip = joint.BodyA.Pose.ToWorldPosition(joint.AnchorPositionALocal); // The first point on the swing cone: Vector3 previousConePoint = (Vector3)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++) { Vector3 conePoint = (Vector3)limit.GetPointOnCone((i + 1) * segmentAngle, coneTip, _scale); // Line from cone tip to cone base. _points[_pointCount] = new VertexPositionColor((Vector3)coneTip, color); IncrementPointCount(); _points[_pointCount] = new VertexPositionColor(conePoint, color); IncrementPointCount(); // Line on the cone base. _points[_pointCount] = new VertexPositionColor(previousConePoint, color); IncrementPointCount(); _points[_pointCount] = new VertexPositionColor(conePoint, color); IncrementPointCount(); previousConePoint = conePoint; } // ----- Draw twist axis. // The x-axis is the twist direction. Vector3F twistAxis = Vector3F.UnitX; // The twist axis relative to body B. Vector3F twistAxisDirectionBLocal = limit.AnchorOrientationBLocal * twistAxis; // The twist axis relative to world space. Vector3F twistAxisDirection = limit.BodyB.Pose.ToWorldDirection(twistAxisDirectionBLocal); // (A similar computation is used in DrawArc() below.) // Line in twist direction. _points[_pointCount] = new VertexPositionColor((Vector3)coneTip, Color.Red); IncrementPointCount(); _points[_pointCount] = new VertexPositionColor((Vector3)(coneTip + twistAxisDirection * _scale), Color.Red); IncrementPointCount(); // 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(constraintToWorld, coneTip, Vector3F.UnitX, Vector3F.UnitY, limit.Minimum.X, limit.Maximum.X, Color.Red); }
/// <summary> /// Adds a TwistSwingLimit between the specified bones. /// </summary> /// <param name="ragdoll">The ragdoll.</param> /// <param name="parent">The parent bone.</param> /// <param name="child">The child bone.</param> /// <param name="parentAnchorOrientationLocal">The constraint anchor orientation relative to the parent bone.</param> /// <param name="childAnchorOrientationLocal">The constraint anchor orientation relative to the child bone.</param> /// <param name="minimum">The minimum limits (twist/swing/swing).</param> /// <param name="maximum">The maximum limits (twist/swing/swing).</param> private static void AddTwistSwingLimit(Ragdoll ragdoll, int parent, int child, Matrix33F parentAnchorOrientationLocal, Matrix33F childAnchorOrientationLocal, Vector3F minimum, Vector3F maximum) { var childBody = ragdoll.Bodies[child]; var childOffset = ragdoll.BodyOffsets[child]; var parentBody = ragdoll.Bodies[parent]; var parentOffset = ragdoll.BodyOffsets[parent]; var limit = new TwistSwingLimit { BodyA = parentBody, BodyB = childBody, AnchorOrientationALocal = parentOffset.Orientation.Transposed * parentAnchorOrientationLocal, AnchorOrientationBLocal = childOffset.Orientation.Transposed * childAnchorOrientationLocal, Minimum = minimum, Maximum = maximum, ErrorReduction = 0.2f, Softness = 0.001f }; ragdoll.Limits.Add(limit); }
private static void AddTwistSwingLimit(Ragdoll ragdoll, Skeleton skeleton, AvatarBone parentBone, AvatarBone childBone, Matrix33F orientationA, Matrix33F orientationB, Vector3F minimum, Vector3F maximum) { int parentIndex = (int)parentBone; int childIndex = (int)childBone; // The difficult part is to define the constraint anchor orientation. // Here is how we do it: // When we look at the front side of an Avatar in bind pose, the x-axis is parallel // to the arms. y points up and z is normal to the those axes. // // To define orientationA/B: // The anchor x-axis is the twist axis. That means, this is already the correct axis // for the hands (wrist joints) and orientationA/B are therefore Matrix33F.Identity. // For the Head, the twist axis must point up. Therefore orientationA/B must be a 90° // rotation about z to rotate the twist axis up. // For the shoulder-elbow connection, orientationA is Matrix.Identity. The swing cone must // not be parallel to the arm axis (because the elbow cannot bend backwards). Therefore, // orientationB defines a rotation that rotates the twist axis (= swing cone center) to the // front. // // To define AnchorOrientationALocal/AnchorOrientationBLocal: // AnchorOrientationALocal must be a rotation matrix that transforms a vector from local // constraint anchor space to local body space of A. // orientationA defines the constraint anchor orientation in model space. // With jointPosesAbsolute[boneAIndex].Orientation.Transposed, we convert from model space // to joint space. With ragdoll.BodyOffsets[boneAIndex].Orientation.Transposed, we convert from joint // space to body space. The combined rotation matrix converts from constraint anchor space // to body space. var limit = new TwistSwingLimit { BodyA = ragdoll.Bodies[parentIndex], BodyB = ragdoll.Bodies[childIndex], AnchorOrientationALocal = ragdoll.BodyOffsets[parentIndex].Orientation.Transposed * skeleton.GetBindPoseAbsoluteInverse(parentIndex).Rotation.ToRotationMatrix33() * orientationA, AnchorOrientationBLocal = ragdoll.BodyOffsets[childIndex].Orientation.Transposed * skeleton.GetBindPoseAbsoluteInverse(childIndex).Rotation.ToRotationMatrix33() * orientationB, Minimum = minimum, Maximum = maximum, ErrorReduction = 0.2f, Softness = 0.001f }; ragdoll.Limits[childIndex] = limit; }