public void SetWorldPose_SetsWorldPositionRotationLength_ChainReversedCreationOrder()
{
CreateChainReversedCreationOrder();
var worldPose = new BonePose[3]
{
BonePose.Create(Pose.Create(Vector3.zero, Quaternion.AngleAxis(30, Vector3.forward)), 0f),
BonePose.Create(Pose.Create(Vector3.zero, Quaternion.AngleAxis(30, Vector3.forward)), 0f),
BonePose.Create(Pose.Create(Vector3.zero, Quaternion.AngleAxis(30, Vector3.forward)), 0f),
};
m_Skeleton.SetWorldPose(worldPose);
Assert.AreEqual(worldPose[0].pose.position, m_Skeleton.GetBone(0).position, "Incorrect position");
Assert.AreEqual(worldPose[1].pose.position, m_Skeleton.GetBone(1).position, "Incorrect position");
Assert.AreEqual(worldPose[2].pose.position, m_Skeleton.GetBone(2).position, "Incorrect position");
Assert.That(m_Skeleton.GetBone(0).rotation, Is.EqualTo(worldPose[0].pose.rotation).Using(quatCompare), "Incorrect rotation");
Assert.That(m_Skeleton.GetBone(1).rotation, Is.EqualTo(worldPose[1].pose.rotation).Using(quatCompare), "Incorrect rotation");
Assert.That(m_Skeleton.GetBone(2).rotation, Is.EqualTo(worldPose[2].pose.rotation).Using(quatCompare), "Incorrect rotation");
Assert.AreEqual(worldPose[0], m_Skeleton.GetBone(0).worldPose, "Incorrect pose");
Assert.AreEqual(worldPose[1], m_Skeleton.GetBone(1).worldPose, "Incorrect pose");
Assert.AreEqual(worldPose[2], m_Skeleton.GetBone(2).worldPose, "Incorrect pose");
}
public void SetLocalPose_SetsLocalPositionRotationLength()
{
CreateChain();
var localPose = new BonePose[3]
{
BonePose.Create(Pose.Create(Vector3.zero, Quaternion.AngleAxis(30, Vector3.forward)), 0f),
BonePose.Create(Pose.Create(Vector3.zero, Quaternion.AngleAxis(30, Vector3.forward)), 0f),
BonePose.Create(Pose.Create(Vector3.zero, Quaternion.AngleAxis(30, Vector3.forward)), 0f),
};
m_Skeleton.SetLocalPose(localPose);
Assert.AreEqual(localPose[0].pose.position, m_Skeleton.GetBone(0).localPosition, "Incorrect position");
Assert.AreEqual(localPose[1].pose.position, m_Skeleton.GetBone(1).localPosition, "Incorrect position");
Assert.AreEqual(localPose[2].pose.position, m_Skeleton.GetBone(2).localPosition, "Incorrect position");
Assert.That(m_Skeleton.GetBone(0).localRotation, Is.EqualTo(localPose[0].pose.rotation).Using(quatCompare), "Incorrect rotation");
Assert.That(m_Skeleton.GetBone(1).localRotation, Is.EqualTo(localPose[1].pose.rotation).Using(quatCompare), "Incorrect rotation");
Assert.That(m_Skeleton.GetBone(2).localRotation, Is.EqualTo(localPose[2].pose.rotation).Using(quatCompare), "Incorrect rotation");
Assert.AreEqual(localPose[0], m_Skeleton.GetBone(0).localPose, "Incorrect pose");
Assert.AreEqual(localPose[1], m_Skeleton.GetBone(1).localPose, "Incorrect pose");
Assert.AreEqual(localPose[2], m_Skeleton.GetBone(2).localPose, "Incorrect pose");
}
private void UpdateVelocity(BonePose from, BonePose to, float deltaTime)
{
Vector3 instantVelocity = (to.position - from.position) / deltaTime;
if (instantVelocity.magnitude > MAX_VELOCITY)
{
instantVelocity = instantVelocity.normalized * MAX_VELOCITY;
}
_handVelocity = Vector3.Lerp(_handVelocity, instantVelocity, deltaTime * VELOCITY_SPEED);
}
private void Initialize()
{
_cleanFingers = (BonePose[])wrapee.Fingers.Clone();
_cleanHand = new BonePose()
{
boneID = BoneId.Hand_Start,
rotation = wrapee.Hand.rotation,
position = wrapee.Hand.position
};
}
private void UpdateBones(float deltaTime)
{
if (wrapee.IsHandHighConfidence())
{
_ready = true;
if (_wasHighConfidence)
{
UpdateVelocity(_cleanHand, wrapee.Hand, deltaTime);
}
else
{
ResetVelocity();
_startHighConfidenceTime = Time.timeSinceLevelLoad;
}
if (_startHighConfidenceTime.HasValue &&
Time.timeSinceLevelLoad - _startHighConfidenceTime.Value < CATCH_UP_TIME)
{
float t = Mathf.Clamp01((Time.timeSinceLevelLoad - _startHighConfidenceTime.Value) / CATCH_UP_TIME);
_cleanHand = BonePose.Lerp(_cleanHand, wrapee.Hand, t).Value;
}
else
{
_startHighConfidenceTime = null;
_cleanHand = wrapee.Hand;
}
_wasHighConfidence = true;
}
else if (_ready)
{
_cleanHand = ApplyVelocity(_handVelocity, _cleanHand, deltaTime);
DampVelocity(deltaTime);
_wasHighConfidence = false;
}
else
{
_cleanHand.rotation = wrapee.Hand.rotation;
_cleanHand.position = wrapee.Hand.position;
}
for (int i = 0; i < wrapee.Fingers.Length; i++)
{
BonePose rawBone = wrapee.Fingers[i];
if (wrapee.IsFingerHighConfidence(rawBone.boneID) ||
!_ready)
{
_cleanFingers[i] = rawBone;
}
}
OnUpdated?.Invoke(deltaTime);
}
private void UpdateBones(float deltaTime)
{
for (int i = 0; i < wrapee.Fingers.Length; i++)
{
BonePose rawBone = wrapee.Fingers[i];
if (wrapee.IsFingerHighConfidence(rawBone.boneID))
{
_cleanFingers[i] = rawBone;
}
}
if (wrapee.IsHandHighConfidence())
{
_cleanHand = wrapee.Hand;
}
}
private BonePose ApplyVelocity(Vector3 velocity, BonePose pose, float deltaTime)
{
pose.position += velocity * deltaTime;
return(pose);
}
/// <summary>
/// Gets the current transform for the given BonePose object in the animation.
/// This is only called when a bone pose is affected by the current animation.
/// </summary>
/// <param name="pose">The BonePose object querying for the current transform in
/// the animation.</param>
/// <returns>The current transform of the bone.</returns>
public virtual Matrix GetCurrentBoneTransform(BonePose pose)
{
AnimationChannelCollection channels = animation.AnimationChannels;
BoneKeyframeCollection channel = channels[pose.Name];
int boneIndex = channel.GetIndexByTime(elapsedTime);
return channel[boneIndex].Transform;
}
/// <summary>
/// Returns true if the animation contains a track for the given BonePose.
/// </summary>
/// <param name="pose">The BonePose to test for track existence.</param>
/// <returns>True if the animation contains a track for the given BonePose.</returns>
public bool ContainsAnimationTrack(BonePose pose)
{
return animation.AnimationChannels.AffectsBone(pose.Name);
}
/// <summary>
/// Returns true if the controller can affect the bone.
/// </summary>
/// <param name="pose">The bone.</param>
/// <returns>True if the controller can affect the bone.</returns>
public bool ContainsAnimationTrack(BonePose pose)
{
return true;
}
/// <summary>
/// Gets the current bone transform.
/// </summary>
/// <param name="pose">The pose.</param>
/// <returns>The current transform of the bone.</returns>
public Matrix GetCurrentBoneTransform(BonePose pose)
{
if (controllerDict.Count == 0)
{
return pose.DefaultTransform;
}
Matrix transform = new Matrix();
Matrix m;
foreach (KeyValuePair<IAnimationController, float> k in controllerDict)
{
if (k.Key.ContainsAnimationTrack(pose))
{
m = k.Key.GetCurrentBoneTransform(pose);
transform += k.Value * m;
}
else
{
transform += k.Value * pose.DefaultTransform;
}
}
return transform;
}
private void Initialize()
{
_cleanFingers = (BonePose[])wrapee.Fingers.Clone();
_cleanHand = wrapee.Hand;
}
/// <summary>
/// Returns the current transform for a bone.
/// </summary>
/// <param name="pose">The bone.</param>
/// <returns>The bone's current transform.</returns>
public override Matrix GetCurrentBoneTransform(BonePose pose)
{
BoneKeyframeCollection channel = base.AnimationSource.AnimationChannels[
pose.Name];
int curIndex = channel.GetIndexByTime(base.ElapsedTime);
if (interpMethod == InterpolationMethod.None)
{
return channel[curIndex].Transform;
}
int nextIndex = curIndex + 1;
if (nextIndex >= channel.Count)
{
return channel[curIndex].Transform;
}
// Numerator of the interpolation factor
double interpNumerator = (double)(ElapsedTime - channel[curIndex].Time);
// Denominator of the interpolation factor
double interpDenom = (double)(channel[nextIndex].Time
- channel[curIndex].Time);
// The interpolation factor, or amount to interpolate between the current
// and next frame
double interpAmount = interpNumerator / interpDenom;
curTransform = channel[curIndex].Transform;
nextTransform = channel[nextIndex].Transform;
if (interpMethod == InterpolationMethod.Linear)
{
Matrix.Lerp(ref curTransform, ref nextTransform,
(float)interpAmount, out transform);
}
else
{
Util.SlerpMatrix(ref curTransform, ref nextTransform,
(float)interpAmount, out transform);
}
return transform;
}
