public Quaternion TrackExponential(Quaternion targetValue, float halfLife, float deltaTime)
{
if (halfLife < MathUtil.Epsilon)
{
VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);
ValueVec = QuaternionUtil.ToVector4(targetValue);
return(targetValue);
}
float angularFrequency = 0.6931472f / halfLife;
float dampingRatio = 1.0f;
return(TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime));
}
public Quaternion TrackHalfLife(Quaternion targetValue, float frequencyHz, float halfLife, float deltaTime)
{
if (halfLife < MathUtil.Epsilon)
{
VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);
ValueVec = QuaternionUtil.ToVector4(targetValue);
return(targetValue);
}
float angularFrequency = frequencyHz * MathUtil.TwoPi;
float dampingRatio = 0.6931472f / (angularFrequency * halfLife);
return(TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime));
}
public Quaternion TrackDampingRatio(Quaternion targetValue, float angularFrequency, float dampingRatio, float deltaTime)
{
if (angularFrequency < MathUtil.Epsilon)
{
VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);
return(QuaternionUtil.FromVector4(ValueVec));
}
Vector4 targetValueVec = QuaternionUtil.ToVector4(targetValue);
// keep in same hemisphere for shorter track delta
if (Vector4.Dot(ValueVec, targetValueVec) < 0.0f)
{
targetValueVec = -targetValueVec;
}
Vector4 delta = targetValueVec - ValueVec;
float f = 1.0f + 2.0f * deltaTime * dampingRatio * angularFrequency;
float oo = angularFrequency * angularFrequency;
float hoo = deltaTime * oo;
float hhoo = deltaTime * hoo;
float detInv = 1.0f / (f + hhoo);
Vector4 detX = f * ValueVec + deltaTime * VelocityVec + hhoo * targetValueVec;
Vector4 detV = VelocityVec + hoo * delta;
VelocityVec = detV * detInv;
ValueVec = detX * detInv;
if (VelocityVec.magnitude < MathUtil.Epsilon && delta.magnitude < MathUtil.Epsilon)
{
VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);;
ValueVec = targetValueVec;
}
return(QuaternionUtil.FromVector4(ValueVec));
}
public void Reset(Quaternion initValue, Quaternion initVelocity)
{
ValueVec = QuaternionUtil.ToVector4(initValue);
VelocityVec = QuaternionUtil.ToVector4(initVelocity);
}
public void Reset(Quaternion initValue)
{
ValueVec = QuaternionUtil.ToVector4(initValue);
VelocityVec = Vector4.zero;
}
public void Reset()
{
ValueVec = QuaternionUtil.ToVector4(Quaternion.identity);
VelocityVec = Vector4.zero;
}