/// <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 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); }
/// <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 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 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 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)); } }
/// <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++; } }