/// <summary> /// Perform mapping in absolute space. /// </summary> private static void MapAbsolute(bool mapTranslations, SkeletonPose skeletonA, int boneIndexA, SkeletonPose skeletonB, int boneIndexB, float scaleAToB, Quaternion rotationBToA, Quaternion 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)? }
/// <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) { UpdateDerivedValues(); if (_totalChainLength == 0) { return; } var skeleton = SkeletonPose.Skeleton; // Make a local copy of the absolute bone poses. The following operations will be performed // on the data in _bone and not on the skeleton pose. var numberOfBones = _boneIndices.Count; for (int i = 0; i < numberOfBones; i++) { _bones[i] = SkeletonPose.GetBonePoseAbsolute(_boneIndices[i]); } // Calculate the position at the tip of the last bone. // If TipOffset is not 0, then we can rotate the last bone. // If TipOffset is 0, then the last bone defines the tip but is not rotated. // --> numberOfBones is set to the number of affected bones. Vector3 tipAbsolute; if (TipOffset.IsNumericallyZero) { numberOfBones--; tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).Translation; } else { tipAbsolute = SkeletonPose.GetBonePoseAbsolute(TipBoneIndex).ToParentPosition(TipOffset); } // The root bone rotation that aligns the whole chain with the target. Quaternion chainRotation = Quaternion.Identity; Vector3 boneToTarget, boneToTip; float remainingChainLength = _totalChainLength; // Apply the soft limit to the distance to the IK goal //vecToIkGoal = Target - _bones[0].Translation; //distToIkGoal = vecToIkGoal.Length; //// Limit the extension to 98% and ramp it up over 5% of the chains length //vecToIkGoal *= (LimitValue(distToIkGoal, _totalChainLength * 0.98f, _totalChainLength * 0.08f)) / distToIkGoal; //Vector3 goalPosition = _bones[0].Translation + vecToIkGoal; var targetAbsolute = Target; // This algorithms iterates once over all bones from root to tip. for (int i = 0; i < numberOfBones; i++) { if (i > 0) { // Transform the bone position by the overall chain offset. var translation = _bones[0].Translation + chainRotation.Rotate(_bones[i].Translation - _bones[0].Translation); _bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation); } // The bone to tip vector of the aligned chain (without other IK rotations!). boneToTip = tipAbsolute - _bones[i].Translation; float boneToTipLength = boneToTip.Length; boneToTip /= boneToTipLength; // TODO: Check for division by 0? if (i > 0) { // Calculate the new absolute bone position. var translation = _bones[i - 1].ToParentPosition(skeleton.GetBindPoseRelative(_boneIndices[i]).Translation); _bones[i] = new SrtTransform(_bones[i].Scale, _bones[i].Rotation, translation); } // The bone to target vector of the new chain configuration. boneToTarget = targetAbsolute - _bones[i].Translation; float boneToTargetLength = boneToTarget.Length; boneToTarget /= boneToTargetLength; if (i == 0) { // This is the first bone: Compute rotation that aligns the whole initial chain with // the target. chainRotation = Quaternion.CreateFromRotationMatrix(boneToTip, boneToTarget); // Update tip. tipAbsolute = _bones[i].Translation + (boneToTarget * boneToTipLength); // Apply chainRotation to root bone. _bones[i] = new SrtTransform(_bones[i].Scale, chainRotation * _bones[i].Rotation, _bones[i].Translation); } else { // Apply the chain alignment rotation. Also the parent bones have changed, so we apply // an additional rotation that accounts for the ancestor rotations. This additional // rotation aligns the last bone with the target. // TODO: Find an explanation/derivation of this additional rotation. _bones[i] = new SrtTransform( _bones[i].Scale, Quaternion.CreateFromRotationMatrix(boneToTip, boneToTarget) * chainRotation * _bones[i].Rotation, _bones[i].Translation); } // Now, solve the bone using trigonometry. // The last bone was already aligned with the target. For the second last bone we use // the law of cosines. For all other bones we use the complicated steps described in the // GPG article. if (i <= numberOfBones - 2) { // Length of chain after this bone. remainingChainLength -= _boneLengths[i]; // The direction of the current bone. For the tip bone we use the TipOffset. Vector3 boneDirection; if (i != TipBoneIndex) { boneDirection = _bones[i].Rotation.Rotate(skeleton.GetBindPoseRelative(_boneIndices[i + 1]).Translation); } else { boneDirection = _bones[i].Rotation.Rotate(TipOffset); } if (!boneDirection.TryNormalize()) { continue; } // The bone rotates around an axis normal to the bone to target direction and the bone // vector. Vector3 rotationAxis = Vector3.Cross(boneToTarget, boneDirection); if (!rotationAxis.TryNormalize()) { continue; // TODO: If this happens, can we choose a useful direction? } // The current angle between bone direction and bone to target vector. float currentAngle = (float)Math.Acos(MathHelper.Clamp(Vector3.Dot(boneDirection, boneToTarget), -1, 1)); // Side lengths of the involved triangles. var a = _boneLengths[i]; var b = boneToTargetLength; var c = remainingChainLength; var d = boneToTipLength; float desiredAngle; if (i == numberOfBones - 2) { // Use trigonometry (law of cosines) to determine the desired angle. desiredAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f)); } else { // The maximal angle that this bone can have where the chain still reaches the tip. float maxTipAngle; if (boneToTipLength > remainingChainLength) { maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * a + d * d - c * c) / (2 * a * d), -1.0f, 1.0f)); } else { // Tip is very near and this bone can bend more than 180�. Add additional chain length // in radians. maxTipAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f)); maxTipAngle += ((c - d) / a); } // The maximal angle that this bone can have where the chain still reaches the target. float maxTargetAngle; if (boneToTargetLength > remainingChainLength) { maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * a + b * b - c * c) / (2 * a * b), -1.0f, 1.0f)); } else { // Target is very near and this bone can bend more than 180�. Add additional chain // length in radians. maxTargetAngle = (float)Math.Acos(MathHelper.Clamp((a * 0.5f) / remainingChainLength, 0.0f, 1.0f)); maxTargetAngle += ((c - b) / a); } // If we set the desired angle to maxTargetAngle, the remain bones must be all // stretched. We want to keep the chain appearance, therefore, we set a smaller angle. // The new angle relative to the final remaining chain should have the same ratio as the // current angle to the current remaining chain. if (!Numeric.IsZero(maxTipAngle)) { desiredAngle = maxTargetAngle * (currentAngle / maxTipAngle); } else { desiredAngle = maxTargetAngle; // Avoiding divide by zero. } } // The rotation angle that we have to apply. float deltaAngle = desiredAngle - currentAngle; // Apply the rotation to the current bones _bones[i] = new SrtTransform( _bones[i].Scale, (Quaternion.CreateFromRotationMatrix(rotationAxis, deltaAngle) * _bones[i].Rotation).Normalized, _bones[i].Translation); } } bool requiresBlending = RequiresBlending(); float maxRotationAngle; bool requiresLimiting = RequiresLimiting(deltaTime, out maxRotationAngle); if (requiresBlending || requiresLimiting) { // We have to blend the computed results with the original bone transforms. // Get original bone transforms. for (int i = 0; i < numberOfBones; i++) { _originalBoneTransforms.Add(SkeletonPose.GetBoneTransform(_boneIndices[i])); } for (int i = 0; i < numberOfBones; i++) { int boneIndex = _boneIndices[i]; var originalBoneTransform = _originalBoneTransforms[i]; // Set absolute bone pose and let the skeleton compute the bone transform for us. SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation); var targetBoneTransform = SkeletonPose.GetBoneTransform(boneIndex); // Apply weight. if (requiresBlending) { BlendBoneTransform(ref originalBoneTransform, ref targetBoneTransform); } // Apply angular velocity limit. if (requiresLimiting) { LimitBoneTransform(ref originalBoneTransform, ref targetBoneTransform, maxRotationAngle); } // Set final bone transform. SkeletonPose.SetBoneTransform(boneIndex, targetBoneTransform); } _originalBoneTransforms.Clear(); } else { // Weight is 1 and angular velocity limit is not active. // --> Just copy the compute rotations. for (int i = 0; i < numberOfBones; i++) { int boneIndex = _boneIndices[i]; SkeletonPose.SetBoneRotationAbsolute(boneIndex, _bones[i].Rotation); } } }