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