Tanoda
HermiteQuaternionSpline.cs
Go to the documentation of this file.
1/******************************************************************************
2 * Copyright (C) Ultraleap, Inc. 2011-2020. *
3 * *
4 * Use subject to the terms of the Apache License 2.0 available at *
5 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement *
6 * between Ultraleap and you, your company or other organization. *
7 ******************************************************************************/
8
9using System;
10using UnityEngine;
11
12namespace Leap.Unity.Animation {
13
22 [Serializable]
23 public struct HermiteQuaternionSpline : ISpline<Quaternion, Vector3> {
24
25 public float t0, t1;
26 public Quaternion rot0, rot1;
27 public Vector3 angVel0, angVel1;
28
34 public HermiteQuaternionSpline(Quaternion rot0, Quaternion rot1) {
35 t0 = 0;
36 t1 = 1;
37
38 this.angVel0 = default(Vector3);
39 this.angVel1 = default(Vector3);
40
41 this.rot0 = rot0;
42 this.rot1 = rot1;
43 }
44
51 public HermiteQuaternionSpline(Quaternion rot0, Quaternion rot1,
52 Vector3 angVel0, Vector3 angVel1) {
53 t0 = 0;
54 t1 = 1;
55
56 this.angVel0 = angVel0;
57 this.angVel1 = angVel1;
58
59 this.rot0 = rot0;
60 this.rot1 = rot1;
61 }
62
69 public HermiteQuaternionSpline(Quaternion rot0, Quaternion rot1,
70 Vector3 angVel0, Vector3 angVel1,
71 float duration) {
72 t0 = 0;
73 t1 = duration;
74
75 this.angVel0 = angVel0;
76 this.angVel1 = angVel1;
77
78 this.rot0 = rot0;
79 this.rot1 = rot1;
80 }
81
86 public HermiteQuaternionSpline(float t0, float t1,
87 Quaternion rot0, Quaternion rot1,
88 Vector3 angVel0, Vector3 angVel1) {
89 this.t0 = t0;
90 this.t1 = t1;
91
92 this.angVel0 = angVel0;
93 this.angVel1 = angVel1;
94
95 this.rot0 = rot0.ToNormalized();
96 this.rot1 = rot1.ToNormalized();
97 }
98
103 public Quaternion RotationAt(float t) {
104 if (t > t1) {
105 float i = ((t - t0) / (t1 - t0)) - 1f;
106 return rot1 * Mathq.Exp(angVel1 * i); //unsure of the ordering here...
107 } else {
108 float i = Mathf.Clamp01((t - t0) / (t1 - t0));
109 float i2 = i * i;
110 float i3 = i2 * i;
111
112 float dt = t1 - t0;
113
114 var oneThird = 1 / 3f;
115
116 var w1 = Quaternion.Inverse(rot0) * angVel0 * dt * oneThird;
117 var w3 = Quaternion.Inverse(rot1) * angVel1 * dt * oneThird;
118 var w2 = Mathq.Log(Mathq.Exp(-w1)
119 * Quaternion.Inverse(rot0)
120 * rot1
121 * Mathq.Exp(-w3));
122
123 var beta1 = i3 - (3 * i2) + (3 * i);
124 var beta2 = -2 * i3 + 3 * i2;
125 var beta3 = i3;
126
127 return rot0 * Mathq.Exp(w1 * beta1) * Mathq.Exp(w2 * beta2) * Mathq.Exp(w3 * beta3);
128 }
129 // A cubic Bezier quaternion curve can be used to define a Hermite quaternion curve
130 // which interpolates two end unit quaternions, q_a and q_b, and two angular
131 // velocities omega_a and omega_b.
132 //
133 // var q_at_t = q_0 * PRODUCT(i = 1, i < 3, exp(omega_i * beta_q(i, t))
134 // "where beta_q(i, t) is beta_q(i, 3, t) for i = 1, 2, 3."
135 //
136 // q coefficients:
137 // q_0 = q_a
138 // q_1 = q_a * exp(omega_a / 3)
139 // q_2 = q_b * exp(omega_b / 3)^-1
140 // q_3 = q_b
141 //
142 // omega coefficients:
143 // omega_1 = omega_a / 3
144 // omega_2 = log(exp(omega_a / 3)^-1 * q_a^-1 * q_b * exp(omega_b / 3)^-1)
145 // omega_3 = omega_b / 3
146 //
147 // Dependencies/definitions:
148 // "Bernstein basis", referred to as "beta"
149 // beta(i, n, t) = (n Choose i) * (1 - t)^(n - i) * t^i
150 // The form used in the formula are the cumulative basis functions:
151 // beta_q(i, n, t) = SUM(j = i, n, beta(i, n, t))
152 //
153 // The Exponential Mapping exp(v) and its Inverse
154 // "The exponential map can be interpreted as a mapping from the angular velocity
155 // vector (measured in S^3) into the unit quaternion which represents the rotation."
156 // ...
157 // "Given a vector v = theta * v_norm (in R^3), the exponential:
158 // exp(v) = SUM(i = 0, i -> inf, v^i) = (cos(theta), v_norm * sin(theta)) in S^3
159 // becomes the unit quaternion which represents the rotation by angle 2*theta
160 // about the axis v_norm, where v^i is computed using the quaternion multiplication."
161 //
162 // In other words... The Exponential converts a VECTOR represented by an ANGLE
163 // and a normalized AXIS to a quaternion. A Unity function for this:
164 // exp(v) -> Q := Quaternion.AngleAxis(v.magnitude, v.normalized);
165 //
166 // Its inverse map, log, would thus be the conversion from a Quaternion to an
167 // Angle-Axis representation. Quaternion.ToAngleAxisVector():
168 // log(Q) -> v := Q.ToAngleAxisVector() -> v
169 }
170
175 public Vector3 AngularVelocityAt(float t) {
176 float i = Mathf.Clamp01((t - t0) / (t1 - t0));
177 float i2 = i * i;
178 float i3 = i2 * i;
179
180 float dt = t1 - t0;
181
182 var oneThird = 1 / 3f;
183
184 var w1 = Quaternion.Inverse(rot0) * angVel0 * dt * oneThird;
185 var w3 = Quaternion.Inverse(rot1) * angVel1 * dt * oneThird;
186 var w2 = (Mathq.Exp(-w1)
187 * Quaternion.Inverse(rot0)
188 * rot1
189 * Mathq.Exp(-w3)).ToAngleAxisVector();
190
191 var beta1 = i3 - (3 * i2) + (3 * i);
192 var beta2 = -2 * i3 + 3 * i2;
193 //var beta3 = i3;
194
195 // Derivatives of beta1, beta2, beta3
196 var dotBeta1 = 3 * i2 - 5 * i + 3;
197 var dotBeta2 = -6 * i2 + 6 * i;
198 var dotBeta3 = 3 * i2;
199
200 var rot0_times_w1beta1 = rot0 * Mathq.Exp(w1 * beta1);
201
202 return
203 rot0 * w1 * dotBeta1 +
204 rot0_times_w1beta1 * w2 * dotBeta2 +
205 rot0_times_w1beta1 * Mathq.Exp(w2 * beta2) * w3 * dotBeta3;
206 }
207
213 public void RotationAndAngVelAt(float t, out Quaternion rotation,
214 out Vector3 angularVelocity) {
215 float i = Mathf.Clamp01((t - t0) / (t1 - t0));
216 float i2 = i * i;
217 float i3 = i2 * i;
218
219 float dt = t1 - t0;
220
221 var oneThird = 1 / 3f;
222
223 var w1 = Quaternion.Inverse(rot0) * angVel0 * dt * oneThird;
224 var w3 = Quaternion.Inverse(rot1) * angVel1 * dt * oneThird;
225 var w2 = Mathq.Log(Mathq.Exp(-w1)
226 * Quaternion.Inverse(rot0)
227 * rot1
228 * Mathq.Exp(-w3));
229
230 var beta1 = i3 - (3 * i2) + (3 * i);
231 var beta2 = -2 * i3 + 3 * i2;
232 var beta3 = i3;
233
234 rotation =
235 rot0 * Mathq.Exp(w1 * beta1) * Mathq.Exp(w2 * beta2) * Mathq.Exp(w3 * beta3);
236
237 // Derivatives of beta1, beta2, beta3
238 var dotBeta1 = 3 * i2 - 5 * i + 3;
239 var dotBeta2 = -6 * i2 + 6 * i;
240 var dotBeta3 = 3 * i2;
241
242 var rot0_times_w1beta1 = rot0 * Mathq.Exp(w1 * beta1);
243
244 angularVelocity =
245 rot0 * w1 * dotBeta1 +
246 rot0_times_w1beta1 * w2 * dotBeta2 +
247 rot0_times_w1beta1 * Mathq.Exp(w2 * beta2) * w3 * dotBeta3;
248 }
249
250 #region ISpline<Quaternion, Vector3>
251
252 public float minT { get { return t0; } }
253
254 public float maxT { get { return t1; } }
255
256 public Quaternion ValueAt(float t) {
257 return RotationAt(t);
258 }
259
260 public Vector3 DerivativeAt(float t) {
261 return AngularVelocityAt(t);
262 }
263
264 public void ValueAndDerivativeAt(float t, out Quaternion value,
265 out Vector3 deltaValuePerSec) {
266 RotationAndAngVelAt(t, out value, out deltaValuePerSec);
267 }
268
269 #endregion
270
271 }
272
276 public static class Mathq {
277
278 #region Exponential Quaternion Map
279
280 // Quaternion CHS reference:
281 // Kim, Kim, and Shin, 1995.
282 // A General Construction Scheme for Unit Quaternion Curves with Simple High Order
283 // Derivatives.
284 // http://graphics.cs.cmu.edu/nsp/course/15-464/Fall05/papers/kimKimShin.pdf
285
286 // also see Slide 41 of:
287 // https://www.cs.indiana.edu/ftp/hanson/Siggraph01QuatCourse/quatvis2.pdf
288 // for converting
289
298 public static Quaternion Exp(Vector3 angleAxisVector) {
299 var angle = angleAxisVector.magnitude;
300 var axis = angleAxisVector / angle;
301 return Quaternion.AngleAxis(angle, axis);
302 }
303
312 public static Vector3 Log(Quaternion quaternion) {
313 return quaternion.ToAngleAxisVector();
314 }
315
316 #endregion
317
318 }
319
320}
Represents a spline for the rotation of a rigid body from one orientation in space to another over a ...
HermiteQuaternionSpline(float t0, float t1, Quaternion rot0, Quaternion rot1, Vector3 angVel0, Vector3 angVel1)
Constructs a spline by specifying the rotations, angular velocities, and times of the endpoints.
void RotationAndAngVelAt(float t, out Quaternion rotation, out Vector3 angularVelocity)
Gets both the rotation and the first derivative of rotation at time t. The time is clamped within the...
HermiteQuaternionSpline(Quaternion rot0, Quaternion rot1, Vector3 angVel0, Vector3 angVel1, float duration)
Constructs a Quaternion spline by specifying the rotations (as quaternions) and angular velocities (a...
void ValueAndDerivativeAt(float t, out Quaternion value, out Vector3 deltaValuePerSec)
Vector3 AngularVelocityAt(float t)
Gets the first derivative of rotation at time t. The time is clamped within the t0 - t1 range....
Quaternion RotationAt(float t)
Gets the rotation at time t along this spline. The time is clamped within the t0 - t1 range.
HermiteQuaternionSpline(Quaternion rot0, Quaternion rot1)
Constructs a Quaternion spline by specifying the rotations at the two endpoints. The angular velocity...
HermiteQuaternionSpline(Quaternion rot0, Quaternion rot1, Vector3 angVel0, Vector3 angVel1)
Constructs a Quaternion spline by specifying the rotations (as quaternions) and angular velocities (a...