/// <summary> /// Called when <see cref="IKSolver.Solve"/> is called. /// </summary> /// <param name="deltaTime">The current time step (in seconds).</param> protected override void OnSolve(float deltaTime) { if (_isDirty) { // Validate bone chain. if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, HingeBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the HingeBoneIndex do not form a valid bone chain."); } if (!SkeletonPose.IsAncestorOrSelf(HingeBoneIndex, TipBoneIndex)) { throw new ArgumentException("The HingeBoneIndex and the TipBoneIndex do not form a valid bone chain."); } } _isDirty = false; if (MinHingeAngle > MaxHingeAngle) { throw new AnimationException("The MinHingeAngle must be less than or equal to the MaxHingeAngle"); } // Remember original bone transforms for interpolation at the end. var originalRootBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex); var originalHingeBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex); var originalTipBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex); var rootBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(RootBoneIndex); var hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex); var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex); // Get tip position in model space. Vector3 tipAbsolute; if (TipBoneOrientation != null) { // If the user has specified an absolute tip rotation, then we consider this rotation and // use the tip bone origin as tip. tipAbsolute = tipBonePoseAbsolute.Translation; Target -= tipBonePoseAbsolute.ToParentDirection(tipBonePoseAbsolute.Scale * TipOffset); } else { // The user hasn't specified a desired tip rotation. Therefore we do not modify the // tip rotation and use the offset position in the tip bone as tip. tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset); } // Abort if we already touch the target. if (Vector3.AreNumericallyEqual(tipAbsolute, Target)) { return; } // Root to target vector. var rootToTarget = Target - rootBonePoseAbsolute.Translation; var rootToTargetLength = rootToTarget.Length; if (Numeric.IsZero(rootToTargetLength)) { return; } rootToTarget /= rootToTargetLength; // ----- Align chain with target. // Align the root to target vector with the root to tip vector. var rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation; var rootToTipLength = rootToTip.Length; if (!Numeric.IsZero(rootToTipLength)) { rootToTip /= rootToTipLength; var rotation = Quaternion.CreateFromRotationMatrix(rootToTip, rootToTarget); if (rotation.Angle > Numeric.EpsilonF) { // Apply rotation to root bone. rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation; SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation); hingeBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(HingeBoneIndex); // Compute new tip absolute tip position from the known quantities. tipAbsolute = rootBonePoseAbsolute.Translation + rootToTarget * rootToTipLength; } } // ----- Compute ideal angle. var rootToHinge = hingeBonePoseAbsolute.Translation - rootBonePoseAbsolute.Translation; var hingeToTip = tipAbsolute - hingeBonePoseAbsolute.Translation; var hingeAxis = hingeBonePoseAbsolute.ToParentDirection(HingeAxis); // Project vectors to hinge plane. Everything should be in a plane for the following // computations. rootToHinge -= hingeAxis * Vector3.Dot(rootToHinge, hingeAxis); hingeToTip -= hingeAxis * Vector3.Dot(hingeToTip, hingeAxis); // Get lengths. float rootToHingeLength = rootToHinge.Length; if (Numeric.IsZero(rootToHingeLength)) { return; } rootToHinge /= rootToHingeLength; float hingeToTipLength = hingeToTip.Length; if (Numeric.IsZero(hingeToTipLength)) { return; } hingeToTip /= hingeToTipLength; // Compute current hinge angle (angle between root bone and hinge bone). float currentHingeAngle = (float)Math.Acos(MathHelper.Clamp(Vector3.Dot(rootToHinge, hingeToTip), -1, 1)); // Make sure the computed angle is about the hingeAxis and not about -hingeAxis. if (Vector3.Dot(Vector3.Cross(rootToHinge, hingeToTip), hingeAxis) < 0) { currentHingeAngle = -currentHingeAngle; } // Using law of cosines to compute the desired hinge angle using the triangle lengths. float cosDesiredHingeAngle = (rootToHingeLength * rootToHingeLength + hingeToTipLength * hingeToTipLength - rootToTargetLength * rootToTargetLength) / (2 * rootToHingeLength * hingeToTipLength); float desiredHingeAngle = ConstantsF.Pi - (float)Math.Acos(MathHelper.Clamp(cosDesiredHingeAngle, -1, 1)); // Apply hinge limits. if (desiredHingeAngle < MinHingeAngle) { desiredHingeAngle = MinHingeAngle; } else if (desiredHingeAngle > MaxHingeAngle) { desiredHingeAngle = MaxHingeAngle; } // Compute delta rotation between current and desired angle. float deltaAngle = desiredHingeAngle - currentHingeAngle; var hingeRotation = Quaternion.CreateFromRotationMatrix(hingeAxis, deltaAngle); hingeBonePoseAbsolute.Rotation = hingeRotation * hingeBonePoseAbsolute.Rotation; // Update tip position. tipAbsolute = hingeBonePoseAbsolute.Translation + hingeRotation.Rotate(tipAbsolute - hingeBonePoseAbsolute.Translation); // ----- Align chain with target. // If we hit a hinge limit, then we can move the tip closer to the target by aligning // the whole chain again. rootToTip = tipAbsolute - rootBonePoseAbsolute.Translation; rootToTipLength = rootToTip.Length; if (!Numeric.IsZero(rootToTipLength)) { rootToTip /= rootToTipLength; var rotation = Quaternion.CreateFromRotationMatrix(rootToTip, rootToTarget); rootBonePoseAbsolute.Rotation = rotation * rootBonePoseAbsolute.Rotation; hingeBonePoseAbsolute.Rotation = rotation * hingeBonePoseAbsolute.Rotation; } // ----- Set results. SkeletonPose.SetBoneRotationAbsolute(RootBoneIndex, rootBonePoseAbsolute.Rotation); SkeletonPose.SetBoneRotationAbsolute(HingeBoneIndex, hingeBonePoseAbsolute.Rotation); if (TipBoneOrientation != null) { SkeletonPose.SetBoneRotationAbsolute(TipBoneIndex, TipBoneOrientation.Value); } // ----- Apply weight, velocity limit and set results. bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { var targetBoneTransform = SkeletonPose.GetBoneTransform(RootBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalRootBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(RootBoneIndex, targetBoneTransform); targetBoneTransform = SkeletonPose.GetBoneTransform(HingeBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalHingeBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(HingeBoneIndex, targetBoneTransform); targetBoneTransform = SkeletonPose.GetBoneTransform(TipBoneIndex); if (requiresBlending) { BlendBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform); } if (requiresLimiting) { LimitBoneTransform(ref originalTipBoneTransform, ref targetBoneTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(TipBoneIndex, targetBoneTransform); } }
/// <summary> /// Called when <see cref="IKSolver.Solve"/> is called. /// </summary> /// <param name="deltaTime">The current time step (in seconds).</param> protected override void OnSolve(float deltaTime) { if (NumberOfIterations <= 0) { return; } if (_isDirty) { if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain."); } } _isDirty = false; var skeleton = SkeletonPose.Skeleton; bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { // Remember original bone transforms for interpolation with the result at the end. // Transforms are stored from tip to root (reverse order!). _originalTransforms.Clear(); int boneIndex = TipBoneIndex; while (true) { _originalTransforms.Add(SkeletonPose.GetBoneTransform(boneIndex)); if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } } int numberOfBones = SkeletonPose.GetNumberOfBones(RootBoneIndex, TipBoneIndex); // The transposed jacobian matrix. var jacobianTransposed = new MatrixF(numberOfBones, 6); // The force vector (3 linear and 3 angular (torque) entries). VectorF force = new VectorF(6); // The rotation axes of the bones. Vector3[] axes = new Vector3[numberOfBones]; float toleranceSquared = AllowedDeviation * AllowedDeviation; // In each iteration we compute the jacobian matrix, compute the bone velocities // an make an euler integration step. for (int iteration = 0; iteration < NumberOfIterations; iteration++) { var tipBoneAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex); var tipAbsolute = tipBoneAbsolute.ToParentPosition(TipOffset); var targetToTip = tipAbsolute - Target; if (targetToTip.LengthSquared() < toleranceSquared) { if (iteration == 0) { return; } break; } // Loop from tip to root and fill Jacobian. // (See description of Jacobian Transpose method to see how the rotation axes and // Jacobian entries must look like). var currentBoneIndex = TipBoneIndex; int exitBoneIndex = RootBoneIndex >= 0 ? skeleton.GetParent(RootBoneIndex) : -1; int i = numberOfBones; do { i--; // Compute rotation axis. Vector3 currentJointAbsolute = SkeletonPose.GetBonePoseAbsolute(currentBoneIndex).Translation; Vector3 jointToTarget = Target - currentJointAbsolute; Vector3 jointToTip = tipAbsolute - currentJointAbsolute; axes[i] = Vector3.Cross(jointToTarget, jointToTip); if (!axes[i].TryNormalize()) { axes[i] = Vector3.UnitX; // TODO: What should we really do in this case? } Vector3 jacobianColumnUpperPart = Vector3.Cross(jointToTip, axes[i]); // Fill J. jacobianTransposed[i, 0] = jacobianColumnUpperPart.X; jacobianTransposed[i, 1] = jacobianColumnUpperPart.Y; jacobianTransposed[i, 2] = jacobianColumnUpperPart.Z; jacobianTransposed[i, 3] = axes[i].X; jacobianTransposed[i, 4] = axes[i].Y; jacobianTransposed[i, 5] = axes[i].Z; currentBoneIndex = skeleton.GetParent(currentBoneIndex); } while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0); Debug.Assert(i == 0); // Set the force. force[0] = targetToTip.X; force[1] = targetToTip.Y; force[2] = targetToTip.Z; force[3] = 0; force[4] = 0; force[5] = 0; // Compute pseudo velocities. VectorF velocities = jacobianTransposed * force; // TODO: Garbage! // Euler integration step. currentBoneIndex = TipBoneIndex; i = numberOfBones; do { i--; // Rotation axis for this bone. Vector3 axis = axes[i]; // Angle is computed using Euler integration with an arbitrary step size. float angle = velocities[i] * StepSize; // Apply rotation. Quaternion rotationChange = Quaternion.CreateFromRotationMatrix(axis, angle); SkeletonPose.RotateBoneAbsolute(currentBoneIndex, rotationChange); currentBoneIndex = skeleton.GetParent(currentBoneIndex); } while (currentBoneIndex != exitBoneIndex && currentBoneIndex >= 0); // Call delegate that checks bone limits. if (LimitBoneTransforms != null) { LimitBoneTransforms(); } } if (requiresBlending || requiresLimiting) { // Apply weight and the angular velocity limit. int boneIndex = TipBoneIndex; int i = 0; while (true) { var originalTransform = _originalTransforms[i]; var targetTransform = SkeletonPose.GetBoneTransform(boneIndex); // Apply weight. if (requiresBlending) { BlendBoneTransform(ref originalTransform, ref targetTransform); } // Apply angular velocity limit. if (requiresLimiting) { LimitBoneTransform(ref originalTransform, ref targetTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(boneIndex, targetTransform); if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); i++; } } _originalTransforms.Clear(); }
private void UpdateDerivedValues() { // Validate chain. Compute bone lengths, bone indices and total chain length. if (_isDirty) { return; } _boneLengths.Clear(); _boneIndices.Clear(); _totalChainLength = -1; try { // Check if chain is valid. if (!SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain."); } // Get bone indices. SkeletonPose.GetChain(RootBoneIndex, TipBoneIndex, _boneIndices); var numberOfBones = _boneIndices.Count; // Get bone lengths. We compute the bone lengths from an actual bone pose because // our archer test model had a scale in the bone transforms. Therefore we cannot // compute the length from only the bind poses. _boneLengths.Capacity = numberOfBones; _totalChainLength = 0; for (int i = 0; i < numberOfBones - 1; i++) { int boneIndex = _boneIndices[i]; int childIndex = _boneIndices[i + 1]; var boneVector = SkeletonPose.GetBonePoseAbsolute(childIndex).Translation - SkeletonPose.GetBonePoseAbsolute(boneIndex).Translation; float boneLength = boneVector.Length; _boneLengths.Add(boneLength); _totalChainLength += boneLength; } // Tip bone. _boneLengths.Add((SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).Scale *TipOffset).Length); _totalChainLength += _boneLengths[numberOfBones - 1]; // Initialize _bones list with dummy values. _bones.Clear(); for (int i = 0; i < numberOfBones; i++) { _bones.Add(SrtTransform.Identity); } _isDirty = true; } catch { _boneLengths.Clear(); _boneIndices.Clear(); _totalChainLength = 0; throw; } }
/// <summary> /// Called when <see cref="IKSolver.Solve"/> is called. /// </summary> /// <param name="deltaTime">The current time step (in seconds).</param> protected override void OnSolve(float deltaTime) { if (NumberOfIterations <= 0) { return; } // Validate new chains. if (_isDirty && !SkeletonPose.IsAncestorOrSelf(RootBoneIndex, TipBoneIndex)) { throw new ArgumentException("The RootBoneIndex and the TipBoneIndex do not form a valid bone chain."); } _isDirty = false; var skeleton = SkeletonPose.Skeleton; bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { // Remember original bone transforms for interpolation with the result at the end. // Transforms are stored from tip to root (reverse order!). _originalTransforms.Clear(); int boneIndex = TipBoneIndex; while (true) { _originalTransforms.Add(SkeletonPose.GetBoneTransform(boneIndex)); if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } } // We iterate NumberOfIteration times or until we are within the allowed deviation. // In each iteration we move each bone once. float toleranceSquared = AllowedDeviation * AllowedDeviation; bool targetReached = false; for (int i = 0; i < NumberOfIterations && !targetReached; i++) { // Iterate bones from tip to root. int boneIndex = TipBoneIndex; while (true) { // Get current tip position in local bone space. var bonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(boneIndex); var targetPositionLocal = bonePoseAbsolute.ToLocalPosition(Target); Vector3 tipLocal; if (boneIndex == TipBoneIndex) { tipLocal = TipOffset; } else { var tipBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex); var tipAbsolute = tipBonePoseAbsolute.ToParentPosition(TipOffset); tipLocal = bonePoseAbsolute.ToLocalPosition(tipAbsolute); } if ((tipLocal - targetPositionLocal).LengthSquared() < toleranceSquared) { // Target reached! If this is the first iteration and the first bone, then we // didn't have to do anything and can abort. Otherwise we just leave the loops. if (i == 0 && boneIndex == TipBoneIndex) { return; } targetReached = true; break; } // Rotate bone so that it points to the target. if (tipLocal.TryNormalize() && targetPositionLocal.TryNormalize()) { var rotation = Quaternion.CreateFromRotationMatrix(tipLocal, targetPositionLocal); var angle = rotation.Angle; // If the bone gain is less than 1, then we make a smaller correction. We will need // more iterations but the change is more evenly distributed over the chain. if (BoneGain < 1) { angle = angle * BoneGain; rotation.Angle = angle; } // Apply rotation to bone transform. if (Numeric.IsGreater(angle, 0)) { var boneTransform = SkeletonPose.GetBoneTransform(boneIndex); boneTransform.Rotation = boneTransform.Rotation * rotation; SkeletonPose.SetBoneTransform(boneIndex, boneTransform); // Call delegate that enforces bone limits. if (LimitBoneTransforms != null) { LimitBoneTransforms(boneIndex); } } } if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); } } if (requiresBlending || requiresLimiting) { // Apply weight and the angular velocity limit. int boneIndex = TipBoneIndex; int i = 0; while (true) { var originalTransform = _originalTransforms[i]; var targetTransform = SkeletonPose.GetBoneTransform(boneIndex); // Apply weight. if (requiresBlending) { BlendBoneTransform(ref originalTransform, ref targetTransform); } // Apply angular velocity limit. if (requiresLimiting) { LimitBoneTransform(ref originalTransform, ref targetTransform, maxRotationAngle); } SkeletonPose.SetBoneTransform(boneIndex, targetTransform); if (boneIndex == RootBoneIndex) { break; } boneIndex = skeleton.GetParent(boneIndex); i++; } } _originalTransforms.Clear(); }