/// <summary> /// Drives the controlled body. /// </summary> /// <param name="skeletonPose">The target skeleton pose.</param> /// <param name="deltaTime">The time step.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> internal void DriveToPose(SkeletonPose skeletonPose, float deltaTime) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } Debug.Assert(Ragdoll != null, "Motor must not be called when Ragdoll property is null."); Debug.Assert(Ragdoll.Simulation != null, "Ragdoll was not added to a simulation."); if (!Enabled) { return; } if (Mode == RagdollMotorMode.Velocity) { // ----- Velocity motor Debug.Assert(_quaternionMotor.Simulation == null, "Velocity motors should not be added to the simulation."); if (BoneIndex >= Ragdoll.Bodies.Count) { return; } var body = Ragdoll.Bodies[BoneIndex]; if (body == null) { return; } var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity; var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset; // Wake the body up. It should move, even if we move it very slowly. body.WakeUp(); // Set velocities. body.LinearVelocity = AnimationHelper.ComputeLinearVelocity(body.Pose.Position, childPose.Position, deltaTime); body.AngularVelocity = AnimationHelper.ComputeAngularVelocity(body.Pose.Orientation, childPose.Orientation, deltaTime); } else { // ----- Constraint motor if (_quaternionMotor.Simulation == null) { // The motor was not added to the simulation. (Invalid configuration) return; } var parentOffset = (ParentIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[ParentIndex] : Pose.Identity; var parentPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(ParentIndex)) * parentOffset; var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity; var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset; // Set the relative motor target. var rotationMatrix = parentPose.Orientation.Transposed * childPose.Orientation; _quaternionMotor.TargetOrientation = Quaternion.CreateFromRotationMatrix(rotationMatrix); } }
/// <summary> /// Updates the pose of a single body, so that the bodies match the bone transforms of the given /// bone. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <remarks> /// See also <see cref="UpdateBodiesFromSkeleton(SkeletonPose)"/>. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public void UpdateBodyFromSkeleton(SkeletonPose skeletonPose, int boneIndex) { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } if (boneIndex < 0 || boneIndex >= Bodies.Count) { return; } var body = Bodies[boneIndex]; if (body == null) { return; } Pose offset = (boneIndex < BodyOffsets.Count) ? BodyOffsets[boneIndex] : Pose.Identity; Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(boneIndex)) * offset; body.Pose = bodyPose; body.LinearVelocity = Vector3F.Zero; body.AngularVelocity = Vector3F.Zero; }
/// <summary> /// Updates the poses of the bodies, so that the bodies match the bone transforms of the given /// skeleton pose. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <remarks> /// The poses of the rigid bodies are changed instantly. The bodies will "teleport" instantly to /// the target positions. They will not interact correctly with other physics objects. The /// velocities of the rigid bodies are set to zero. The bodies will be positioned relative to /// the world space pose defined by <see cref="Pose"/>. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public void UpdateBodiesFromSkeleton(SkeletonPose skeletonPose) // = Teleport of bodies. { if (skeletonPose == null) { throw new ArgumentNullException("skeletonPose"); } var skeleton = skeletonPose.Skeleton; for (int i = 0; i < Bodies.Count && i < skeleton.NumberOfBones; i++) { var body = Bodies[i]; if (body == null) { continue; } Pose offset = (i < BodyOffsets.Count) ? BodyOffsets[i] : Pose.Identity; Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(i)) * offset; body.Pose = bodyPose; body.LinearVelocity = Vector3F.Zero; body.AngularVelocity = Vector3F.Zero; } }
public void Update(float deltaTime, Matrix world) { if (deltaTime <= 0) { return; } // Reset bone transform. SkeletonPose.SetBoneTransform(BoneIndex, SrtTransform.Identity); // Get new fixed point position in world space. var bonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(BoneIndex); var bonePoseWorld = world * bonePoseAbsolute; var fixedPointPosition = bonePoseWorld.TransformPosition(Offset); // If we haven't set the fixed point position before, then store the position // and we are done. if (_fixedPointPosition.IsNaN) { _fixedPointPosition = fixedPointPosition; return; } // New position and velocity of fixed point. _fixedPointVelocity = (fixedPointPosition - _fixedPointPosition) / deltaTime; _fixedPointPosition = fixedPointPosition; // If the particle position was not set before, then we only store the current values. // The real work starts in the next frame. if (_particlePosition.IsNaN) { _particlePosition = _fixedPointPosition; _particleVelocity = _fixedPointVelocity; return; } // Compute the spring force between the particle and the fixed point. var force = Spring * (_fixedPointPosition - _particlePosition) + Damping * (_fixedPointVelocity - _particleVelocity); // Update velocity and position of the particle using symplectic Euler. _particleVelocity = _particleVelocity + force * deltaTime; _particlePosition = _particlePosition + _particleVelocity * deltaTime; // Convert particle position back to bone space. var particleLocal = bonePoseWorld.Inverse.TransformPosition(_particlePosition); // Create rotation between the fixed point vector and the particle vector. var boneTransform = new SrtTransform(Quaternion.CreateFromRotationMatrix(Offset, particleLocal)); SkeletonPose.SetBoneTransform(BoneIndex, boneTransform); }
private void CorrectWorldSpacePose() { // Notes: // The Ragdoll class is simply a container for rigid bodies, joints, limits, motors, etc. // It has a Ragdoll.Pose property that determines the world space pose of the model. // The Ragdoll class does not update this property. It only reads it. // Let's say the ragdoll and model are created at the world space origin. Then the user // grabs the ragdoll and throws it 100 units away. Then the Ragdoll.Pose (and the root bone) // is still at the origin and the first body (the pelvis) is 100 units away. // You can observe this if you comment out this method and look at the debug rendering of // the skeleton. // To avoid this we correct the Ragdoll.Pose and make sure that it is always near the // pelvis bone. var pelvis = _targetSkeletonPose.Skeleton.GetIndex("Pelvis"); // This is different from the PassiveRagdollSample: // We use the _targetSkeletonPose to define the distance between pelvis and the Pose. var pelvisBonePoseAbsoluteInverse = _targetSkeletonPose.GetBonePoseAbsolute(pelvis).Inverse; _ragdoll.Pose = _ragdoll.Bodies[pelvis].Pose * _ragdoll.BodyOffsets[pelvis].Inverse * (Pose)pelvisBonePoseAbsoluteInverse; _meshNode.PoseWorld = _ragdoll.Pose; }
/// <summary> /// Initializes the ragdoll for the given skeleton pose. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="ragdoll">The ragdoll.</param> /// <param name="simulation">The simulation in which the ragdoll will be used.</param> /// <param name="scale">A scaling factor to scale the size of the ragdoll.</param> public static void Create(SkeletonPose skeletonPose, Ragdoll ragdoll, Simulation simulation, float scale) { var skeleton = skeletonPose.Skeleton; const float totalMass = 80; // The total mass of the ragdoll. const int numberOfBodies = 17; // Get distance from foot to head as a measure for the size of the ragdoll. int head = skeleton.GetIndex("Head"); int footLeft = skeleton.GetIndex("L_Ankle1"); var headPosition = skeletonPose.GetBonePoseAbsolute(head).Translation; var footPosition = skeletonPose.GetBonePoseAbsolute(footLeft).Translation; var headToFootDistance = (headPosition - footPosition).Length; // We use the same mass properties for all bodies. This is not realistic but more stable // because large mass differences or thin bodies (arms!) are less stable. // We use the mass properties of sphere proportional to the size of the model. var massFrame = MassFrame.FromShapeAndMass(new SphereShape(headToFootDistance / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1); var material = new UniformMaterial(); #region ----- Add Bodies and Body Offsets ----- var numberOfBones = skeleton.NumberOfBones; ragdoll.Bodies.AddRange(Enumerable.Repeat <RigidBody>(null, numberOfBones)); ragdoll.BodyOffsets.AddRange(Enumerable.Repeat(Pose.Identity, numberOfBones)); var pelvis = skeleton.GetIndex("Pelvis"); ragdoll.Bodies[pelvis] = new RigidBody(new BoxShape(0.3f * scale, 0.4f * scale, 0.55f * scale), massFrame, material); ragdoll.BodyOffsets[pelvis] = new Pose(new Vector3F(0, 0, 0)); var backLower = skeleton.GetIndex("Spine"); ragdoll.Bodies[backLower] = new RigidBody(new BoxShape(0.36f * scale, 0.4f * scale, 0.55f * scale), massFrame, material); ragdoll.BodyOffsets[backLower] = new Pose(new Vector3F(0.18f * scale, 0, 0)); var backUpper = skeleton.GetIndex("Spine2"); ragdoll.Bodies[backUpper] = new RigidBody(new BoxShape(0.5f * scale, 0.4f * scale, 0.65f * scale), massFrame, material); ragdoll.BodyOffsets[backUpper] = new Pose(new Vector3F(0.25f * scale, 0, 0)); var neck = skeleton.GetIndex("Neck"); ragdoll.Bodies[neck] = new RigidBody(new CapsuleShape(0.12f * scale, 0.3f * scale), massFrame, material); ragdoll.BodyOffsets[neck] = new Pose(new Vector3F(0.15f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); ragdoll.Bodies[neck].CollisionObject.Enabled = false; ragdoll.Bodies[head] = new RigidBody(new SphereShape(0.2f * scale), massFrame, material); ragdoll.BodyOffsets[head] = new Pose(new Vector3F(0.15f * scale, 0.02f * scale, 0)); var armUpperLeft = skeleton.GetIndex("L_UpperArm"); ragdoll.Bodies[armUpperLeft] = new RigidBody(new CapsuleShape(0.12f * scale, 0.6f * scale), massFrame, material); ragdoll.BodyOffsets[armUpperLeft] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var armLowerLeft = skeleton.GetIndex("L_Forearm"); ragdoll.Bodies[armLowerLeft] = new RigidBody(new CapsuleShape(0.08f * scale, 0.5f * scale), massFrame, material); ragdoll.BodyOffsets[armLowerLeft] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var handLeft = skeleton.GetIndex("L_Hand"); ragdoll.Bodies[handLeft] = new RigidBody(new BoxShape(0.2f * scale, 0.06f * scale, 0.15f * scale), massFrame, material); ragdoll.BodyOffsets[handLeft] = new Pose(new Vector3F(0.1f * scale, 0, 0)); var armUpperRight = skeleton.GetIndex("R_UpperArm"); ragdoll.Bodies[armUpperRight] = new RigidBody(new CapsuleShape(0.12f * scale, 0.6f * scale), massFrame, material); ragdoll.BodyOffsets[armUpperRight] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var armLowerRight = skeleton.GetIndex("R_Forearm"); ragdoll.Bodies[armLowerRight] = new RigidBody(new CapsuleShape(0.08f * scale, 0.5f * scale), massFrame, material); ragdoll.BodyOffsets[armLowerRight] = new Pose(new Vector3F(0.2f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var handRight = skeleton.GetIndex("R_Hand"); ragdoll.Bodies[handRight] = new RigidBody(new BoxShape(0.2f * scale, 0.06f * scale, 0.15f * scale), massFrame, material); ragdoll.BodyOffsets[handRight] = new Pose(new Vector3F(0.1f * scale, 0, 0)); var legUpperLeft = skeleton.GetIndex("L_Thigh1"); ragdoll.Bodies[legUpperLeft] = new RigidBody(new CapsuleShape(0.16f * scale, 0.8f * scale), massFrame, material); ragdoll.BodyOffsets[legUpperLeft] = new Pose(new Vector3F(0.4f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var legLowerLeft = skeleton.GetIndex("L_Knee2"); ragdoll.Bodies[legLowerLeft] = new RigidBody(new CapsuleShape(0.12f * scale, 0.65f * scale), massFrame, material); ragdoll.BodyOffsets[legLowerLeft] = new Pose(new Vector3F(0.32f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); //var footLeft = skeleton.GetIndex("L_Ankle1"); ragdoll.Bodies[footLeft] = new RigidBody(new BoxShape(0.20f * scale, 0.5f * scale, 0.3f * scale), massFrame, material); ragdoll.BodyOffsets[footLeft] = new Pose(new Vector3F(0.16f * scale, 0.15f * scale, 0)); var legUpperRight = skeleton.GetIndex("R_Thigh"); ragdoll.Bodies[legUpperRight] = new RigidBody(new CapsuleShape(0.16f * scale, 0.8f * scale), massFrame, material); ragdoll.BodyOffsets[legUpperRight] = new Pose(new Vector3F(0.4f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var legLowerRight = skeleton.GetIndex("R_Knee"); ragdoll.Bodies[legLowerRight] = new RigidBody(new CapsuleShape(0.12f * scale, 0.65f * scale), massFrame, material); ragdoll.BodyOffsets[legLowerRight] = new Pose(new Vector3F(0.32f * scale, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var footRight = skeleton.GetIndex("R_Ankle"); ragdoll.Bodies[footRight] = new RigidBody(new BoxShape(0.20f * scale, 0.5f * scale, 0.3f * scale), massFrame, material); ragdoll.BodyOffsets[footRight] = new Pose(new Vector3F(0.16f * scale, 0.15f * scale, 0)); #endregion #region ----- Set Collision Filters ----- // Collisions between connected bodies will be disabled in AddJoint(). (A BallJoint // has a property CollisionEnabled which decides whether connected bodies can // collide.) // But we need to disable some more collision between bodies that are not directly // connected but still too close to each other. var filter = (ICollisionFilter)simulation.CollisionDomain.CollisionDetection.CollisionFilter; filter.Set(ragdoll.Bodies[backUpper].CollisionObject, ragdoll.Bodies[head].CollisionObject, false); filter.Set(ragdoll.Bodies[armUpperRight].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false); filter.Set(ragdoll.Bodies[armUpperLeft].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false); filter.Set(ragdoll.Bodies[legUpperLeft].CollisionObject, ragdoll.Bodies[legUpperRight].CollisionObject, false); #endregion #region ----- Add Joints ----- AddJoint(skeletonPose, ragdoll, pelvis, backLower); AddJoint(skeletonPose, ragdoll, backLower, backUpper); AddJoint(skeletonPose, ragdoll, backUpper, neck); AddJoint(skeletonPose, ragdoll, neck, head); AddJoint(skeletonPose, ragdoll, backUpper, armUpperLeft); AddJoint(skeletonPose, ragdoll, armUpperLeft, armLowerLeft); AddJoint(skeletonPose, ragdoll, armLowerLeft, handLeft); AddJoint(skeletonPose, ragdoll, backUpper, armUpperRight); AddJoint(skeletonPose, ragdoll, armUpperRight, armLowerRight); AddJoint(skeletonPose, ragdoll, armLowerRight, handRight); AddJoint(skeletonPose, ragdoll, pelvis, legUpperLeft); AddJoint(skeletonPose, ragdoll, legUpperLeft, legLowerLeft); AddJoint(skeletonPose, ragdoll, legLowerLeft, footLeft); AddJoint(skeletonPose, ragdoll, pelvis, legUpperRight); AddJoint(skeletonPose, ragdoll, legUpperRight, legLowerRight); AddJoint(skeletonPose, ragdoll, legLowerRight, footRight); #endregion #region ----- Add Limits ----- // Choosing limits is difficult. // We create hinge limits with AngularLimits in the back and in the knee. // For all other joints we use TwistSwingLimits with symmetric cones. AddAngularLimit(skeletonPose, ragdoll, pelvis, backLower, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f)); AddAngularLimit(skeletonPose, ragdoll, backLower, backUpper, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.4f)); AddAngularLimit(skeletonPose, ragdoll, backUpper, neck, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f)); AddTwistSwingLimit(ragdoll, neck, head, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.5f, -0.7f), new Vector3F(0.1f, 0.5f, 0.7f)); var parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse; var childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperLeft).Inverse; var bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, backUpper, armUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(-0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armUpperLeft, armLowerLeft, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armLowerLeft, handLeft, Matrix33F.Identity, Matrix33F.CreateRotationX(+ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f)); parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse; childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperRight).Inverse; bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, backUpper, armUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armUpperRight, armLowerRight, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armLowerRight, handRight, Matrix33F.Identity, Matrix33F.CreateRotationX(-ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f)); parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse; childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperLeft).Inverse; bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, pelvis, legUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f)); AddAngularLimit(skeletonPose, ragdoll, legUpperLeft, legLowerLeft, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f)); AddTwistSwingLimit(ragdoll, legLowerLeft, footLeft, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f)); parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse; childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperRight).Inverse; bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, pelvis, legUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f)); AddAngularLimit(skeletonPose, ragdoll, legUpperRight, legLowerRight, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f)); AddTwistSwingLimit(ragdoll, legLowerRight, footRight, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f)); #endregion #region ----- Add Motors ----- ragdoll.Motors.AddRange(Enumerable.Repeat <RagdollMotor>(null, numberOfBones)); ragdoll.Motors[pelvis] = new RagdollMotor(pelvis, -1); ragdoll.Motors[backLower] = new RagdollMotor(backLower, pelvis); ragdoll.Motors[backUpper] = new RagdollMotor(backUpper, backLower); ragdoll.Motors[neck] = new RagdollMotor(neck, backUpper); ragdoll.Motors[head] = new RagdollMotor(head, neck); ragdoll.Motors[armUpperLeft] = new RagdollMotor(armUpperLeft, backUpper); ragdoll.Motors[armLowerLeft] = new RagdollMotor(armLowerLeft, armUpperLeft); ragdoll.Motors[handLeft] = new RagdollMotor(handLeft, armLowerLeft); ragdoll.Motors[armUpperRight] = new RagdollMotor(armUpperRight, backUpper); ragdoll.Motors[armLowerRight] = new RagdollMotor(armLowerRight, armUpperRight); ragdoll.Motors[handRight] = new RagdollMotor(handRight, armLowerRight); ragdoll.Motors[legUpperLeft] = new RagdollMotor(legUpperLeft, pelvis); ragdoll.Motors[legLowerLeft] = new RagdollMotor(legLowerLeft, legUpperLeft); ragdoll.Motors[footLeft] = new RagdollMotor(footLeft, legLowerLeft); ragdoll.Motors[legUpperRight] = new RagdollMotor(legUpperRight, pelvis); ragdoll.Motors[legLowerRight] = new RagdollMotor(legLowerRight, legUpperRight); ragdoll.Motors[footRight] = new RagdollMotor(footRight, legLowerRight); #endregion }
/// <summary> /// Initializes the ragdoll for the given skeleton pose. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="ragdoll">The ragdoll.</param> /// <param name="simulation">The simulation in which the ragdoll will be used.</param> public static void Create(SkeletonPose skeletonPose, Ragdoll ragdoll, Simulation simulation) { var skeleton = skeletonPose.Skeleton; const float totalMass = 80; // The total mass of the ragdoll. const int numberOfBodies = 17; // Get distance from foot to head as a measure for the size of the ragdoll. int head = skeleton.GetIndex("Head"); int footLeft = skeleton.GetIndex("L_Ankle1"); var headPosition = skeletonPose.GetBonePoseAbsolute(head).Translation; var footPosition = skeletonPose.GetBonePoseAbsolute(footLeft).Translation; var headToFootDistance = (headPosition - footPosition).Length; // We use the same mass properties for all bodies. This is not realistic but more stable // because large mass differences or thin bodies (arms!) are less stable. // We use the mass properties of sphere proportional to the size of the model. var massFrame = MassFrame.FromShapeAndMass(new SphereShape(headToFootDistance / 8), Vector3F.One, totalMass / numberOfBodies, 0.1f, 1); var material = new UniformMaterial(); #region ----- Add Bodies and Body Offsets ----- var numberOfBones = skeleton.NumberOfBones; ragdoll.Bodies.AddRange(Enumerable.Repeat<RigidBody>(null, numberOfBones)); ragdoll.BodyOffsets.AddRange(Enumerable.Repeat(Pose.Identity, numberOfBones)); var pelvis = skeleton.GetIndex("Pelvis"); ragdoll.Bodies[pelvis] = new RigidBody(new BoxShape(0.3f, 0.4f, 0.55f), massFrame, material); ragdoll.BodyOffsets[pelvis] = new Pose(new Vector3F(0.0f, 0, 0)); var backLower = skeleton.GetIndex("Spine"); ragdoll.Bodies[backLower] = new RigidBody(new BoxShape(0.36f, 0.4f, 0.55f), massFrame, material); ragdoll.BodyOffsets[backLower] = new Pose(new Vector3F(0.18f, 0, 0)); var backUpper = skeleton.GetIndex("Spine2"); ragdoll.Bodies[backUpper] = new RigidBody(new BoxShape(0.5f, 0.4f, 0.65f), massFrame, material); ragdoll.BodyOffsets[backUpper] = new Pose(new Vector3F(0.25f, 0, 0)); var neck = skeleton.GetIndex("Neck"); ragdoll.Bodies[neck] = new RigidBody(new CapsuleShape(0.12f, 0.3f), massFrame, material); ragdoll.BodyOffsets[neck] = new Pose(new Vector3F(0.15f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); ragdoll.Bodies[neck].CollisionObject.Enabled = false; ragdoll.Bodies[head] = new RigidBody(new SphereShape(0.2f), massFrame, material); ragdoll.BodyOffsets[head] = new Pose(new Vector3F(0.15f, 0.02f, 0)); var armUpperLeft = skeleton.GetIndex("L_UpperArm"); ragdoll.Bodies[armUpperLeft] = new RigidBody(new CapsuleShape(0.12f, 0.6f), massFrame, material); ragdoll.BodyOffsets[armUpperLeft] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var armLowerLeft = skeleton.GetIndex("L_Forearm"); ragdoll.Bodies[armLowerLeft] = new RigidBody(new CapsuleShape(0.08f, 0.5f), massFrame, material); ragdoll.BodyOffsets[armLowerLeft] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var handLeft = skeleton.GetIndex("L_Hand"); ragdoll.Bodies[handLeft] = new RigidBody(new BoxShape(0.2f, 0.06f, 0.15f), massFrame, material); ragdoll.BodyOffsets[handLeft] = new Pose(new Vector3F(0.1f, 0, 0)); var armUpperRight = skeleton.GetIndex("R_UpperArm"); ragdoll.Bodies[armUpperRight] = new RigidBody(new CapsuleShape(0.12f, 0.6f), massFrame, material); ragdoll.BodyOffsets[armUpperRight] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var armLowerRight = skeleton.GetIndex("R_Forearm"); ragdoll.Bodies[armLowerRight] = new RigidBody(new CapsuleShape(0.08f, 0.5f), massFrame, material); ragdoll.BodyOffsets[armLowerRight] = new Pose(new Vector3F(0.2f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var handRight = skeleton.GetIndex("R_Hand"); ragdoll.Bodies[handRight] = new RigidBody(new BoxShape(0.2f, 0.06f, 0.15f), massFrame, material); ragdoll.BodyOffsets[handRight] = new Pose(new Vector3F(0.1f, 0, 0)); var legUpperLeft = skeleton.GetIndex("L_Thigh1"); ragdoll.Bodies[legUpperLeft] = new RigidBody(new CapsuleShape(0.16f, 0.8f), massFrame, material); ragdoll.BodyOffsets[legUpperLeft] = new Pose(new Vector3F(0.4f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var legLowerLeft = skeleton.GetIndex("L_Knee2"); ragdoll.Bodies[legLowerLeft] = new RigidBody(new CapsuleShape(0.12f, 0.65f), massFrame, material); ragdoll.BodyOffsets[legLowerLeft] = new Pose(new Vector3F(0.32f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); //var footLeft = skeleton.GetIndex("L_Ankle1"); ragdoll.Bodies[footLeft] = new RigidBody(new BoxShape(0.20f, 0.5f, 0.3f), massFrame, material); ragdoll.BodyOffsets[footLeft] = new Pose(new Vector3F(0.16f, 0.15f, 0)); var legUpperRight = skeleton.GetIndex("R_Thigh"); ragdoll.Bodies[legUpperRight] = new RigidBody(new CapsuleShape(0.16f, 0.8f), massFrame, material); ragdoll.BodyOffsets[legUpperRight] = new Pose(new Vector3F(0.4f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var legLowerRight = skeleton.GetIndex("R_Knee"); ragdoll.Bodies[legLowerRight] = new RigidBody(new CapsuleShape(0.12f, 0.65f), massFrame, material); ragdoll.BodyOffsets[legLowerRight] = new Pose(new Vector3F(0.32f, 0, 0), QuaternionF.CreateRotationZ(ConstantsF.PiOver2)); var footRight = skeleton.GetIndex("R_Ankle"); ragdoll.Bodies[footRight] = new RigidBody(new BoxShape(0.20f, 0.5f, 0.3f), massFrame, material); ragdoll.BodyOffsets[footRight] = new Pose(new Vector3F(0.16f, 0.15f, 0)); #endregion #region ----- Set Collision Filters ----- // Collisions between connected bodies will be disabled in AddJoint(). (A BallJoint // has a property CollisionEnabled which decides whether connected bodies can // collide.) // But we need to disable some more collision between bodies that are not directly // connected but still too close to each other. var filter = (ICollisionFilter)simulation.CollisionDomain.CollisionDetection.CollisionFilter; filter.Set(ragdoll.Bodies[backUpper].CollisionObject, ragdoll.Bodies[head].CollisionObject, false); filter.Set(ragdoll.Bodies[armUpperRight].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false); filter.Set(ragdoll.Bodies[armUpperLeft].CollisionObject, ragdoll.Bodies[backLower].CollisionObject, false); filter.Set(ragdoll.Bodies[legUpperLeft].CollisionObject, ragdoll.Bodies[legUpperRight].CollisionObject, false); #endregion #region ----- Add Joints ----- AddJoint(skeletonPose, ragdoll, pelvis, backLower); AddJoint(skeletonPose, ragdoll, backLower, backUpper); AddJoint(skeletonPose, ragdoll, backUpper, neck); AddJoint(skeletonPose, ragdoll, neck, head); AddJoint(skeletonPose, ragdoll, backUpper, armUpperLeft); AddJoint(skeletonPose, ragdoll, armUpperLeft, armLowerLeft); AddJoint(skeletonPose, ragdoll, armLowerLeft, handLeft); AddJoint(skeletonPose, ragdoll, backUpper, armUpperRight); AddJoint(skeletonPose, ragdoll, armUpperRight, armLowerRight); AddJoint(skeletonPose, ragdoll, armLowerRight, handRight); AddJoint(skeletonPose, ragdoll, pelvis, legUpperLeft); AddJoint(skeletonPose, ragdoll, legUpperLeft, legLowerLeft); AddJoint(skeletonPose, ragdoll, legLowerLeft, footLeft); AddJoint(skeletonPose, ragdoll, pelvis, legUpperRight); AddJoint(skeletonPose, ragdoll, legUpperRight, legLowerRight); AddJoint(skeletonPose, ragdoll, legLowerRight, footRight); #endregion #region ----- Add Limits ----- // Choosing limits is difficult. // We create hinge limits with AngularLimits in the back and in the knee. // For all other joints we use TwistSwingLimits with symmetric cones. AddAngularLimit(skeletonPose, ragdoll, pelvis, backLower, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f)); AddAngularLimit(skeletonPose, ragdoll, backLower, backUpper, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.4f)); AddAngularLimit(skeletonPose, ragdoll, backUpper, neck, new Vector3F(0, 0, -0.3f), new Vector3F(0, 0, 0.3f)); AddTwistSwingLimit(ragdoll, neck, head, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.5f, -0.7f), new Vector3F(0.1f, 0.5f, 0.7f)); var parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse; var childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperLeft).Inverse; var bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, backUpper, armUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(-0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armUpperLeft, armLowerLeft, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armLowerLeft, handLeft, Matrix33F.Identity, Matrix33F.CreateRotationX(+ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f)); parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(backUpper).Inverse; childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(armUpperRight).Inverse; bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, backUpper, armUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationY(0.5f) * Matrix33F.CreateRotationZ(-0.5f), Matrix33F.Identity, new Vector3F(-0.7f, -1.2f, -1.2f), new Vector3F(0.7f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armUpperRight, armLowerRight, Matrix33F.CreateRotationZ(-1.2f), Matrix33F.Identity, new Vector3F(-0.3f, -1.2f, -1.2f), new Vector3F(0.3f, 1.2f, 1.2f)); AddTwistSwingLimit(ragdoll, armLowerRight, handRight, Matrix33F.Identity, Matrix33F.CreateRotationX(-ConstantsF.PiOver2), new Vector3F(-0.3f, -0.7f, -0.7f), new Vector3F(0.3f, 0.7f, 0.7f)); parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse; childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperLeft).Inverse; bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, pelvis, legUpperLeft, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f)); AddAngularLimit(skeletonPose, ragdoll, legUpperLeft, legLowerLeft, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f)); AddTwistSwingLimit(ragdoll, legLowerLeft, footLeft, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f)); parentBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(pelvis).Inverse; childBindPoseAbsolute = (Pose)skeleton.GetBindPoseAbsoluteInverse(legUpperRight).Inverse; bindPoseRelative = parentBindPoseAbsolute.Inverse * childBindPoseAbsolute; AddTwistSwingLimit(ragdoll, pelvis, legUpperRight, bindPoseRelative.Orientation * Matrix33F.CreateRotationZ(1.2f), Matrix33F.Identity, new Vector3F(-0.1f, -0.7f, -1.5f), new Vector3F(+0.1f, +0.7f, +1.5f)); AddAngularLimit(skeletonPose, ragdoll, legUpperRight, legLowerRight, new Vector3F(0, 0, -2.2f), new Vector3F(0, 0, 0.0f)); AddTwistSwingLimit(ragdoll, legLowerRight, footRight, Matrix33F.Identity, Matrix33F.Identity, new Vector3F(-0.1f, -0.3f, -0.7f), new Vector3F(0.1f, 0.3f, 0.7f)); #endregion #region ----- Add Motors ----- ragdoll.Motors.AddRange(Enumerable.Repeat<RagdollMotor>(null, numberOfBones)); ragdoll.Motors[pelvis] = new RagdollMotor(pelvis, -1); ragdoll.Motors[backLower] = new RagdollMotor(backLower, pelvis); ragdoll.Motors[backUpper] = new RagdollMotor(backUpper, backLower); ragdoll.Motors[neck] = new RagdollMotor(neck, backUpper); ragdoll.Motors[head] = new RagdollMotor(head, neck); ragdoll.Motors[armUpperLeft] = new RagdollMotor(armUpperLeft, backUpper); ragdoll.Motors[armLowerLeft] = new RagdollMotor(armLowerLeft, armUpperLeft); ragdoll.Motors[handLeft] = new RagdollMotor(handLeft, armLowerLeft); ragdoll.Motors[armUpperRight] = new RagdollMotor(armUpperRight, backUpper); ragdoll.Motors[armLowerRight] = new RagdollMotor(armLowerRight, armUpperRight); ragdoll.Motors[handRight] = new RagdollMotor(handRight, armLowerRight); ragdoll.Motors[legUpperLeft] = new RagdollMotor(legUpperLeft, pelvis); ragdoll.Motors[legLowerLeft] = new RagdollMotor(legLowerLeft, legUpperLeft); ragdoll.Motors[footLeft] = new RagdollMotor(footLeft, legLowerLeft); ragdoll.Motors[legUpperRight] = new RagdollMotor(legUpperRight, pelvis); ragdoll.Motors[legLowerRight] = new RagdollMotor(legLowerRight, legUpperRight); ragdoll.Motors[footRight] = new RagdollMotor(footRight, legLowerRight); #endregion }
/// <summary> /// Drives the controlled body. /// </summary> /// <param name="skeletonPose">The target skeleton pose.</param> /// <param name="deltaTime">The time step.</param> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> internal void DriveToPose(SkeletonPose skeletonPose, float deltaTime) { if (skeletonPose == null) throw new ArgumentNullException("skeletonPose"); Debug.Assert(Ragdoll != null, "Motor must not be called when Ragdoll property is null."); Debug.Assert(Ragdoll.Simulation != null, "Ragdoll was not added to a simulation."); if (!Enabled) return; if (Mode == RagdollMotorMode.Velocity) { // ----- Velocity motor Debug.Assert(_quaternionMotor.Simulation == null, "Velocity motors should not be added to the simulation."); if (BoneIndex >= Ragdoll.Bodies.Count) return; var body = Ragdoll.Bodies[BoneIndex]; if (body == null) return; var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity; var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset; // Wake the body up. It should move, even if we move it very slowly. body.WakeUp(); // Set velocities. body.LinearVelocity = AnimationHelper.ComputeLinearVelocity(body.Pose.Position, childPose.Position, deltaTime); body.AngularVelocity = AnimationHelper.ComputeAngularVelocity(body.Pose.Orientation, childPose.Orientation, deltaTime); } else { // ----- Constraint motor if (_quaternionMotor.Simulation == null) { // The motor was not added to the simulation. (Invalid configuration) return; } var parentOffset = (ParentIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[ParentIndex] : Pose.Identity; var parentPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(ParentIndex)) * parentOffset; var childOffset = (BoneIndex < Ragdoll.BodyOffsets.Count) ? Ragdoll.BodyOffsets[BoneIndex] : Pose.Identity; var childPose = Ragdoll.Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(BoneIndex)) * childOffset; // Set the relative motor target. var rotationMatrix = parentPose.Orientation.Transposed * childPose.Orientation; _quaternionMotor.TargetOrientation = QuaternionF.CreateRotation(rotationMatrix); } }
/// <summary> /// Perform mapping in absolute space. /// </summary> private static void MapAbsolute(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, QuaternionF rotationBToA, QuaternionF rotationAToB) { // The current absolute bone pose of bone A. var boneAActualRotationAbsolute = skeletonA.GetBonePoseAbsolute(boneIndexA).Rotation; var boneABindRotationAbsolute = skeletonA.Skeleton.GetBindPoseAbsoluteInverse(boneIndexA).Rotation; var boneBBindRotationAbsolute = skeletonB.Skeleton.GetBindPoseAbsoluteInverse(boneIndexB).Rotation.Inverse; var relativeRotation = boneAActualRotationAbsolute * boneABindRotationAbsolute; // Rotation: Using similarity transformation: (Read from right to left.) // Rotate from model B space to model A space. // Apply the bone transform rotation in model A space // Rotate back from model A space to model B space. relativeRotation = rotationAToB * relativeRotation * rotationBToA; skeletonB.SetBoneRotationAbsolute(boneIndexB, relativeRotation * boneBBindRotationAbsolute); // TODO: Map translations. // How? // Map translation relative to model space? // Map translation relative to the next common bone ancestor that is in both skeletons // (then we would need a mechanism or user input that gives us this ancestor, complicated)? // Map translation relative to local space (the bone transform translation as in MapLocal)? }
private static void DoWork(QuaternionF skeletonOffset, SkeletonPose skeletonA, SkeletonPose skeletonB, int boneIndexA, int neckBoneIndexA, int leftShoulderBoneIndexA, int rightShoulderBoneIndexA, int boneIndexB, int neckBoneIndexB, int leftShoulderBoneIndexB, int rightShoulderBoneIndexB) { // Reset root bone. skeletonB.ResetBoneTransforms(boneIndexB, boneIndexB, false, true, false); // Get absolute positions all bones. var boneA = skeletonA.GetBonePoseAbsolute(boneIndexA).Translation; var neckA = skeletonA.GetBonePoseAbsolute(neckBoneIndexA).Translation; var leftShoulderA = skeletonA.GetBonePoseAbsolute(leftShoulderBoneIndexA).Translation; var rightShoulderA = skeletonA.GetBonePoseAbsolute(rightShoulderBoneIndexA).Translation; var boneB = skeletonB.GetBonePoseAbsolute(boneIndexB).Translation; var neckB = skeletonB.GetBonePoseAbsolute(neckBoneIndexB).Translation; var leftShoulderB = skeletonB.GetBonePoseAbsolute(leftShoulderBoneIndexB).Translation; var rightShoulderB = skeletonB.GetBonePoseAbsolute(rightShoulderBoneIndexB).Translation; // Abort if any bone to bone distance is 0. if (Vector3F.AreNumericallyEqual(boneA, neckA) || Vector3F.AreNumericallyEqual(boneA, rightShoulderA) || Vector3F.AreNumericallyEqual(leftShoulderA, rightShoulderA) || Vector3F.AreNumericallyEqual(boneB, neckB) || Vector3F.AreNumericallyEqual(boneB, rightShoulderB) || Vector3F.AreNumericallyEqual(leftShoulderB, rightShoulderB)) { return; } // Get shoulder axis vectors in model B space. var shoulderAxisA = rightShoulderA - leftShoulderA; shoulderAxisA = skeletonOffset.Rotate(shoulderAxisA); var shoulderAxisB = rightShoulderB - leftShoulderB; // Create a twist rotation from the shoulder vectors. var shoulderRotation = QuaternionF.CreateRotation(shoulderAxisB, shoulderAxisA); // Apply this twist to the spine. (Modifies the neckB position.) neckB = boneB + shoulderRotation.Rotate(neckB - boneB); // Get spine vectors in model B space. var spineAxisA = neckA - boneA; spineAxisA = skeletonOffset.Rotate(spineAxisA); var spineAxisB = neckB - boneB; // Create swing rotation from spine vectors. var spineRotation = QuaternionF.CreateRotation(spineAxisB, spineAxisA); // Apply the shoulder twist rotation followed by the spine swing rotation. skeletonB.RotateBoneAbsolute(boneIndexB, spineRotation * shoulderRotation); }
/// <summary> /// Updates the pose of a single body, so that the bodies match the bone transforms of the given /// bone. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <param name="boneIndex">The index of the bone.</param> /// <remarks> /// See also <see cref="UpdateBodiesFromSkeleton(SkeletonPose)"/>. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public void UpdateBodyFromSkeleton(SkeletonPose skeletonPose, int boneIndex) { if (skeletonPose == null) throw new ArgumentNullException("skeletonPose"); if (boneIndex < 0 || boneIndex >= Bodies.Count) return; var body = Bodies[boneIndex]; if (body == null) return; Pose offset = (boneIndex < BodyOffsets.Count) ? BodyOffsets[boneIndex] : Pose.Identity; Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(boneIndex)) * offset; body.Pose = bodyPose; body.LinearVelocity = Vector3F.Zero; body.AngularVelocity = Vector3F.Zero; }
// = Teleport of bodies. /// <summary> /// Updates the poses of the bodies, so that the bodies match the bone transforms of the given /// skeleton pose. /// </summary> /// <param name="skeletonPose">The skeleton pose.</param> /// <remarks> /// The poses of the rigid bodies are changed instantly. The bodies will "teleport" instantly to /// the target positions. They will not interact correctly with other physics objects. The /// velocities of the rigid bodies are set to zero. The bodies will be positioned relative to /// the world space pose defined by <see cref="Pose"/>. /// </remarks> /// <exception cref="ArgumentNullException"> /// <paramref name="skeletonPose" /> is <see langword="null"/>. /// </exception> public void UpdateBodiesFromSkeleton(SkeletonPose skeletonPose) { if (skeletonPose == null) throw new ArgumentNullException("skeletonPose"); var skeleton = skeletonPose.Skeleton; for (int i = 0; i < Bodies.Count && i < skeleton.NumberOfBones; i++) { var body = Bodies[i]; if (body == null) continue; Pose offset = (i < BodyOffsets.Count) ? BodyOffsets[i] : Pose.Identity; Pose bodyPose = Pose * ((Pose)skeletonPose.GetBonePoseAbsolute(i)) * offset; body.Pose = bodyPose; body.LinearVelocity = Vector3F.Zero; body.AngularVelocity = Vector3F.Zero; } }
public void DrawSkeleton(SkeletonPose skeletonPose, Pose pose, Vector3F scale, float axisLength, Color color, bool drawOverScene) { if (!Enabled || skeletonPose == null) return; LineBatch lineBatch; TextBatch textBatch; if (drawOverScene) { lineBatch = OverSceneLineBatch; textBatch = OverScene3DTextBatch; } else { lineBatch = InSceneLineBatch; textBatch = InSceneTextBatch; } var skeleton = skeletonPose.Skeleton; var world = pose * Matrix44F.CreateScale(scale); for (int i = 0; i < skeleton.NumberOfBones; i++) { string name = skeleton.GetName(i); SrtTransform bonePose = skeletonPose.GetBonePoseAbsolute(i); var translation = bonePose.Translation; var rotation = bonePose.Rotation.ToRotationMatrix33(); var translationWorld = world.TransformPosition(translation); int parentIndex = skeleton.GetParent(i); if (parentIndex >= 0) { // Draw line to parent joint representing the parent bone. SrtTransform parentPose = skeletonPose.GetBonePoseAbsolute(parentIndex); lineBatch.Add(translationWorld, world.TransformPosition(parentPose.Translation), color); } // Add three lines in Red, Green and Blue. lineBatch.Add(translationWorld, world.TransformPosition(translation + rotation.GetColumn(0) * axisLength), Color.Red); lineBatch.Add(translationWorld, world.TransformPosition(translation + rotation.GetColumn(1) * axisLength), Color.Green); lineBatch.Add(translationWorld, world.TransformPosition(translation + rotation.GetColumn(2) * axisLength), Color.Blue); // Draw name. if (string.IsNullOrEmpty(name)) name = "-"; textBatch.Add( string.Format(CultureInfo.InvariantCulture, "{0} {1}", name, i), world.TransformPosition(translation + rotation.GetColumn(0) * axisLength * 0.5f), new Vector2F(0, 1), color); } }