Tanoda
CatmullRom.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
10using UnityEngine;
11
12namespace Leap.Unity.Splines {
13
17 public static class CatmullRom {
18
26 public static HermiteSpline3 ToCHS(
27 Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3,
28 bool centripetal = true)
29 {
30 Vector3 v1, v2;
31 CalculateCatmullRomParameterization(p0, p1, p2, p3, out v1, out v2,
32 centripetal);
33
34 return new HermiteSpline3(p1, p2, v1, v2);
35 }
36
41 private static void CalculateCatmullRomParameterization(
42 Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3,
43 out Vector3 v1, out Vector3 v2,
44 bool centripetal = true)
45 {
46 v1 = Vector3.zero;
47 v2 = Vector3.zero;
48
49 if (!centripetal) {
50 // (Uniform Catmull-Rom)
51 v1 = (p2 - p0) / 2f;
52 v2 = (p3 - p1) / 2f;
53 } else {
54 // Centripetal Catmull-Rom
55 var dt0 = Mathf.Pow((p0 - p1).sqrMagnitude, 0.25f);
56 var dt1 = Mathf.Pow((p1 - p2).sqrMagnitude, 0.25f);
57 var dt2 = Mathf.Pow((p2 - p3).sqrMagnitude, 0.25f);
58
59 // Check for repeated points.
60 if (dt1 < 1e-4f) dt1 = 1.0f;
61 if (dt0 < 1e-4f) dt0 = dt1;
62 if (dt2 < 1e-4f) dt2 = dt1;
63
64 // Centripetal Catmull-Rom
65 v1 = (p1 - p0) / dt0 - (p2 - p0) / (dt0 + dt1) + (p2 - p1) / dt1;
66 v2 = (p2 - p1) / dt1 - (p3 - p1) / (dt1 + dt2) + (p3 - p2) / dt2;
67
68 // Rescale for [0, 1] parameterization
69 v1 *= dt1;
70 v2 *= dt1;
71 }
72 }
73
78 public static HermiteQuaternionSpline ToQuaternionCHS(
79 Quaternion q0, Quaternion q1, Quaternion q2, Quaternion q3,
80 bool centripetal = true)
81 {
82 Vector3 aV1, aV2;
83 CalculateCatmullRomParameterization(q0, q1, q2, q3, out aV1, out aV2, centripetal);
84
85 return new HermiteQuaternionSpline(q1, q2, aV1, aV2);
86 }
87
93 private static void CalculateCatmullRomParameterization(Quaternion q0, Quaternion q1,
94 Quaternion q2, Quaternion q3,
95 out Vector3 aV1,
96 out Vector3 aV2,
97 bool centripetal = true) {
98 aV1 = Vector3.zero;
99 aV2 = Vector3.zero;
100
101 if (!centripetal) {
102 // (Uniform Catmull-Rom)
103 aV1 = Mathq.Log(Quaternion.Inverse(q0) * q2)/2f;
104 aV2 = Mathq.Log(Quaternion.Inverse(q1) * q3)/2f;
105 } else {
106 // Centripetal Catmull-Rom
107 // Handy little trick for using Euclidean-3 spline math on Quaternions, see
108 // slide 41 of
109 // https://www.cs.indiana.edu/ftp/hanson/Siggraph01QuatCourse/quatvis2.pdf
110 // "The trick: Where you would see 'x0 + t(x1 - x0)' in a Euclidean spline,
111 // replace (x1 - x0) by log((q0)^-1 * q1) "
112
113 // Centripetal Catmull-Rom parameterization
114 var dt0 = Mathf.Pow(Mathq.Log(Quaternion.Inverse(q1) * q0).sqrMagnitude, 0.25f);
115 var dt1 = Mathf.Pow(Mathq.Log(Quaternion.Inverse(q2) * q1).sqrMagnitude, 0.25f);
116 var dt2 = Mathf.Pow(Mathq.Log(Quaternion.Inverse(q3) * q2).sqrMagnitude, 0.25f);
117
118 // Check for repeated points.
119 if (dt1 < 1e-4f) dt1 = 1.0f;
120 if (dt0 < 1e-4f) dt0 = dt1;
121 if (dt2 < 1e-4f) dt2 = dt1;
122
123 // A - B -> Mathq.Log(Quaternion.Inverse(B) * A)
124
125 // Centripetal Catmull-Rom
126 aV1 = Mathq.Log(Quaternion.Inverse(q0) * q1) / dt0
127 - Mathq.Log(Quaternion.Inverse(q0) * q2) / (dt0 + dt1)
128 + Mathq.Log(Quaternion.Inverse(q1) * q2) / dt1;
129 aV2 = Mathq.Log(Quaternion.Inverse(q1) * q2) / dt1
130 - Mathq.Log(Quaternion.Inverse(q1) * q3) / (dt1 + dt2)
131 + Mathq.Log(Quaternion.Inverse(q2) * q3) / dt2;
132
133 // Rescale for [0, 1] parameterization
134 aV1 *= dt1;
135 aV2 *= dt1;
136 }
137 }
138
139 public static HermitePoseSpline ToPoseCHS(Pose p0, Pose p1, Pose p2, Pose p3) {
140 Vector3 v1, v2, aV1, aV2;
141 CalculateCatmullRomParameterization(p0.position, p1.position,
142 p2.position, p3.position,
143 out v1, out v2);
144 CalculateCatmullRomParameterization(p0.rotation, p1.rotation,
145 p2.rotation, p3.rotation,
146 out aV1, out aV2);
147
148 return new HermitePoseSpline(p1, p2, new Movement(v1, aV1), new Movement(v2, aV2));
149 }
150
151 #region Archived code -- legacy code relies on this. It is incorrect though!
152
153 // TODO: The below isn't ACTUALLY a centripetal Catmull-Rom spline, it's just a
154 // spline parameterization -- centripetal-ness requires choosing "time values" that
155 // correspond to the distances between the four positions. The implementation
156 // above is better.
157 // deleeeeete it!
158
163 public static void InterpolatePoints(Vector3[] fourPositions, float[] timeValues, ref Vector3[] outPoints) {
164 InterpolatePoints(fourPositions, timeValues, ref outPoints, numPoints: outPoints.Length);
165 }
166
176 public static void InterpolatePoints(Vector3[] fourPositions, float[] timeValues,
177 ref Vector3[] outPoints, int numPoints) {
178 for (int i = 0; i < numPoints; i++) {
179 outPoints[i] = Interpolate(fourPositions, timeValues, i * (1F / (numPoints - 1)));
180 }
181 }
182
184 // http://stackoverflow.com/questions/9489736/catmull-rom-curve-with-no-cusps-and-no-self-intersections
185 public static Vector3 Interpolate(Vector3[] P, float[] T, float t) {
186 Vector3 L01 = P[0] * (T[1] - t) / (T[1] - T[0]) + P[1] * (t - T[0]) / (T[1] - T[0]);
187 Vector3 L12 = P[1] * (T[2] - t) / (T[2] - T[1]) + P[2] * (t - T[1]) / (T[2] - T[1]);
188 Vector3 L23 = P[2] * (T[3] - t) / (T[3] - T[2]) + P[3] * (t - T[2]) / (T[3] - T[2]);
189 Vector3 L012 = L01 * (T[2] - t) / (T[2] - T[0]) + L12 * (t - T[0]) / (T[2] - T[0]);
190 Vector3 L123 = L12 * (T[3] - t) / (T[3] - T[1]) + L23 * (t - T[1]) / (T[3] - T[1]);
191 Vector3 C12 = L012 * (T[2] - t) / (T[2] - T[1]) + L123 * (t - T[1]) / (T[2] - T[1]);
192 return C12;
193 }
194
195 #endregion
196
197 }
198
199}
Represents a spline for poses – positions and rotations – that travel from one position and rotation ...
Represents a spline for the rotation of a rigid body from one orientation in space to another over a ...
Represents a spline that travels from one point in space to another over a specified time frame....