private static float EvaluateTimescale(ref Key <TransformPoint> k0, bool haveK0,
ref Key <TransformPoint> k1,
ref Key <TransformPoint> k2,
ref Key <TransformPoint> k3, bool haveK3,
float t)
{
return(Math.Max(0f, SplineUtil.Linear(
t, k1.value.timescale, k2.value.timescale)));
}
/// <summary>
/// Interpolate rotation dumbly using cubic Hermite interpolation of
/// each Quaternion components. This creates odd effects.
/// </summary>
private static Quaternion EvaluateRotationComponent(
ref Key <TransformPoint> k0, bool haveK0,
ref Key <TransformPoint> k1,
ref Key <TransformPoint> k2,
ref Key <TransformPoint> k3, bool haveK3,
float t)
{
float dp = k2.param - k1.param;
Quaternion m0 = new Quaternion(0, 0, 0, 0);
if (haveK0)
{
m0.x = SplineUtil.T(
k0.param, k1.param, k2.param,
k0.value.rotation.x, k1.value.rotation.x, k2.value.rotation.x) * dp;
m0.y = SplineUtil.T(
k0.param, k1.param, k2.param,
k0.value.rotation.y, k1.value.rotation.y, k2.value.rotation.y) * dp;
m0.z = SplineUtil.T(
k0.param, k1.param, k2.param,
k0.value.rotation.z, k1.value.rotation.z, k2.value.rotation.z) * dp;
m0.w = SplineUtil.T(
k0.param, k1.param, k2.param,
k0.value.rotation.w, k1.value.rotation.w, k2.value.rotation.w) * dp;
}
Quaternion m1 = new Quaternion(0, 0, 0, 0);
if (haveK3)
{
m1.x = SplineUtil.T(
k1.param, k2.param, k3.param,
k1.value.rotation.x, k2.value.rotation.x, k3.value.rotation.x) * dp;
m1.y = SplineUtil.T(
k1.param, k2.param, k3.param,
k1.value.rotation.y, k2.value.rotation.y, k3.value.rotation.y) * dp;
m1.z = SplineUtil.T(
k1.param, k2.param, k3.param,
k1.value.rotation.z, k2.value.rotation.z, k3.value.rotation.z) * dp;
m1.w = SplineUtil.T(
k1.param, k2.param, k3.param,
k1.value.rotation.w, k2.value.rotation.w, k3.value.rotation.w) * dp;
}
return(new Quaternion {
x = SplineUtil.CubicHermite(t, k1.value.rotation.x, m0.x, k2.value.rotation.x, m1.x),
y = SplineUtil.CubicHermite(t, k1.value.rotation.y, m0.y, k2.value.rotation.y, m1.y),
z = SplineUtil.CubicHermite(t, k1.value.rotation.z, m0.z, k2.value.rotation.z, m1.z),
w = SplineUtil.CubicHermite(t, k1.value.rotation.w, m0.w, k2.value.rotation.w, m1.w)
});
}
private static Vector3 EvaluatePosition(
ref Key <TransformPoint> k0, bool haveK0,
ref Key <TransformPoint> k1,
ref Key <TransformPoint> k2,
ref Key <TransformPoint> k3, bool haveK3,
float t)
{
float dp = k2.param - k1.param;
Vector3 m0 = new Vector3(0, 0, 0);
if (haveK0)
{
m0.x = SplineUtil.T(
k0.param, k1.param, k2.param,
k0.value.position.x, k1.value.position.x, k2.value.position.x) * dp;
m0.y = SplineUtil.T(
k0.param, k1.param, k2.param,
k0.value.position.y, k1.value.position.y, k2.value.position.y) * dp;
m0.z = SplineUtil.T(
k0.param, k1.param, k2.param,
k0.value.position.z, k1.value.position.z, k2.value.position.z) * dp;
}
Vector3 m1 = new Vector3(0, 0, 0);
if (haveK3)
{
m1.x = SplineUtil.T(
k1.param, k2.param, k3.param,
k1.value.position.x, k2.value.position.x, k3.value.position.x) * dp;
m1.y = SplineUtil.T(
k1.param, k2.param, k3.param,
k1.value.position.y, k2.value.position.y, k3.value.position.y) * dp;
m1.z = SplineUtil.T(
k1.param, k2.param, k3.param,
k1.value.position.z, k2.value.position.z, k3.value.position.z) * dp;
}
Vector3 position = new Vector3 {
x = SplineUtil.CubicHermite(t, k1.value.position.x, m0.x, k2.value.position.x, m1.x),
y = SplineUtil.CubicHermite(t, k1.value.position.y, m0.y, k2.value.position.y, m1.y),
z = SplineUtil.CubicHermite(t, k1.value.position.z, m0.z, k2.value.position.z, m1.z)
};
return(position);
}
public TransformPoint Evaluate(
Key <TransformPoint> k0, bool haveK0,
Key <TransformPoint> k1,
Key <TransformPoint> k2,
Key <TransformPoint> k3, bool haveK3,
float t
)
{
float dt1 = k2.param - k1.param;
if (SplineUtil.AreParamsClose(k1.param, k2.param))
{
// Don't attempt to interpolate between points that are
// *really* close together or even at the same "time".
// This works around some nan/inf problems in such cases.
return(new TransformPoint {
position = k1.value.position,
rotation = k1.value.rotation
});
}
if (haveK0 && SplineUtil.AreParamsClose(k0.param, k1.param))
{
// Avoid interpolation data from k0 to k1.
haveK0 = false;
}
if (haveK3 && SplineUtil.AreParamsClose(k2.param, k3.param))
{
// Avoid interpolation data from k2 to k3.
haveK3 = false;
}
return(new TransformPoint {
position = EvaluatePosition(
ref k0, haveK0, ref k1, ref k2, ref k3, haveK3, t),
rotation = EvaluateRotation(ref k0, haveK0, ref k1, ref k2, ref k3, haveK3, t),
timescale = EvaluateTimescale(ref k0, haveK0, ref k1, ref k2, ref k3, haveK3, t),
});
}