/// <summary>
/// Gets the first derivative of rotation at time t. The time is clamped within the
/// t0 - t1 range. Angular velocity is encoded as an angle-axis vector.
/// </summary>
public Vector3 AngularVelocityAt(float t)
{
float i = Mathf.Clamp01((t - t0) / (t1 - t0));
float i2 = i * i;
float i3 = i2 * i;
float dt = t1 - t0;
var oneThird = 1 / 3f;
var w1 = Quaternion.Inverse(rot0) * angVel0 * dt * oneThird;
var w3 = Quaternion.Inverse(rot1) * angVel1 * dt * oneThird;
var w2 = (Mathq.Exp(-w1)
* Quaternion.Inverse(rot0)
* rot1
* Mathq.Exp(-w3)).ToAngleAxisVector();
var beta1 = i3 - (3 * i2) + (3 * i);
var beta2 = -2 * i3 + 3 * i2;
//var beta3 = i3;
// Derivatives of beta1, beta2, beta3
var dotBeta1 = 3 * i2 - 5 * i + 3;
var dotBeta2 = -6 * i2 + 6 * i;
var dotBeta3 = 3 * i2;
var rot0_times_w1beta1 = rot0 * Mathq.Exp(w1 * beta1);
return
(rot0 * w1 * dotBeta1 +
rot0_times_w1beta1 * w2 * dotBeta2 +
rot0_times_w1beta1 * Mathq.Exp(w2 * beta2) * w3 * dotBeta3);
}
/// <summary>
/// Calculates the endpoint derivatives at q1 and q2 that define a Hermite spline
/// equivalent to a centripetal Catmull-Rom spline defined by the four input
/// rotations. "aV" refers to angular velocity and represents an angle-axis vector.
/// </summary>
private static void CalculateCatmullRomParameterization(Quaternion q0, Quaternion q1,
Quaternion q2, Quaternion q3,
out Vector3 aV1,
out Vector3 aV2,
bool centripetal = true)
{
aV1 = Vector3.zero;
aV2 = Vector3.zero;
if (!centripetal)
{
// (Uniform Catmull-Rom)
aV1 = Mathq.Log(Quaternion.Inverse(q0) * q2) / 2f;
aV2 = Mathq.Log(Quaternion.Inverse(q1) * q3) / 2f;
}
else
{
// Centripetal Catmull-Rom
// Handy little trick for using Euclidean-3 spline math on Quaternions, see
// slide 41 of
// https://www.cs.indiana.edu/ftp/hanson/Siggraph01QuatCourse/quatvis2.pdf
// "The trick: Where you would see 'x0 + t(x1 - x0)' in a Euclidean spline,
// replace (x1 - x0) by log((q0)^-1 * q1) "
// Centripetal Catmull-Rom parameterization
var dt0 = Mathf.Pow(Mathq.Log(Quaternion.Inverse(q1) * q0).sqrMagnitude, 0.25f);
var dt1 = Mathf.Pow(Mathq.Log(Quaternion.Inverse(q2) * q1).sqrMagnitude, 0.25f);
var dt2 = Mathf.Pow(Mathq.Log(Quaternion.Inverse(q3) * q2).sqrMagnitude, 0.25f);
// Check for repeated points.
if (dt1 < 1e-4f)
{
dt1 = 1.0f;
}
if (dt0 < 1e-4f)
{
dt0 = dt1;
}
if (dt2 < 1e-4f)
{
dt2 = dt1;
}
// A - B -> Mathq.Log(Quaternion.Inverse(B) * A)
// Centripetal Catmull-Rom
aV1 = Mathq.Log(Quaternion.Inverse(q0) * q1) / dt0
- Mathq.Log(Quaternion.Inverse(q0) * q2) / (dt0 + dt1)
+ Mathq.Log(Quaternion.Inverse(q1) * q2) / dt1;
aV2 = Mathq.Log(Quaternion.Inverse(q1) * q2) / dt1
- Mathq.Log(Quaternion.Inverse(q1) * q3) / (dt1 + dt2)
+ Mathq.Log(Quaternion.Inverse(q2) * q3) / dt2;
// Rescale for [0, 1] parameterization
aV1 *= dt1;
aV2 *= dt1;
}
}
/// <summary>
/// Gets both the rotation and the first derivative of rotation at time t. The time
/// is clamped within the t0 - t1 range. Angular velocity is encoded as an angle-axis
/// vector.
/// </summary>
public void RotationAndAngVelAt(float t, out Quaternion rotation,
out Vector3 angularVelocity)
{
float i = Mathf.Clamp01((t - t0) / (t1 - t0));
float i2 = i * i;
float i3 = i2 * i;
float dt = t1 - t0;
var oneThird = 1 / 3f;
var w1 = Quaternion.Inverse(rot0) * angVel0 * dt * oneThird;
var w3 = Quaternion.Inverse(rot1) * angVel1 * dt * oneThird;
var w2 = Mathq.Log(Mathq.Exp(-w1)
* Quaternion.Inverse(rot0)
* rot1
* Mathq.Exp(-w3));
var beta1 = i3 - (3 * i2) + (3 * i);
var beta2 = -2 * i3 + 3 * i2;
var beta3 = i3;
rotation =
rot0 * Mathq.Exp(w1 * beta1) * Mathq.Exp(w2 * beta2) * Mathq.Exp(w3 * beta3);
// Derivatives of beta1, beta2, beta3
var dotBeta1 = 3 * i2 - 5 * i + 3;
var dotBeta2 = -6 * i2 + 6 * i;
var dotBeta3 = 3 * i2;
var rot0_times_w1beta1 = rot0 * Mathq.Exp(w1 * beta1);
angularVelocity =
rot0 * w1 * dotBeta1 +
rot0_times_w1beta1 * w2 * dotBeta2 +
rot0_times_w1beta1 * Mathq.Exp(w2 * beta2) * w3 * dotBeta3;
}
/// <summary>
/// Gets the rotation at time t along this spline. The time is clamped within the
/// t0 - t1 range.
/// </summary>
public Quaternion RotationAt(float t)
{
if (t > t1)
{
float i = ((t - t0) / (t1 - t0)) - 1f;
return(rot1 * Mathq.Exp(angVel1 * i)); //unsure of the ordering here...
}
else
{
float i = Mathf.Clamp01((t - t0) / (t1 - t0));
float i2 = i * i;
float i3 = i2 * i;
float dt = t1 - t0;
var oneThird = 1 / 3f;
var w1 = Quaternion.Inverse(rot0) * angVel0 * dt * oneThird;
var w3 = Quaternion.Inverse(rot1) * angVel1 * dt * oneThird;
var w2 = Mathq.Log(Mathq.Exp(-w1)
* Quaternion.Inverse(rot0)
* rot1
* Mathq.Exp(-w3));
var beta1 = i3 - (3 * i2) + (3 * i);
var beta2 = -2 * i3 + 3 * i2;
var beta3 = i3;
return(rot0 * Mathq.Exp(w1 * beta1) * Mathq.Exp(w2 * beta2) * Mathq.Exp(w3 * beta3));
}
// A cubic Bezier quaternion curve can be used to define a Hermite quaternion curve
// which interpolates two end unit quaternions, q_a and q_b, and two angular
// velocities omega_a and omega_b.
//
// var q_at_t = q_0 * PRODUCT(i = 1, i < 3, exp(omega_i * beta_q(i, t))
// "where beta_q(i, t) is beta_q(i, 3, t) for i = 1, 2, 3."
//
// q coefficients:
// q_0 = q_a
// q_1 = q_a * exp(omega_a / 3)
// q_2 = q_b * exp(omega_b / 3)^-1
// q_3 = q_b
//
// omega coefficients:
// omega_1 = omega_a / 3
// omega_2 = log(exp(omega_a / 3)^-1 * q_a^-1 * q_b * exp(omega_b / 3)^-1)
// omega_3 = omega_b / 3
//
// Dependencies/definitions:
// "Bernstein basis", referred to as "beta"
// beta(i, n, t) = (n Choose i) * (1 - t)^(n - i) * t^i
// The form used in the formula are the cumulative basis functions:
// beta_q(i, n, t) = SUM(j = i, n, beta(i, n, t))
//
// The Exponential Mapping exp(v) and its Inverse
// "The exponential map can be interpreted as a mapping from the angular velocity
// vector (measured in S^3) into the unit quaternion which represents the rotation."
// ...
// "Given a vector v = theta * v_norm (in R^3), the exponential:
// exp(v) = SUM(i = 0, i -> inf, v^i) = (cos(theta), v_norm * sin(theta)) in S^3
// becomes the unit quaternion which represents the rotation by angle 2*theta
// about the axis v_norm, where v^i is computed using the quaternion multiplication."
//
// In other words... The Exponential converts a VECTOR represented by an ANGLE
// and a normalized AXIS to a quaternion. A Unity function for this:
// exp(v) -> Q := Quaternion.AngleAxis(v.magnitude, v.normalized);
//
// Its inverse map, log, would thus be the conversion from a Quaternion to an
// Angle-Axis representation. Quaternion.ToAngleAxisVector():
// log(Q) -> v := Q.ToAngleAxisVector() -> v
}