/// <summary>
/// Adds a key frame for the specified bone.
/// </summary>
/// <param name="boneIndex">The index of the bone.</param>
/// <param name="time">The time of the key frame.</param>
/// <param name="boneTransform">The bone transform.</param>
/// <exception cref="ArgumentOutOfRangeException">
/// <paramref name="boneIndex"/> is out of range.
/// </exception>
public void AddKeyFrame(int boneIndex, TimeSpan time, SrtTransform boneTransform)
{
if (boneIndex < 0)
{
throw new ArgumentOutOfRangeException("boneIndex", "boneIndex must not be negative.");
}
Unfreeze();
EnsurePreprocessingData();
// Get channel with key frames.
List <BoneKeyFrameSRT> channel;
if (!_preprocessData.Channels.TryGetValue(boneIndex, out channel))
{
// Create a new key frame list.
channel = new List <BoneKeyFrameSRT>();
_preprocessData.Channels.Add(boneIndex, channel);
}
// Add key frame to bone channel.
channel.Add(new BoneKeyFrameSRT {
Time = time, Transform = boneTransform
});
}
/// <summary>
/// Gets the bone transform for a certain time considering key frame interpolation.
/// </summary>
/// <param name="channelIndex">The index in <see cref="_channels"/>.</param>
/// <param name="timeIndex">The index in <see cref="_times"/>.</param>
/// <param name="time">The animation time.</param>
/// <returns>The animation value.</returns>
private SrtTransform GetBoneTransform(int channelIndex, int timeIndex, TimeSpan time)
{
// Get index in the key frames list using the _indices lookup table.
int keyFrameIndex = _indices[timeIndex * _channels.Length + channelIndex];
if (EnableInterpolation && keyFrameIndex + 1 < ((Array)_keyFrames[channelIndex]).Length)
{
// ----- Key frame interpolation.
// Get the key frame before and after the specified time.
TimeSpan previousTime, nextTime;
SrtTransform previousTransform, nextTransform;
GetBoneKeyFrames(channelIndex, keyFrameIndex, out previousTime, out previousTransform, out nextTime, out nextTransform);
float parameter = (float)(time.Ticks - previousTime.Ticks) / (nextTime - previousTime).Ticks;
SrtTransform.Interpolate(ref previousTransform, ref nextTransform, parameter, ref previousTransform);
return(previousTransform);
}
// ----- No key frame interpolation.
//TimeSpan time;
SrtTransform transform;
GetBoneKeyFrame(channelIndex, keyFrameIndex, out time, out transform);
return(transform);
}
/// <summary>
/// Applies max velocity limit to the given bone transform.
/// </summary>
/// <param name="originalTransform">
/// In: The original bone transform.
/// </param>
/// <param name="targetTransform">
/// In: The target bone transform.<br/>
/// Out: The limited bone transform.
/// </param>
/// <param name="maxRotationAngle">The max rotation angle.</param>
internal void LimitBoneTransform(ref SrtTransform originalTransform, ref SrtTransform targetTransform, float maxRotationAngle)
{
if (maxRotationAngle < ConstantsF.Pi)
{
// Compute relative rotation.
var rotationChange = targetTransform.Rotation * originalTransform.Rotation.Conjugated;
// Make sure we rotate around the shortest arc.
if (Quaternion.Dot(originalTransform.Rotation, targetTransform.Rotation) < 0)
{
rotationChange = -rotationChange;
}
if (rotationChange.Angle > maxRotationAngle && !rotationChange.V.IsNumericallyZero)
{
// ReSharper disable EmptyGeneralCatchClause
try
{
// Limit rotation.
rotationChange.Angle = maxRotationAngle;
targetTransform.Rotation = rotationChange * originalTransform.Rotation;
}
catch
{
// rotationChange.Angle = xxx. Can cause DivideByZeroException or similar.
// The !rotationChange.V.IsNumericallyZero should avoid this. But just to go sure.
}
// ReSharper restore EmptyGeneralCatchClause
}
}
}
/// <summary>
/// Gets two bone key frame for a given channel and key frame index.
/// </summary>
/// <param name="channelIndex">The index in <see cref="_channels"/>.</param>
/// <param name="keyFrameIndex">The index of the first key frame.</param>
/// <param name="time0">The time of the first key frame.</param>
/// <param name="transform0">The transform of the first key frame.</param>
/// <param name="time1">The time of the second key frame.</param>
/// <param name="transform1">The transform of the second key frame.</param>
private void GetBoneKeyFrames(int channelIndex, int keyFrameIndex,
out TimeSpan time0, out SrtTransform transform0,
out TimeSpan time1, out SrtTransform transform1)
{
Debug.Assert(keyFrameIndex + 1 < ((Array)_keyFrames[channelIndex]).Length, "Call GetBoneKeyFrame() instead of GetBoneKeyFrames()!");
var boneKeyFrameType = _keyFrameTypes[channelIndex];
if (boneKeyFrameType == BoneKeyFrameType.R)
{
var keyFrames = (BoneKeyFrameR[])_keyFrames[channelIndex];
var keyFrame = keyFrames[keyFrameIndex];
time0 = keyFrame.Time;
transform0.Scale = Vector3.One;
transform0.Rotation = keyFrame.Rotation;
transform0.Translation = Vector3.Zero;
keyFrame = keyFrames[keyFrameIndex + 1];
time1 = keyFrame.Time;
transform1.Scale = Vector3.One;
transform1.Rotation = keyFrame.Rotation;
transform1.Translation = Vector3.Zero;
}
else if (boneKeyFrameType == BoneKeyFrameType.RT)
{
var keyFrames = (BoneKeyFrameRT[])_keyFrames[channelIndex];
var keyFrame = keyFrames[keyFrameIndex];
time0 = keyFrame.Time;
transform0.Scale = Vector3.One;
transform0.Rotation = keyFrame.Rotation;
transform0.Translation = keyFrame.Translation;
keyFrame = keyFrames[keyFrameIndex + 1];
time1 = keyFrame.Time;
transform1.Scale = Vector3.One;
transform1.Rotation = keyFrame.Rotation;
transform1.Translation = keyFrame.Translation;
}
else
{
var keyFrames = (BoneKeyFrameSRT[])_keyFrames[channelIndex];
var keyFrame = keyFrames[keyFrameIndex];
time0 = keyFrame.Time;
transform0 = keyFrame.Transform;
keyFrame = keyFrames[keyFrameIndex + 1];
time1 = keyFrame.Time;
transform1 = keyFrame.Transform;
}
}
/// <summary>
/// Converts the bind pose matrices to SRT transforms.
/// </summary>
/// <param name="bindPoses">The bind pose matrices.</param>
/// <returns>The equivalent SRT transforms.</returns>
private static IList <SrtTransform> ConvertToSrt(IList <Matrix> bindPoses)
{
var numberOfBones = bindPoses.Count;
var result = new SrtTransform[numberOfBones];
for (int i = 0; i < numberOfBones; i++)
{
result[i] = SrtTransform.FromMatrix(bindPoses[i]);
}
return(result);
}
/// <summary>
/// Updates the absolute bone pose for the specified bone.
/// </summary>
/// <param name="boneIndex">The index of the bone.</param>
private void UpdateBonePoseAbsolute(int boneIndex)
{
if (_isBonePoseAbsoluteDirty[boneIndex])
{
lock (_syncRoot)
{
if (_isBonePoseAbsoluteDirty[boneIndex])
{
// Make sure relative bone pose is up-to-date.
UpdateBonePoseRelative(boneIndex);
int parentIndex = _skeletonPose.Skeleton.GetParent(boneIndex);
if (parentIndex < 0)
{
// No parent.
_bonePoseAbsolute[boneIndex] = _bonePoseRelative[boneIndex];
}
else
{
// Make sure parent is up-to-date. (Recursively update ancestors.)
UpdateBonePoseAbsolute(parentIndex);
//_bonePoseAbsolute[boneIndex] = _bonePoseAbsolute[parentIndex] * _bonePoseRelative[boneIndex];
SrtTransform.Multiply(ref _bonePoseAbsolute[parentIndex], ref _bonePoseRelative[boneIndex],
out _bonePoseAbsolute[boneIndex]);
}
Thread.MemoryBarrier();
#else
Interlocked.MemoryBarrier();
_isBonePoseAbsoluteDirty[boneIndex] = false;
}
}
}
}
/// <summary>
/// Updates the relative bone pose for the specified bone.
/// </summary>
/// <param name="boneIndex">The index of the bone.</param>
private void UpdateBonePoseRelative(int boneIndex)
{
if (_isBonePoseRelativeDirty[boneIndex])
{
lock (_syncRoot)
{
if (_isBonePoseRelativeDirty[boneIndex])
{
// _bonePoseRelative[boneIndex] = _skeletonPose.Skeleton.BindPosesRelative[boneIndex] * _skeletonPose.BoneTransforms[boneIndex];
SrtTransform.Multiply(ref _skeletonPose.Skeleton.BindPosesRelative[boneIndex],
ref _skeletonPose.BoneTransforms[boneIndex], out _bonePoseRelative[boneIndex]);
Thread.MemoryBarrier();
#else
Interlocked.MemoryBarrier();
_isBonePoseRelativeDirty[boneIndex] = false;
}
}
}
}
/// <summary>
/// Sets the bone transform of the specified bone.
/// </summary>
/// <param name="boneIndex">The index of the bone.</param>
/// <param name="boneTransform">The bone transform.</param>
public void SetBoneTransform(int boneIndex, SrtTransform boneTransform)
{
BoneTransforms[boneIndex] = boneTransform;
Invalidate(boneIndex);
}
/// <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)
{
// Get cosine of limit or -1 if unlimited.
float cosLimits = -1;
if (0 <= Limit && Limit < ConstantsF.PiOver2)
{
cosLimits = (float)Math.Cos(Limit);
}
var skeleton = SkeletonPose.Skeleton;
int parentIndex = skeleton.GetParent(BoneIndex);
// Bone pose without a bone transform.
var unanimatedBonePoseAbsolute = SkeletonPose.GetBonePoseAbsolute(parentIndex) * skeleton.GetBindPoseRelative(BoneIndex);
// Target position and direction in bone space.
var targetPositionLocal = unanimatedBonePoseAbsolute.ToLocalPosition(Target);
var targetDirection = targetPositionLocal - EyeOffset;
if (!targetDirection.TryNormalize())
{
return;
}
// The axes of the view space (where forward is -z, relative to bone space).
Vector3 forward = Forward;
Vector3 up = Up;
Vector3 side = Vector3.Cross(up, -forward);
// This matrix converts from view space to bone space (in other words, it
// rotates the -z direction into the view direction).
var boneFromView = new Matrix(side.X, up.X, -forward.X,
side.Y, up.Y, -forward.Y,
side.Z, up.Z, -forward.Z);
// Get the components of the target direction relative to the view space axes.
float targetUp = Vector3.Dot(targetDirection, up);
float targetSide = Vector3.Dot(targetDirection, side);
float targetForward = Vector3.Dot(targetDirection, forward);
// Limit rotations of the desired up and side vector.
// The target forward direction is inverted if necessary. (If limited the bone
// does not never rotate back.)
if (cosLimits > 0)
{
cosLimits = (float)Math.Sqrt(1 - cosLimits * cosLimits);
if (targetUp > 0 && targetUp > cosLimits)
{
targetUp = cosLimits;
}
else if (targetUp < 0 && -targetUp > cosLimits)
{
targetUp = -cosLimits;
}
if (targetSide > 0 && targetSide > cosLimits)
{
targetSide = cosLimits;
}
else if (targetSide < 0 && -targetSide > cosLimits)
{
targetSide = -cosLimits;
}
targetForward = Math.Abs(targetForward);
}
// Make new target direction vector that conforms to the limits.
targetDirection = Math.Sign(targetForward)
* forward * (float)Math.Sqrt(Math.Max(0, 1 - targetUp * targetUp - targetSide * targetSide))
+ side * targetSide + up * targetUp;
Debug.Assert(targetDirection.IsNumericallyNormalized);
// Make axes of desired view space.
forward = targetDirection;
side = Vector3.Cross(up, -forward);
if (!side.TryNormalize())
{
return;
}
up = Vector3.Cross(side, forward);
Debug.Assert(up.IsNumericallyNormalized);
// Create new view space matrix.
var boneFromNewView = new Matrix(
side.X, up.X, -forward.X,
side.Y, up.Y, -forward.Y,
side.Z, up.Z, -forward.Z);
// Apply a bone transform that rotates the rest view space to the desired view space.
Quaternion boneTransform = Quaternion.CreateFromRotationMatrix(boneFromNewView * boneFromView.Transposed);
var startTransform = SkeletonPose.GetBoneTransform(BoneIndex);
var lookAtTransform = new SrtTransform(startTransform.Scale, boneTransform, startTransform.Translation);
// Apply weight.
if (RequiresBlending())
{
BlendBoneTransform(ref startTransform, ref lookAtTransform);
}
// Apply angular velocity limit.
float maxRotationAngle;
if (RequiresLimiting(deltaTime, out maxRotationAngle))
{
LimitBoneTransform(ref startTransform, ref lookAtTransform, maxRotationAngle);
}
SkeletonPose.SetBoneTransform(BoneIndex, lookAtTransform);
}
/// <summary>
/// Updates all bone transformations.
/// </summary>
public void Update()
{
// This method updates does not check the individual dirty flags. If something is
// dirty all transforms are updated. Checking the dirty flags makes the average
// case (whole skeleton is animated) a lot slower.
if (_isDirty)
{
lock (_syncRoot)
{
if (_isDirty)
{
var skeleton = _skeletonPose.Skeleton;
int numberOfBones = skeleton.NumberOfBones;
// ----- Update dirty relative bone poses.
var boneTransforms = _skeletonPose.BoneTransforms;
var bindPosesRelative = skeleton.BindPosesRelative;
for (int i = 0; i < numberOfBones; i++)
{
//_bonePoseRelative[i] = bindPosesRelative[i] * boneTransforms[i];
SrtTransform.Multiply(ref bindPosesRelative[i], ref boneTransforms[i], out _bonePoseRelative[i]);
}
// ----- Update dirty absolute bone poses.
_bonePoseAbsolute[0] = _bonePoseRelative[0];
for (int i = 1; i < numberOfBones; i++)
{
int parentIndex = skeleton.GetParent(i);
//_bonePoseAbsolute[i] = _bonePoseAbsolute[parentIndex] * _bonePoseRelative[i];
SrtTransform.Multiply(ref _bonePoseAbsolute[parentIndex], ref _bonePoseRelative[i],
out _bonePoseAbsolute[i]);
}
// ----- Update skinning matrices (either the Matrix or the XNA variant).
if (_skinningMatrices != null)
{
for (int i = 0; i < numberOfBones; i++)
{
//_skinningMatrices[i] = _bonePoseAbsolute[i] * skeleton.BindPosesAbsoluteInverse[i];
SrtTransform.Multiply(ref _bonePoseAbsolute[i], ref skeleton.BindPosesAbsoluteInverse[i],
out _skinningMatrices[i]);
}
}
if (_skinningMatricesXna != null)
{
if (_skinningMatrices != null)
{
for (int i = 0; i < numberOfBones; i++)
_skinningMatricesXna[i] = (Matrix)_skinningMatrices[i];
}
else
{
for (int i = 0; i < numberOfBones; i++)
{
//_skinningMatricesXna[i] = _bonePoseAbsolute[i] * skeleton.BindPosesAbsoluteInverse[i];
SrtTransform.Multiply(ref _bonePoseAbsolute[i], ref skeleton.BindPosesAbsoluteInverse[i],
out _skinningMatricesXna[i]);
}
}
}
Thread.MemoryBarrier();
#else
Interlocked.MemoryBarrier();
_isBonePoseRelativeDirty.SetAll(false);
_isBonePoseAbsoluteDirty.SetAll(false);
_isSkinningMatrixDirty.SetAll(false);
_isDirty = false;
}
}
}
}
/// <summary>
/// Initializes the skeleton.
/// </summary>
/// <param name="boneParents">The bone parents.</param>
/// <param name="boneNames">The bone names.</param>
/// <param name="bindPosesRelative">The bind poses.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="boneParents"/>, <paramref name="boneNames"/> or
/// <paramref name="bindPosesRelative"/> is <see langword="null"/>.
/// </exception>
/// <exception cref="ArgumentException">
/// Either the given lists are empty, have different length, or the
/// <paramref name="boneParents"/> are invalid (parent bones must come be before their child
/// bones).
/// </exception>
internal void Initialize(IList <int> boneParents, IList <string> boneNames, IList <SrtTransform> bindPosesRelative)
{
if (boneParents == null)
{
throw new ArgumentNullException("boneParents");
}
if (boneNames == null)
{
throw new ArgumentNullException("boneNames");
}
if (bindPosesRelative == null)
{
throw new ArgumentNullException("bindPosesRelative");
}
var numberOfBones = boneParents.Count;
if (numberOfBones == 0)
{
throw new ArgumentException("boneParents list must not be empty.");
}
if (boneNames.Count == 0)
{
throw new ArgumentException("boneNames list must not be empty.");
}
if (bindPosesRelative.Count == 0)
{
throw new ArgumentException("bindPosesRelative list must not be empty.");
}
if (numberOfBones != boneNames.Count || numberOfBones != bindPosesRelative.Count)
{
throw new ArgumentException("The lists must have the same number of elements.");
}
// Check if bone parents come before children. This ordering also forbids cycles.
for (int index = 0; index < numberOfBones; index++)
{
var parentIndex = boneParents[index];
if (parentIndex >= index)
{
throw new ArgumentException("Invalid boneParents list. Parent bones must have a lower index than child bones.");
}
}
// Build list of bone nodes.
Bones = new Bone[numberOfBones];
var children = new List <int>();
for (int index = 0; index < numberOfBones; index++)
{
Bone bone = new Bone();
// Set parent index.
int parentIndex = boneParents[index];
if (parentIndex < -1)
{
parentIndex = -1;
}
bone.Parent = parentIndex;
// Create array of child indices.
children.Clear();
for (int childIndex = index + 1; childIndex < numberOfBones; childIndex++)
{
if (boneParents[childIndex] == index)
{
children.Add(childIndex);
}
}
if (children.Count > 0)
{
bone.Children = children.ToArray();
}
Bones[index] = bone;
}
// Initialize bone name/index dictionary.
if (BoneNames == null)
{
BoneNames = new Dictionary <string, int>(numberOfBones);
}
else
{
BoneNames.Clear();
}
for (int index = 0; index < numberOfBones; index++)
{
var name = boneNames[index];
if (name != null)
{
BoneNames[name] = index;
}
}
// Copy relative bind poses.
if (BindPosesRelative == null)
{
BindPosesRelative = new SrtTransform[numberOfBones];
}
for (int index = 0; index < numberOfBones; index++)
{
BindPosesRelative[index] = bindPosesRelative[index];
}
// Initialize absolute bind poses (inverse).
if (BindPosesAbsoluteInverse == null)
{
BindPosesAbsoluteInverse = new SrtTransform[numberOfBones];
}
// First store the non-inverted BindTransforms in model space.
BindPosesAbsoluteInverse[0] = BindPosesRelative[0];
for (int index = 1; index < numberOfBones; index++)
{
var parentIndex = Bones[index].Parent;
//BindPosesAbsoluteInverse[index] = BindPosesAbsoluteInverse[parentIndex] * BindPosesRelative[index];
SrtTransform.Multiply(ref BindPosesAbsoluteInverse[parentIndex], ref BindPosesRelative[index], out BindPosesAbsoluteInverse[index]);
}
// Invert matrices.
for (int index = 0; index < numberOfBones; index++)
{
BindPosesAbsoluteInverse[index].Invert();
}
}
/// <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);
}
}
}
/// <summary>
/// Draws the skeleton bones, bone space axes and bone names for debugging.
/// (Only available in the XNA-compatible build.)
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="graphicsDevice">The graphics device.</param>
/// <param name="effect">
/// A <see cref="BasicEffect"/> instance. The effect parameters <see cref="BasicEffect.World"/>,
/// <see cref="BasicEffect.View"/>, and <see cref="BasicEffect.Projection"/> must be
/// correctly initialized before this method is called.
/// </param>
/// <param name="axisLength">The visible length of the bone space axes.</param>
/// <param name="spriteBatch">
/// A <see cref="SpriteBatch"/>. Can be <see langword="null"/> to skip text rendering.
/// </param>
/// <param name="spriteFont">
/// A <see cref="SpriteFont"/>. Can be <see langword="null"/> to skip text rendering.
/// </param>
/// <param name="color">The color for the bones and the bone names.</param>
/// <remarks>
/// <para>
/// This method is available only in the XNA-compatible build of the DigitalRune.Animation.dll.
/// </para>
/// <para>
/// This method draws the skeleton for debugging. It draws a line for each bone and the bone
/// name. At the bone origin it draws 3 lines (red, green, blue) that visualize the bone
/// space axes (x, y, z).
/// </para>
/// </remarks>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose"/>, <paramref name="graphicsDevice"/> or
/// <paramref name="effect"/> is <see langword="null"/>.
/// </exception>
public static void DrawBones(this SkeletonPose skeletonPose, GraphicsDevice graphicsDevice,
BasicEffect effect, float axisLength, SpriteBatch spriteBatch, SpriteFont spriteFont, Color color)
{
if (skeletonPose == null)
{
throw new ArgumentNullException("skeletonPose");
}
if (graphicsDevice == null)
{
throw new ArgumentNullException("graphicsDevice");
}
if (effect == null)
{
throw new ArgumentNullException("effect");
}
var oldVertexColorEnabled = effect.VertexColorEnabled;
effect.VertexColorEnabled = true;
// No font, then we don't need the sprite batch.
if (spriteFont == null)
{
spriteBatch = null;
}
if (spriteBatch != null)
{
spriteBatch.Begin();
}
List <VertexPositionColor> vertices = new List <VertexPositionColor>();
var skeleton = skeletonPose.Skeleton;
for (int i = 0; i < skeleton.NumberOfBones; i++)
{
string name = skeleton.GetName(i);
SrtTransform bonePose = skeletonPose.GetBonePoseAbsolute(i);
var translation = (Vector3)bonePose.Translation;
var rotation = (Quaternion)bonePose.Rotation;
int parentIndex = skeleton.GetParent(i);
if (parentIndex >= 0)
{
// Draw line to parent joint representing the parent bone.
SrtTransform parentPose = skeletonPose.GetBonePoseAbsolute(parentIndex);
vertices.Add(new VertexPositionColor(translation, color));
vertices.Add(new VertexPositionColor((Vector3)parentPose.Translation, color));
}
// Add three lines in Red, Green and Blue.
vertices.Add(new VertexPositionColor(translation, Color.Red));
vertices.Add(new VertexPositionColor(translation + Vector3.Transform(Vector3.UnitX, rotation) * axisLength, Color.Red));
vertices.Add(new VertexPositionColor(translation, Color.Green));
vertices.Add(new VertexPositionColor(translation + Vector3.Transform(Vector3.UnitY, rotation) * axisLength, Color.Green));
vertices.Add(new VertexPositionColor(translation, Color.Blue));
vertices.Add(new VertexPositionColor(translation + Vector3.Transform(Vector3.UnitZ, rotation) * axisLength, Color.Blue));
// Draw name.
if (spriteBatch != null && !string.IsNullOrEmpty(name))
{
// Compute the 3D position in view space. Text is rendered near drawn x axis.
Vector3 textPosition = translation + Vector3.TransformNormal(Vector3.UnitX, bonePose) * axisLength * 0.5f;
var textPositionWorld = Vector3.Transform(textPosition, effect.World);
var textPositionView = Vector3.Transform(textPositionWorld, effect.View);
// Check if the text is in front of the camera.
if (textPositionView.Z < 0)
{
// Project text position to screen.
Vector3 textPositionProjected = graphicsDevice.Viewport.Project(textPosition, effect.Projection, effect.View, effect.World);
spriteBatch.DrawString(spriteFont, name + " " + i, new Vector2(textPositionProjected.X, textPositionProjected.Y), color);
}
}
}
if (spriteBatch != null)
{
spriteBatch.End();
}
// Draw axis lines in one batch.
graphicsDevice.DepthStencilState = DepthStencilState.None;
effect.CurrentTechnique.Passes[0].Apply();
graphicsDevice.DrawUserPrimitives(PrimitiveType.LineList, vertices.ToArray(), 0, vertices.Count / 2);
effect.VertexColorEnabled = oldVertexColorEnabled;
}
/// <summary>
/// Applies the weight to a given bone by blending the bone transforms.
/// </summary>
/// <param name="originalTransform">
/// In: The original bone transform.
/// </param>
/// <param name="targetTransform">
/// In: The target bone transform.<br/>
/// Out: The blended bone transform.
/// </param>
internal void BlendBoneTransform(ref SrtTransform originalTransform, ref SrtTransform targetTransform)
{
targetTransform = SrtTransform.Interpolate(originalTransform, targetTransform, Weight);
}
/// <summary>
/// Sets the bone transform to create a desired pose in model space.
/// </summary>
/// <param name="skeletonPose">The skeleton pose.</param>
/// <param name="boneIndex">The index of the bone.</param>
/// <param name="bonePoseAbsolute">The bone pose in model space.</param>
/// <exception cref="ArgumentNullException">
/// <paramref name="skeletonPose" /> is <see langword="null"/>.
/// </exception>
public static void SetBonePoseAbsolute(this SkeletonPose skeletonPose, int boneIndex, SrtTransform bonePoseAbsolute)
{
if (skeletonPose == null)
{
throw new ArgumentNullException("skeletonPose");
}
var skeleton = skeletonPose.Skeleton;
var bindPoseRelative = skeleton.GetBindPoseRelative(boneIndex);
var parentIndex = skeleton.GetParent(boneIndex);
var parentBonePoseAbsolute = (parentIndex >= 0) ? skeletonPose.GetBonePoseAbsolute(parentIndex) : SrtTransform.Identity;
// Solving this equation:
// bonePoseAbsolute = parentBonePoseAbsolute * bindPoseRelative * BoneTransform;
var boneTransform = bindPoseRelative.Inverse * parentBonePoseAbsolute.Inverse * bonePoseAbsolute;
boneTransform.Rotation.Normalize();
skeletonPose.SetBoneTransform(boneIndex, boneTransform);
}
/// <exception cref="AnimationException">
/// This animation is not frozen. <see cref="Freeze"/> must be called before the animation can
/// be used.
/// </exception>
/// <exception cref="ArgumentNullException">
/// <paramref name="defaultSource"/>, <paramref name="defaultTarget"/> or
/// <paramref name="result"/> is <see langword="null"/>.
/// </exception>
/// <inheritdoc/>
public void GetValue(TimeSpan time, ref SkeletonPose defaultSource, ref SkeletonPose defaultTarget, ref SkeletonPose result)
{
if (!IsFrozen)
{
throw new AnimationException("This animation is not frozen. Freeze() must be called before the animation can be used.");
}
if (defaultSource == null)
{
throw new ArgumentNullException("defaultSource");
}
if (defaultTarget == null)
{
throw new ArgumentNullException("defaultTarget");
}
if (result == null)
{
throw new ArgumentNullException("result");
}
TimeSpan?animationTime = GetAnimationTime(time);
if (animationTime == null)
{
// Animation is inactive and does not produce any output.
if (defaultSource == result)
{
return;
}
// Copy bone transforms of defaultSource to result for the animated channels.
for (int i = 0; i < _channels.Length; i++)
{
int boneIndex = _channels[i];
if (boneIndex >= result.BoneTransforms.Length)
{
break;
}
result.BoneTransforms[boneIndex] = defaultSource.BoneTransforms[boneIndex];
}
return;
}
time = animationTime.Value;
// Clamp time to allowed range.
var startTime = _times[0];
var endTime = _totalDuration;
if (time < startTime)
{
time = startTime;
}
if (time > endTime)
{
time = endTime;
}
int timeIndex = GetTimeIndex(time);
if (!IsAdditive)
{
// Evaluate animation.
for (int channelIndex = 0; channelIndex < _channels.Length; channelIndex++)
{
int boneIndex = _channels[channelIndex];
if (boneIndex >= result.BoneTransforms.Length)
{
break;
}
Debug.Assert(((Array)_keyFrames[channelIndex]).Length > 0, "Each channel must have at least 1 key frame.");
float weight = _weights[channelIndex];
if (weight == 0 && defaultSource != result)
{
// This channel is inactive.
result.BoneTransforms[boneIndex] = defaultSource.BoneTransforms[boneIndex];
}
else if (weight == 1)
{
// Channel is fully active.
result.BoneTransforms[boneIndex] = GetBoneTransform(channelIndex, timeIndex, time);
}
else
{
// Mix channel with source.
SrtTransform boneTransform = GetBoneTransform(channelIndex, timeIndex, time);
SrtTransform.Interpolate(ref defaultSource.BoneTransforms[boneIndex], ref boneTransform, weight, ref boneTransform);
result.BoneTransforms[boneIndex] = boneTransform;
}
}
}
else
{
// Additive animation.
for (int channelIndex = 0; channelIndex < _channels.Length; channelIndex++)
{
int boneIndex = _channels[channelIndex];
if (boneIndex >= result.BoneTransforms.Length)
{
break;
}
Debug.Assert(((Array)_keyFrames[channelIndex]).Length > 0, "Each channel must have at least 1 key frame.");
float weight = _weights[channelIndex];
if (weight == 0 && defaultSource != result)
{
// Channel is inactive.
result.BoneTransforms[boneIndex] = defaultSource.BoneTransforms[boneIndex];
}
else if (weight == 1)
{
// Channel is fully active.
result.BoneTransforms[boneIndex] = defaultSource.BoneTransforms[boneIndex] * GetBoneTransform(channelIndex, timeIndex, time);
}
else
{
// Add only a part of this animation value.
SrtTransform boneTransform = GetBoneTransform(channelIndex, timeIndex, time);
SrtTransform identity = SrtTransform.Identity;
SrtTransform.Interpolate(ref identity, ref boneTransform, weight, ref boneTransform);
result.BoneTransforms[boneIndex] = defaultSource.BoneTransforms[boneIndex] * boneTransform;
}
}
}
}
/// <summary>
/// Gets one bone key frame for a given channel and key frame index.
/// </summary>
/// <param name="channelIndex">The index in <see cref="_channels"/>.</param>
/// <param name="keyFrameIndex">
/// The index in <see cref="_keyFrames"/> for the given channel.
/// </param>
/// <param name="time">The key frame time.</param>
/// <param name="transform">The transform.</param>
private void GetBoneKeyFrame(int channelIndex, int keyFrameIndex, out TimeSpan time, out SrtTransform transform)
{
var boneKeyFrameType = _keyFrameTypes[channelIndex];
if (boneKeyFrameType == BoneKeyFrameType.R)
{
var keyFrames = (BoneKeyFrameR[])_keyFrames[channelIndex];
var keyFrame = keyFrames[keyFrameIndex];
time = keyFrame.Time;
transform.Scale = Vector3.One;
transform.Rotation = keyFrame.Rotation;
transform.Translation = Vector3.Zero;
}
else if (boneKeyFrameType == BoneKeyFrameType.RT)
{
var keyFrames = (BoneKeyFrameRT[])_keyFrames[channelIndex];
var keyFrame = keyFrames[keyFrameIndex];
time = keyFrame.Time;
transform.Scale = Vector3.One;
transform.Rotation = keyFrame.Rotation;
transform.Translation = keyFrame.Translation;
}
else
{
var keyFrames = (BoneKeyFrameSRT[])_keyFrames[channelIndex];
var keyFrame = keyFrames[keyFrameIndex];
time = keyFrame.Time;
transform = keyFrame.Transform;
}
}