/// <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);
}
/// <inheritdoc/>
/// <exception cref="ArgumentNullException">
/// <paramref name="source"/>, <paramref name="target"/> or <paramref name="result"/> is
/// <see langword="null"/>.
/// </exception>
public void Interpolate(ref SkeletonPose source, ref SkeletonPose target, float parameter, ref SkeletonPose result)
{
if (source == null)
{
throw new ArgumentNullException("source");
}
if (target == null)
{
throw new ArgumentNullException("target");
}
if (result == null)
{
throw new ArgumentNullException("result");
}
var sourceTransforms = source.BoneTransforms;
var targetTransforms = target.BoneTransforms;
var resultTransforms = result.BoneTransforms;
for (int i = 0; i < resultTransforms.Length; i++)
{
SrtTransform.Interpolate(ref sourceTransforms[i], ref targetTransforms[i], parameter, ref resultTransforms[i]);
}
result.Invalidate();
}
/// <inheritdoc/>
public void BlendNext(ref SrtTransform value, ref SrtTransform nextValue, float normalizedWeight)
{
var rotation = value.Rotation;
var nextRotation = nextValue.Rotation;
// Get angle between quaternions:
//float cosθ = Quaternion.Dot(rotation, nextRotation);
float cosθ = rotation.W * nextRotation.W + rotation.X * nextRotation.X + rotation.Y * nextRotation.Y + rotation.Z * nextRotation.Z;
// Invert one quaternion if we would move along the long arc of interpolation.
if (cosθ < 0)
{
// Blend with inverted quaternion!
rotation.W = rotation.W - normalizedWeight * nextRotation.W;
rotation.X = rotation.X - normalizedWeight * nextRotation.X;
rotation.Y = rotation.Y - normalizedWeight * nextRotation.Y;
rotation.Z = rotation.Z - normalizedWeight * nextRotation.Z;
}
else
{
// Blend with normal quaternion.
rotation.W = rotation.W + normalizedWeight * nextRotation.W;
rotation.X = rotation.X + normalizedWeight * nextRotation.X;
rotation.Y = rotation.Y + normalizedWeight * nextRotation.Y;
rotation.Z = rotation.Z + normalizedWeight * nextRotation.Z;
}
value.Rotation = rotation;
value.Scale += normalizedWeight * nextValue.Scale;
value.Translation += normalizedWeight * nextValue.Translation;
}
public void BlendTest()
{
var traits = SrtTransformTraits.Instance;
var value0 = NextRandomValue();
var value1 = NextRandomValue();
var value2 = NextRandomValue();
var w0 = 0.3f;
var w1 = 0.4f;
var w2 = 1 - w0 - w1;
SrtTransform result = new SrtTransform();
traits.BeginBlend(ref result);
traits.BlendNext(ref result, ref value0, w0);
traits.BlendNext(ref result, ref value1, w1);
traits.BlendNext(ref result, ref value2, w2);
traits.EndBlend(ref result);
Assert.IsTrue(Vector3F.AreNumericallyEqual(value0.Scale * w0 + value1.Scale * w1 + value2.Scale * w2, result.Scale));
Assert.IsTrue(Vector3F.AreNumericallyEqual(value0.Translation * w0 + value1.Translation * w1 + value2.Translation * w2, result.Translation));
QuaternionF expected;
expected = value0.Rotation * w0;
// Consider "selective negation" when blending quaternions!
if (QuaternionF.Dot(expected, value1.Rotation) < 0)
value1.Rotation = -value1.Rotation;
expected += value1.Rotation * w1;
if (QuaternionF.Dot(expected, value2.Rotation) < 0)
value2.Rotation = -value2.Rotation;
expected += value2.Rotation * w2;
expected.Normalize();
Assert.IsTrue(QuaternionF.AreNumericallyEqual(expected, result.Rotation));
}
/// <inheritdoc/>
public void Multiply(ref SrtTransform value, int factor, ref SrtTransform result)
{
if (factor == 0)
{
result = SrtTransform.Identity;
return;
}
SrtTransform srt;
if (factor < 0)
{
srt = value.Inverse;
factor = -factor;
}
else
{
srt = value;
}
result = srt;
for (int i = 1; i < factor; i++)
{
result = srt * result;
}
}
public void CompressEmptySrtKeyFrameAnimation1()
{
// Animation with 1 keyframe, which is Identity.
var srtKeyFrameAnimation = new SrtKeyFrameAnimation();
var time = TimeSpan.FromTicks(100000);
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame<SrtTransform>(time, SrtTransform.Identity));
var srtAnimation = AnimationHelper.Compress(srtKeyFrameAnimation, 0, 0, 0);
Assert.IsNotNull(srtAnimation);
Assert.AreEqual(srtKeyFrameAnimation.GetTotalDuration(), srtAnimation.GetTotalDuration());
var defaultSource = SrtTransform.Identity;
var defaultTarget = SrtTransform.Identity;
var result = new SrtTransform();
srtAnimation.GetValue(time, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(srtKeyFrameAnimation.KeyFrames[0].Value, result);
// Only 1 channel is needed.
int numberOfChannels = 0;
if (srtAnimation.Scale != null)
numberOfChannels++;
if (srtAnimation.Rotation != null)
numberOfChannels++;
if (srtAnimation.Translation != null)
numberOfChannels++;
Assert.AreEqual(1, numberOfChannels);
}
public override void Update(GameTime gameTime)
{
float deltaTime = (float)gameTime.ElapsedGameTime.TotalSeconds;
// Change rotation angle.
if (_moveArmDown)
_upperArmAngle -= 0.3f * deltaTime;
else
_upperArmAngle += 0.3f * deltaTime;
// Change direction when a certain angle is reached.
if (Math.Abs(_upperArmAngle) > 0.5f)
_moveArmDown = !_moveArmDown;
// Get the bone index of the upper arm bone.
int upperArmIndex = _skeletonPose.Skeleton.GetIndex("L_UpperArm");
// Define the desired bone transform.
SrtTransform boneTransform = new SrtTransform(QuaternionF.CreateRotationY(_upperArmAngle));
// Set the new bone transform.
_skeletonPose.SetBoneTransform(upperArmIndex, boneTransform);
// The class SkeletonHelper provides some useful extension methods.
// One is SetBoneRotationAbsolute() which sets the orientation of a bone relative
// to model space.
int handIndex = _skeletonPose.Skeleton.GetIndex("L_Hand");
SkeletonHelper.SetBoneRotationAbsolute(_skeletonPose, handIndex, QuaternionF.CreateRotationX(ConstantsF.Pi));
base.Update(gameTime);
}
/// <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
});
}
public void CompressEmptySrtKeyFrameAnimation4()
{
var random = new Random(12345);
// Animation with 2 keyframe, which are not Identity.
var srtKeyFrameAnimation = new SrtKeyFrameAnimation();
var time0 = TimeSpan.FromTicks(100000);
var value0 = new SrtTransform(random.NextVector3F(-2, 2), random.NextQuaternionF(), random.NextVector3F(-10, 10));
var time1 = TimeSpan.FromTicks(200000);
var value1 = new SrtTransform(random.NextVector3F(-2, 2), random.NextQuaternionF(), random.NextVector3F(-10, 10));
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame <SrtTransform>(time0, value0));
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame <SrtTransform>(time1, value1));
var srtAnimation = AnimationHelper.Compress(srtKeyFrameAnimation, 2, 360, 10);
Assert.IsNotNull(srtAnimation);
Assert.AreEqual(srtKeyFrameAnimation.GetTotalDuration(), srtAnimation.GetTotalDuration());
var defaultSource = SrtTransform.Identity;
var defaultTarget = SrtTransform.Identity;
var result = new SrtTransform();
srtAnimation.GetValue(time0, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(srtKeyFrameAnimation.KeyFrames[0].Value, result);
srtAnimation.GetValue(time1, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(srtKeyFrameAnimation.KeyFrames[1].Value, result);
}
public void InterpolateTest()
{
SrtTransform a = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
SrtTransform b = a;
var c = SrtTransform.Interpolate(a, b, 0.5f);
Assert.AreEqual(a, c);
b = new SrtTransform(new Vector3F(7, 9, 8), new QuaternionF(6, 6, 4, 2).Normalized, new Vector3F(-2, 4, -9));
c = SrtTransform.Interpolate(a, b, 0);
Assert.AreEqual(a, c);
c = SrtTransform.Interpolate(a, b, 1);
Assert.AreEqual(b, c);
c = SrtTransform.Interpolate(a, b, 0.3f);
Assert.AreEqual(a.Translation * 0.7f + b.Translation * 0.3f, c.Translation);
Assert.AreEqual(a.Scale * 0.7f + b.Scale * 0.3f, c.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(
new QuaternionF(
a.Rotation.W * 0.7f + b.Rotation.W * 0.3f,
a.Rotation.V * 0.7f + b.Rotation.V * 0.3f).Normalized,
c.Rotation));
}
/// <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 (QuaternionF.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
}
}
}
public void MultiplyWithUniformScaleIsAssociative()
{
var a = new SrtTransform(new Vector3F(2), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3), new QuaternionF(3, -2, 1, 9).Normalized, new Vector3F(7, -4, 2));
var c = new SrtTransform(new Vector3F(4), new QuaternionF(7, -5, 3, 1).Normalized, new Vector3F(-8, -1, -7));
// Assocative for uniform scale
Assert.IsTrue(SrtTransform.AreNumericallyEqual((a * b) * c, a * (b * c)));
}
public void MultiplyWithScaleWithoutRotationIsAssociative()
{
var a = new SrtTransform(new Vector3F(2), QuaternionF.Identity, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3), QuaternionF.Identity, new Vector3F(7, -4, 2));
var c = new SrtTransform(new Vector3F(4), QuaternionF.Identity, new Vector3F(-8, -1, -7));
// Assocative for uniform scale
Assert.IsTrue(SrtTransform.AreNumericallyEqual((a * b) * c, a * (b * c)));
}
public void ToParentPosition()
{
var a = new SrtTransform(new Vector3(1, 2, 7), new Quaternion(1, 2, 3, 4).Normalized, new Vector3(4, -5, 6));
var v = new Vector3(7, 9, -12);
var result1 = a.ToParentPosition(v);
var result2 = a.ToMatrix().TransformPosition(v);
Assert.IsTrue(Vector3.AreNumericallyEqual(result1, result2));
}
public void IsValidTest()
{
var m = Matrix44F.CreateTranslation(1, 2, 3) * Matrix44F.CreateRotationY(0.1f) * Matrix44F.CreateScale(-2, 3, 4);
Assert.IsTrue(SrtTransform.IsValid(m));
// Concatenating to SRTs creates a skew.
m = Matrix44F.CreateRotationZ(0.1f) * Matrix44F.CreateScale(-2, 3, 4) * m;
Assert.IsFalse(SrtTransform.IsValid(m));
}
public void Multiply()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3, 9, -2), new QuaternionF(3, -2, 1, 9).Normalized, new Vector3F(7, -4, 2));
var result1 = SrtTransform.Multiply(a, b).ToMatrix44F();
var result2 = a * b;
Assert.IsTrue(Matrix44F.AreNumericallyEqual(result1, result2));
}
public void ToLocalDirection()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var v = new Vector3F(7, 9, -12);
var result1 = a.ToLocalDirection(v);
var result2 = a.Rotation.ToRotationMatrix33().Transposed *v;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
}
public void ToLocalPosition()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var v = new Vector3F(7, 9, -12);
var result1 = a.ToLocalPosition(v);
var result2 = a.ToMatrix44F().Inverse.TransformPosition(v);
Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
}
public void MultiplyWithoutRotationIsTheSameAsMatrixMultiply()
{
// Result is the same as Matrix mulitiplication without scale.
var a = new SrtTransform(new Vector3F(1, 2, 3), QuaternionF.Identity, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(5, 6, -3), QuaternionF.Identity, new Vector3F(7, -4, 2));
var result1 = (a * b).ToMatrix44F();
var result2 = a.ToMatrix44F() * b.ToMatrix44F();
Assert.IsTrue(Matrix44F.AreNumericallyEqual(result1, result2));
}
protected override void GetValueCore(TimeSpan time, ref SkeletonPose defaultSource, ref SkeletonPose defaultTarget, ref SkeletonPose result)
{
// Update time of the AvatarAnimation.
_avatarAnimation.CurrentPosition = time;
// Get bone transforms and convert to type SrtTransform.
for (int i = 0; i < AvatarRenderer.BoneCount; i++)
{
result.SetBoneTransform(i, SrtTransform.FromMatrix(_avatarAnimation.BoneTransforms[i]));
}
}
public void InterpolationTest()
{
var traits = SrtTransformTraits.Instance;
var value0 = NextRandomValue();
var value1 = NextRandomValue();
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value0, traits.Interpolate(value0, value1, 0.0f)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value1, traits.Interpolate(value0, value1, 1.0f)) ||
SrtTransform.AreNumericallyEqual(new SrtTransform(value1.Scale, -value1.Rotation, value1.Translation), traits.Interpolate(value0, value1, 1.0f)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(SrtTransform.Interpolate(value0, value1, 0.75f), traits.Interpolate(value0, value1, 0.75f)));
}
public void MultiplyWithUniformScaleIsTheSameAsMatrixMultiply()
{
// Result is the same as Matrix mulitiplication without scale.
var a = new SrtTransform(new Vector3F(7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3), new QuaternionF(3, -2, 1, 9).Normalized, new Vector3F(7, -4, 2));
var result1 = (a * b).ToMatrix44F();
var result2 = a.ToMatrix44F() * b.ToMatrix44F();
Assert.IsTrue(Matrix44F.AreNumericallyEqual(result1, result2));
}
/// <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 = Vector3F.One;
transform0.Rotation = keyFrame.Rotation;
transform0.Translation = Vector3F.Zero;
keyFrame = keyFrames[keyFrameIndex + 1];
time1 = keyFrame.Time;
transform1.Scale = Vector3F.One;
transform1.Rotation = keyFrame.Rotation;
transform1.Translation = Vector3F.Zero;
}
else if (boneKeyFrameType == BoneKeyFrameType.RT)
{
var keyFrames = (BoneKeyFrameRT[])_keyFrames[channelIndex];
var keyFrame = keyFrames[keyFrameIndex];
time0 = keyFrame.Time;
transform0.Scale = Vector3F.One;
transform0.Rotation = keyFrame.Rotation;
transform0.Translation = keyFrame.Translation;
keyFrame = keyFrames[keyFrameIndex + 1];
time1 = keyFrame.Time;
transform1.Scale = Vector3F.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);
}
public void HasRotationTest()
{
var srt = new SrtTransform(new Vector3F(1, 1, 1), QuaternionF.Identity, Vector3F.Zero);
Assert.IsFalse(srt.HasRotation);
srt.Rotation = QuaternionF.CreateRotationX(0.000001f);
Assert.IsFalse(srt.HasRotation);
srt.Rotation = QuaternionF.CreateRotationX(0.1f);
Assert.IsTrue(srt.HasRotation);
}
private void InitializeSkeletonPoses()
{
// Create a list of the bone/joint names of a Kinect skeleton.
int numberOfJoints = Enum.GetNames(typeof(JointType)).Length;
var boneNames = new string[numberOfJoints];
for (int i = 0; i < numberOfJoints; i++)
{
boneNames[i] = ((JointType)i).ToString();
}
// Create list with one entry per bone. Each entry is the index of the parent bone.
var boneParents = new[]
{
-1,
(int)JointType.HipCenter,
(int)JointType.Spine,
(int)JointType.ShoulderCenter,
(int)JointType.ShoulderCenter,
(int)JointType.ShoulderLeft,
(int)JointType.ElbowLeft,
(int)JointType.WristLeft,
(int)JointType.ShoulderCenter,
(int)JointType.ShoulderRight,
(int)JointType.ElbowRight,
(int)JointType.WristRight,
(int)JointType.HipCenter,
(int)JointType.HipLeft,
(int)JointType.KneeLeft,
(int)JointType.AnkleLeft,
(int)JointType.HipCenter,
(int)JointType.HipRight,
(int)JointType.KneeRight,
(int)JointType.AnkleRight,
};
// Create a list of the bind pose transformations of all bones. Since we do not
// get such information from Kinect, we position all bones in the local origin.
var boneBindPoses = new SrtTransform[numberOfJoints];
for (int i = 0; i < numberOfJoints; i++)
{
boneBindPoses[i] = SrtTransform.Identity;
}
// Create a skeleton that defines the bone hierarchy and rest pose.
var skeleton = new DRSkeleton(boneParents, boneNames, boneBindPoses);
// Create a SkeletonPose for each player.
SkeletonPoseA = SkeletonPose.Create(skeleton);
SkeletonPoseB = SkeletonPose.Create(skeleton);
}
public void MultiplyTest()
{
var traits = SrtTransformTraits.Instance;
var value = NextRandomValue();
Assert.IsTrue(SrtTransform.AreNumericallyEqual(SrtTransform.Identity, traits.Multiply(value, 0)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value, traits.Multiply(value, 1)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value * value, traits.Multiply(value, 2)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value * value * value, traits.Multiply(value, 3)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value.Inverse, traits.Multiply(value, -1)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value.Inverse * value.Inverse, traits.Multiply(value, -2)));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(value.Inverse * value.Inverse * value.Inverse, traits.Multiply(value, -3)));
}
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);
}
public void ToPose()
{
var srt = new SrtTransform(new Vector3F(4, 5, 6), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(1, 2, 3));
var pose = srt.ToPose();
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
srt = new SrtTransform(new Vector3F(4, 5, 6), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(1, 2, 3));
pose = (Pose)srt;
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
}
private void BuildSkeleton()
{
// Get an array of all bones in depth-first order.
// (Same as MeshHelper.FlattenSkeleton(root).)
var bones = TreeHelper.GetSubtree(_rootBone, n => n.Children.OfType <BoneContent>(), true)
.ToList();
// Create list of parent indices, bind pose transformations and bone names.
var boneParents = new List <int>();
var bindTransforms = new List <SrtTransform>();
var boneNames = new List <string>();
int numberOfWarnings = 0;
foreach (var bone in bones)
{
int parentIndex = bones.IndexOf(bone.Parent as BoneContent);
boneParents.Add(parentIndex);
// Log warning for invalid transform matrices - but not too many warnings.
if (numberOfWarnings < 2)
{
if (!SrtTransform.IsValid((Matrix)bone.Transform))
{
if (numberOfWarnings < 1)
{
_context.Logger.LogWarning(null, _input.Identity, "Bone transform is not supported. Bone transform matrices may only contain scaling, rotation and translation.");
}
else
{
_context.Logger.LogWarning(null, _input.Identity, "More unsupported bone transform found.");
}
numberOfWarnings++;
}
}
bindTransforms.Add(SrtTransform.FromMatrix(bone.Transform));
if (boneNames.Contains(bone.Name))
{
string message = String.Format(CultureInfo.InvariantCulture, "Duplicate bone name (\"{0}\") found.", bone.Name);
throw new InvalidContentException(message, _input.Identity);
}
boneNames.Add(bone.Name);
}
// Create and return a new skeleton instance.
_skeleton = new Skeleton(boneParents, boneNames, bindTransforms);
}
public void MultiplyVector4()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var v = new Vector4F(7, 9, -12, -2);
var result1 = a * v;
var result2 = a.ToMatrix44F() * v;
Assert.IsTrue(Vector4F.AreNumericallyEqual(result1, result2));
result1 = SrtTransform.Multiply(a, v);
result2 = a.ToMatrix44F() * v;
Assert.IsTrue(Vector4F.AreNumericallyEqual(result1, result2));
}
public void FromPose()
{
var pose = new Pose(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized);
var srt = SrtTransform.FromPose(pose);
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
pose = new Pose(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized);
srt = pose;
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
}
public void FromByTest()
{
// IAnimationValueTraits<T> is used in a from-by animation to a add a relative offset to
// the start value.
var traits = SrtTransformTraits.Instance;
var from = NextRandomValue();
var by = NextRandomValue();
var to = traits.Add(from, by);
Assert.IsTrue(SrtTransform.AreNumericallyEqual(by * from, to));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(from, traits.Add(to, traits.Inverse(by))));
Assert.IsTrue(SrtTransform.AreNumericallyEqual(by, traits.Add(traits.Inverse(from), to)));
}
public void ConstructorTest()
{
var rotationQ = new QuaternionF(1, 2, 3, 4).Normalized;
var srt = new SrtTransform(rotationQ.ToRotationMatrix33());
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(Vector3F.Zero, srt.Translation);
srt = new SrtTransform(rotationQ);
Assert.AreEqual(Vector3F.One, srt.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(Vector3F.Zero, srt.Translation);
srt = new SrtTransform(new Vector3F(-1, 2, -3), rotationQ.ToRotationMatrix33(), new Vector3F(10, 9, -8));
Assert.AreEqual(new Vector3F(-1, 2, -3), srt.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(rotationQ, srt.Rotation));
Assert.AreEqual(new Vector3F(10, 9, -8), srt.Translation);
}
public void MultiplyWithoutRotationIsTheSameAsMatrixMultiply()
{
// Result is the same as Matrix mulitiplication without scale.
var a = new SrtTransform(new Vector3F(1, 2, 3), QuaternionF.Identity, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(5, 6, -3), QuaternionF.Identity, new Vector3F(7, -4, 2));
var result1 = (a * b).ToMatrix44F();
var result2 = a.ToMatrix44F() * b.ToMatrix44F();
Assert.IsTrue(Matrix44F.AreNumericallyEqual(result1, result2));
}
public void ToParentPosition()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var v = new Vector3F(7, 9, -12);
var result1 = a.ToParentPosition(v);
var result2 = a.ToMatrix44F().TransformPosition(v);
Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <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();
}
private void InitializeSkeletonPoses()
{
// Create a list of the bone/joint names of a Kinect skeleton.
int numberOfJoints = Enum.GetNames(typeof(JointType)).Length;
var boneNames = new string[numberOfJoints];
for (int i = 0; i < numberOfJoints; i++)
boneNames[i] = ((JointType)i).ToString();
// Create list with one entry per bone. Each entry is the index of the parent bone.
var boneParents = new[]
{
-1,
(int)JointType.HipCenter,
(int)JointType.Spine,
(int)JointType.ShoulderCenter,
(int)JointType.ShoulderCenter,
(int)JointType.ShoulderLeft,
(int)JointType.ElbowLeft,
(int)JointType.WristLeft,
(int)JointType.ShoulderCenter,
(int)JointType.ShoulderRight,
(int)JointType.ElbowRight,
(int)JointType.WristRight,
(int)JointType.HipCenter,
(int)JointType.HipLeft,
(int)JointType.KneeLeft,
(int)JointType.AnkleLeft,
(int)JointType.HipCenter,
(int)JointType.HipRight,
(int)JointType.KneeRight,
(int)JointType.AnkleRight,
};
// Create a list of the bind pose transformations of all bones. Since we do not
// get such information from Kinect, we position all bones in the local origin.
var boneBindPoses = new SrtTransform[numberOfJoints];
for (int i = 0; i < numberOfJoints; i++)
boneBindPoses[i] = SrtTransform.Identity;
// Create a skeleton that defines the bone hierarchy and rest pose.
var skeleton = new DRSkeleton(boneParents, boneNames, boneBindPoses);
// Create a SkeletonPose for each player.
SkeletonPoseA = SkeletonPose.Create(skeleton);
SkeletonPoseB = SkeletonPose.Create(skeleton);
}
public void ToPose()
{
var srt = new SrtTransform(new Vector3F(4, 5, 6), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(1, 2, 3));
var pose = srt.ToPose();
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
srt = new SrtTransform(new Vector3F(4, 5, 6), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(1, 2, 3));
pose = (Pose)srt;
Assert.AreEqual(pose.Position, srt.Translation);
Assert.IsTrue(Matrix33F.AreNumericallyEqual(pose.Orientation, srt.Rotation.ToRotationMatrix33()));
}
public void HasRotationTest()
{
var srt = new SrtTransform(new Vector3F(1, 1, 1), QuaternionF.Identity, Vector3F.Zero);
Assert.IsFalse(srt.HasRotation);
srt.Rotation = QuaternionF.CreateRotationX(0.000001f);
Assert.IsFalse(srt.HasRotation);
srt.Rotation = QuaternionF.CreateRotationX(0.1f);
Assert.IsTrue(srt.HasRotation);
}
/// <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);
}
public void MultiplyWithScaleWithoutRotationIsAssociative()
{
var a = new SrtTransform(new Vector3F(2), QuaternionF.Identity, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3), QuaternionF.Identity, new Vector3F(7, -4, 2));
var c = new SrtTransform(new Vector3F(4), QuaternionF.Identity, new Vector3F(-8, -1, -7));
// Assocative for uniform scale
Assert.IsTrue(SrtTransform.AreNumericallyEqual((a * b) * c, a * (b * c)));
}
public void HasScaleTest()
{
var srt = new SrtTransform(new Vector3F(1, 1, 1), QuaternionF.Identity, Vector3F.Zero);
Assert.IsFalse(srt.HasScale);
srt.Scale = new Vector3F(1.00001f, 1.000001f, 1.000001f);
Assert.IsFalse(srt.HasScale);
srt.Scale = new Vector3F(1.1f, 1, 1);
Assert.IsTrue(srt.HasScale);
srt.Scale = new Vector3F(1, 1.1f, 1);
Assert.IsTrue(srt.HasScale);
srt.Scale = new Vector3F(1, 1, 1.1f);
Assert.IsTrue(srt.HasScale);
srt.Scale = new Vector3F(1.1f);
Assert.IsTrue(srt.HasScale);
}
public void ToParentDirection()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var v = new Vector3F(7, 9, -12);
var result1 = a.ToParentDirection(v);
var result2 = a.Rotation.ToRotationMatrix33() * v;
Assert.IsTrue(Vector3F.AreNumericallyEqual(result1, result2));
}
public void HasTranslationTest()
{
var srt = new SrtTransform(QuaternionF.Identity, Vector3F.Zero);
Assert.IsFalse(srt.HasTranslation);
srt.Translation = new Vector3F(0.000001f, 0.000001f, 0.000001f);
Assert.IsFalse(srt.HasTranslation);
srt.Translation = new Vector3F(1.1f, 0, 0);
Assert.IsTrue(srt.HasTranslation);
srt.Translation = new Vector3F(0, 1.1f, 0);
Assert.IsTrue(srt.HasTranslation);
srt.Translation = new Vector3F(0, 0, 1.1f);
Assert.IsTrue(srt.HasTranslation);
srt.Translation = new Vector3F(1.1f);
Assert.IsTrue(srt.HasTranslation);
}
public void Update(float deltaTime, Matrix44F 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(QuaternionF.CreateRotation(Offset, particleLocal));
SkeletonPose.SetBoneTransform(BoneIndex, boneTransform);
}
public void EqualsTest()
{
var a = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
var b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(((object)a).Equals(3));
Assert.IsTrue(a.Equals(b));
Assert.IsTrue(((object)a).Equals(b));
Assert.IsTrue(a == b);
Assert.IsFalse(a != b);
Assert.AreEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 22, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 33), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(11, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(11, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 22, 3, 4).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 33, 4).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 44).Normalized, new Vector3F(4, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(44, 5, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 55, 6));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
b = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 66));
Assert.IsFalse(a.Equals(b));
Assert.IsFalse(((object)a).Equals(b));
Assert.IsFalse(a == b);
Assert.IsTrue(a != b);
Assert.AreNotEqual(a.GetHashCode(), b.GetHashCode());
}
//--------------------------------------------------------------
/// <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).
Vector3F forward = Forward;
Vector3F up = Up;
Vector3F side = Vector3F.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 Matrix33F(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 = Vector3F.Dot(targetDirection, up);
float targetSide = Vector3F.Dot(targetDirection, side);
float targetForward = Vector3F.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 = Vector3F.Cross(up, -forward);
if (!side.TryNormalize())
return;
up = Vector3F.Cross(side, forward);
Debug.Assert(up.IsNumericallyNormalized);
// Create new view space matrix.
var boneFromNewView = new Matrix33F(
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.
QuaternionF boneTransform = QuaternionF.CreateRotation(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);
}
public void MultiplyWithUniformScaleIsAssociative()
{
var a = new SrtTransform(new Vector3F(2), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3), new QuaternionF(3, -2, 1, 9).Normalized, new Vector3F(7, -4, 2));
var c = new SrtTransform(new Vector3F(4), new QuaternionF(7, -5, 3, 1).Normalized, new Vector3F(-8, -1, -7));
// Assocative for uniform scale
Assert.IsTrue(SrtTransform.AreNumericallyEqual((a*b)*c, a*(b*c)));
}
public void MultiplyVector4()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var v = new Vector4F(7, 9, -12, -2);
var result1 = a * v;
var result2 = a.ToMatrix44F() * v;
Assert.IsTrue(Vector4F.AreNumericallyEqual(result1, result2));
result1 = SrtTransform.Multiply(a, v);
result2 = a.ToMatrix44F() * v;
Assert.IsTrue(Vector4F.AreNumericallyEqual(result1, result2));
}
public void Multiply()
{
var a = new SrtTransform(new Vector3F(1, 2, 7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3, 9, -2), new QuaternionF(3, -2, 1, 9).Normalized, new Vector3F(7, -4, 2));
var result1 = SrtTransform.Multiply(a, b).ToMatrix44F();
var result2 = a * b;
Assert.IsTrue(Matrix44F.AreNumericallyEqual(result1, result2));
}
public void InverseTest()
{
var identity = SrtTransform.Identity;
var a = new SrtTransform(new Vector3F(-2, -2, -2), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var aInverse = a.Inverse;
var aa = a * aInverse;
Assert.IsTrue(SrtTransform.AreNumericallyEqual(identity, aa));
aa = aInverse * a;
Assert.IsTrue(SrtTransform.AreNumericallyEqual(identity, aa));
a = new SrtTransform(new Vector3F(-3, 7, -4), QuaternionF.Identity, new Vector3F(4, -5, 6));
aInverse = a.Inverse;
aa = a * aInverse;
Assert.IsTrue(SrtTransform.AreNumericallyEqual(identity, aa));
aa = aInverse * a;
Assert.IsTrue(SrtTransform.AreNumericallyEqual(identity, aa));
}
public void CompressEmptySrtKeyFrameAnimation2()
{
var random = new Random(12345);
// Animation with 1 keyframe, which is not Identity.
var srtKeyFrameAnimation = new SrtKeyFrameAnimation();
var time = TimeSpan.FromTicks(100000);
var value = new SrtTransform(random.NextVector3F(-2, 2), random.NextQuaternionF(), random.NextVector3F(-10, 10));
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame<SrtTransform>(time, value));
var srtAnimation = AnimationHelper.Compress(srtKeyFrameAnimation, 2, 360, 10);
Assert.IsNotNull(srtAnimation);
Assert.AreEqual(srtKeyFrameAnimation.GetTotalDuration(), srtAnimation.GetTotalDuration());
var defaultSource = SrtTransform.Identity;
var defaultTarget = SrtTransform.Identity;
var result = new SrtTransform();
srtAnimation.GetValue(time, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(srtKeyFrameAnimation.KeyFrames[0].Value, result);
}
public void InterpolateTest()
{
SrtTransform a = new SrtTransform(new Vector3F(1, 2, 3), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, 5, 6));
SrtTransform b = a;
var c = SrtTransform.Interpolate(a, b, 0.5f);
Assert.AreEqual(a, c);
b = new SrtTransform(new Vector3F(7, 9, 8), new QuaternionF(6, 6, 4, 2).Normalized, new Vector3F(-2, 4, -9));
c = SrtTransform.Interpolate(a, b, 0);
Assert.AreEqual(a, c);
c = SrtTransform.Interpolate(a, b, 1);
Assert.AreEqual(b, c);
c = SrtTransform.Interpolate(a, b, 0.3f);
Assert.AreEqual(a.Translation * 0.7f + b.Translation * 0.3f, c.Translation);
Assert.AreEqual(a.Scale * 0.7f + b.Scale * 0.3f, c.Scale);
Assert.IsTrue(QuaternionF.AreNumericallyEqual(
new QuaternionF(
a.Rotation.W * 0.7f + b.Rotation.W * 0.3f,
a.Rotation.V * 0.7f + b.Rotation.V * 0.3f).Normalized,
c.Rotation));
}
public void CompressSrtKeyFrameAnimation()
{
var random = new Random(12345);
float scaleThreshold = 0.1f;
float rotationThreshold = 2; // [°]
float translationThreshold = 0.2f;
var srtKeyFrameAnimation = new SrtKeyFrameAnimation();
// Define a view important keyframes.
var time0 = TimeSpan.FromTicks(100000);
var value0 = new SrtTransform(Vector3F.One, QuaternionF.Identity, Vector3F.Zero);
var time1 = TimeSpan.FromTicks(200000);
var value1 = new SrtTransform(new Vector3F(2, 2, 2), QuaternionF.CreateRotationX(MathHelper.ToRadians(10)), new Vector3F(1, 1, 1));
var time2 = TimeSpan.FromTicks(400000);
var value2 = new SrtTransform(new Vector3F(-1, -1, -1), QuaternionF.CreateRotationX(MathHelper.ToRadians(80)), new Vector3F(10, 10, 10));
var time3 = TimeSpan.FromTicks(500000);
var value3 = new SrtTransform(new Vector3F(3, 3, 3), QuaternionF.CreateRotationX(MathHelper.ToRadians(-10)), new Vector3F(-2, -2, -2));
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame<SrtTransform>(time0, value0));
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame<SrtTransform>(time1, value1));
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame<SrtTransform>(time2, value2));
srtKeyFrameAnimation.KeyFrames.Add(new KeyFrame<SrtTransform>(time3, value3));
// Add random keyframes within tolerance.
InsertRandomKeyFrames(random, srtKeyFrameAnimation, time0, time1, scaleThreshold, rotationThreshold, translationThreshold);
InsertRandomKeyFrames(random, srtKeyFrameAnimation, time1, time2, scaleThreshold, rotationThreshold, translationThreshold);
InsertRandomKeyFrames(random, srtKeyFrameAnimation, time2, time3, scaleThreshold, rotationThreshold, translationThreshold);
// ---- Compress animation with tolerance.
var srtAnimation = AnimationHelper.Compress(srtKeyFrameAnimation, scaleThreshold, rotationThreshold, translationThreshold);
Assert.IsNotNull(srtAnimation);
Assert.AreEqual(srtKeyFrameAnimation.GetTotalDuration(), srtAnimation.GetTotalDuration());
Assert.IsNotNull(srtAnimation.Scale);
Assert.AreEqual(4, ((KeyFrameAnimation<Vector3F>)srtAnimation.Scale).KeyFrames.Count);
Assert.AreEqual(4, ((KeyFrameAnimation<QuaternionF>)srtAnimation.Rotation).KeyFrames.Count);
Assert.AreEqual(4, ((KeyFrameAnimation<Vector3F>)srtAnimation.Translation).KeyFrames.Count);
var defaultSource = SrtTransform.Identity;
var defaultTarget = SrtTransform.Identity;
var result = new SrtTransform();
srtAnimation.GetValue(time0, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(value0, result);
srtAnimation.GetValue(time1, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(value1, result);
srtAnimation.GetValue(time2, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(value2, result);
srtAnimation.GetValue(time3, ref defaultSource, ref defaultTarget, ref result);
Assert.AreEqual(value3, result);
// Take a view samples.
const int numberOfSamples = 10;
long tickIncrement = (time3 - time0).Ticks / (numberOfSamples + 1);
for (int i = 0; i < numberOfSamples; i++)
{
var time = TimeSpan.FromTicks(time0.Ticks + (i + 1) * tickIncrement);
var valueRef = new SrtTransform();
srtKeyFrameAnimation.GetValue(time, ref defaultSource, ref defaultTarget, ref valueRef);
var valueNew = new SrtTransform();
srtAnimation.GetValue(time, ref defaultSource, ref defaultTarget, ref valueNew);
Assert.IsTrue((valueRef.Scale - valueNew.Scale).Length <= scaleThreshold);
Assert.IsTrue(QuaternionF.GetAngle(valueRef.Rotation, valueNew.Rotation) <= MathHelper.ToRadians(rotationThreshold));
Assert.IsTrue((valueRef.Translation - valueNew.Translation).Length <= translationThreshold);
}
// ----- Compress animation with zero tolerance.
srtAnimation = AnimationHelper.Compress(srtKeyFrameAnimation, 0, 0, 0);
Assert.IsNotNull(srtAnimation);
Assert.AreEqual(srtKeyFrameAnimation.GetTotalDuration(), srtAnimation.GetTotalDuration());
Assert.IsNotNull(srtAnimation.Scale);
Assert.AreEqual(srtKeyFrameAnimation.KeyFrames.Count, ((KeyFrameAnimation<Vector3F>)srtAnimation.Scale).KeyFrames.Count);
Assert.AreEqual(srtKeyFrameAnimation.KeyFrames.Count, ((KeyFrameAnimation<QuaternionF>)srtAnimation.Rotation).KeyFrames.Count);
Assert.AreEqual(srtKeyFrameAnimation.KeyFrames.Count, ((KeyFrameAnimation<Vector3F>)srtAnimation.Translation).KeyFrames.Count);
// Take a view samples.
for (int i = 0; i < numberOfSamples; i++)
{
var time = TimeSpan.FromTicks(time0.Ticks + (i + 1) * tickIncrement);
var valueRef = new SrtTransform();
srtKeyFrameAnimation.GetValue(time, ref defaultSource, ref defaultTarget, ref valueRef);
var valueNew = new SrtTransform();
srtAnimation.GetValue(time, ref defaultSource, ref defaultTarget, ref valueNew);
Assert.IsTrue(SrtTransform.AreNumericallyEqual(valueRef, valueNew));
}
}
public void MultiplyWithUniformScaleIsTheSameAsMatrixMultiply()
{
// Result is the same as Matrix mulitiplication without scale.
var a = new SrtTransform(new Vector3F(7), new QuaternionF(1, 2, 3, 4).Normalized, new Vector3F(4, -5, 6));
var b = new SrtTransform(new Vector3F(-3), new QuaternionF(3, -2, 1, 9).Normalized, new Vector3F(7, -4, 2));
var result1 = (a * b).ToMatrix44F();
var result2 = a.ToMatrix44F() * b.ToMatrix44F();
Assert.IsTrue(Matrix44F.AreNumericallyEqual(result1, result2));
}
/// <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.
Vector3F 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.
QuaternionF chainRotation = QuaternionF.Identity;
Vector3F 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;
//Vector3F 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 = QuaternionF.CreateRotation(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,
QuaternionF.CreateRotation(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.
Vector3F 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.
Vector3F rotationAxis = Vector3F.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(Vector3F.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,
(QuaternionF.CreateRotation(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);
}
}
}
private static void InsertRandomKeyFrames(Random random, SrtKeyFrameAnimation animation, TimeSpan time0, TimeSpan time1,
float scaleThreshold, float rotationThreshold, float translationThreshold)
{
rotationThreshold = MathHelper.ToRadians(rotationThreshold);
var defaultSource = SrtTransform.Identity;
var defaultTarget = SrtTransform.Identity;
var value = new SrtTransform();
int insertionIndex = 0;
for (int i = 0; i < animation.KeyFrames.Count; i++)
{
if (animation.KeyFrames[i].Time == time0)
{
insertionIndex = i + 1;
break;
}
}
Debug.Assert(insertionIndex > 0);
const int numberOfKeyFrames = 2;
long tickIncrement = (time1 - time0).Ticks / (numberOfKeyFrames + 1);
for (int i = 0; i < numberOfKeyFrames; i++)
{
var time = TimeSpan.FromTicks(time0.Ticks + (i + 1) * tickIncrement);
Debug.Assert(time0 < time && time < time1);
// Get interpolated animation value.
animation.GetValue(time, ref defaultSource, ref defaultTarget, ref value);
// Apply small variation (within thresholds).
value.Scale += random.NextVector3F(-1, 1).Normalized * (scaleThreshold / 2);
value.Rotation = QuaternionF.CreateRotation(random.NextVector3F(-1, 1), rotationThreshold / 2) * value.Rotation;
value.Translation += random.NextVector3F(-1, 1).Normalized * (translationThreshold / 2);
animation.KeyFrames.Insert(insertionIndex, new KeyFrame<SrtTransform>(time, value));
insertionIndex++;
}
}
