public static void DrawArc(Vector3 center, Vector3 from, Vector3 normal, float angle, float radius, int numSegments, Color color, bool depthTest = true)
{
if (numSegments <= 0)
{
return;
}
from.Normalize();
from *= radius;
var aVert = new Vector3[numSegments + 1];
aVert[0] = center + from;
float numSegmentsInv = 1.0f / numSegments;
Quaternion rotStep = QuaternionUtil.AxisAngle(normal, angle * numSegmentsInv);
Vector3 vec = rotStep * from;
for (int i = 1; i <= numSegments; ++i)
{
aVert[i] = center + vec;
vec = rotStep * vec;
}
DrawLineStrip(aVert, color, depthTest);
}
// master sterp function
public static Quaternion Sterp
(
Quaternion a,
Quaternion b,
Vector3 twistAxis,
float tSwing,
float tTwist,
SterpMode mode,
out Quaternion swing,
out Quaternion twist
)
{
Quaternion q = b * Quaternion.Inverse(a);
Quaternion swingFull;
Quaternion twistFull;
QuaternionUtil.DecomposeSwingTwist(q, twistAxis, out swingFull, out twistFull);
switch (mode)
{
default:
case SterpMode.Nlerp:
swing = Nlerp(Quaternion.identity, swingFull, tSwing);
twist = Nlerp(Quaternion.identity, twistFull, tTwist);
break;
case SterpMode.Slerp:
swing = Quaternion.Slerp(Quaternion.identity, swingFull, tSwing);
twist = Quaternion.Slerp(Quaternion.identity, twistFull, tTwist);
break;
}
return(twist * swing);
}
protected override void Draw(Color color, DebugUtil.Style style, bool depthTest)
{
Quaternion startRot = QuaternionUtil.AxisAngle(Vector3.forward, StartAngle * MathUtil.Deg2Rad);
DebugUtil.DrawArc
(
transform.position,
transform.rotation * startRot * Vector3.right,
transform.rotation * Vector3.forward,
ArcAngle * MathUtil.Deg2Rad, Radius, NumSegments,
color, depthTest
);
}
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;
}
