1// tSpline.cpp 
2// 
3// Implementations for splines and paths with various degrees of smoothness. A 'path', or 'spline', is arbitrarily long 
4// and may be composed of smaller path sections called 'curves'. For example, a Bezier path is made from multiple 
5// Bezier curves. Implementations for piecewise-linear paths, Hermite/Bezier curves and paths, and Nonuniform 
6// Nonrational Cubic-Basis splines can be found in this file. 
7// 
8// Regarding naming, the word 'spline' refers to any path that is composed of piecewise parts. Strictly speaking one 
9// could call a composite of multiple Bezier curves a 'Bezier Spline' but it is not a common term. In this file the 
10// word 'path' is used for a composite of Bezier curves or a composite of line segments, and we reserve the word spline 
11// for paths composed of multiple cubic polynomial pieces. 
12// 
13// Copyright (c) 2006, 2017, 2020 Tristan Grimmer. 
14// Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby 
15// granted, provided that the above copyright notice and this permission notice appear in all copies. 
16// 
17// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL 
18// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, 
19// INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN 
20// AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR 
21// PERFORMANCE OF THIS SOFTWARE. 
22 
23#include <Foundation/tStandard.h> 
24#include "Math/tSpline.h" 
25 
26 
27void tMath::tPiecewiseLinearPath::SetPoints(const tVector3* points, int numPoints) 
28{ 
29 tAssert(numPoints > 1); 
30 Clear(); 
31 NumPoints = numPoints; 
32 Points = new tVector3[numPoints]; 
33 tStd::tMemcpy(Points, points, numPoints*sizeof(tVector3)); 
34 PercentagePoints = new float[numPoints]; 
35 
36 // Fill in the percentage points array with distances. 
37 PercentagePoints[0] = 0.0f; 
38 TotalLength = 0.0f; 
39 for (int p = 0; p < NumPoints-1; p++) 
40 { 
41 TotalLength += tDistBetween(Points[p], Points[p+1]); 
42 PercentagePoints[p+1] = TotalLength; 
43 } 
44 
45 // Now modify the percentage points array so it represents percentages. 
46 PercentagePoints[NumPoints-1] = 1.0f; 
47 for (int p = 1; p < NumPoints-1; p++) 
48 { 
49 if (TotalLength) 
50 PercentagePoints[p] /= TotalLength; 
51 else 
52 PercentagePoints[p] = 0.0f; 
53 } 
54} 
55 
56 
57tMath::tVector3 tMath::tPiecewiseLinearPath::GetPoint(float t) const 
58{ 
59 if (!IsValid()) 
60 return tVector3(0.0f, 0.0f, 0.0f); 
61 
62 // We need the slow truncating cast here. 
63 int firstIndex = int(t); 
64 int lastIndex = firstIndex + 1; 
65 if (t < 0.0f) 
66 { 
67 firstIndex = 0; 
68 lastIndex = 1; 
69 } 
70 else if (t > float(NumPoints - 1)) 
71 { 
72 firstIndex = NumPoints - 2; 
73 lastIndex = NumPoints - 1; 
74 } 
75 
76 t -= float(firstIndex); 
77 tVector3& first = Points[firstIndex]; 
78 tVector3& last = Points[lastIndex]; 
79 
80 float x = tLisc(t, first.x, last.x); 
81 float y = tLisc(t, first.y, last.y); 
82 float z = tLisc(t, first.z, last.z); 
83 
84 return tVector3(x, y, z); 
85} 
86 
87 
88tMath::tVector3 tMath::tPiecewiseLinearPath::GetPercentPoint(float t) const 
89{ 
90 if (!IsValid()) 
91 return tVector3::zero; 
92 
93 tiSaturate(t); 
94 tVector3* a = 0; 
95 tVector3* b = 0; 
96 float pa = 0.0f; 
97 float pb = 0.0f; 
98 
99 for (int p = 0; p < NumPoints-1; p++) 
100 { 
101 pa = PercentagePoints[p]; 
102 pb = PercentagePoints[p+1]; 
103 if ((t >= pa) && (t <= pb)) 
104 { 
105 a = Points + p; 
106 b = Points + p + 1; 
107 break; 
108 } 
109 } 
110 tAssert(a && b); 
111 
112 t = (t-pa) / (pb-pa); 
113 float x = tLisc(t, a->x, b->x); 
114 float y = tLisc(t, a->y, b->y); 
115 float z = tLisc(t, a->z, b->z); 
116 
117 return tVector3(x, y, z); 
118} 
119 
120 
121void tMath::tBezierCurve::GetPoint(tVector3& point, float t) const 
122{ 
123 tAssert((t >= 0.0f) && (t <= 1.0f)); 
124 float c = 1.0f - t; 
125 
126 // The Bernstein polynomials. 
127 float bb0 = c*c*c; 
128 float bb1 = 3*t*c*c; 
129 float bb2 = 3*t*t*c; 
130 float bb3 = t*t*t; 
131 
132 point = ControlVerts[0]*bb0 + ControlVerts[1]*bb1 + ControlVerts[2]*bb2 + ControlVerts[3]*bb3; 
133} 
134 
135 
136void tMath::tBezierCurve::GetTangent(tVector3& v, float t) const 
137{ 
138 // See: http://bimixual.org/AnimationLibrary/beziertangents.html 
139 tAssert((t >= 0.0f) && (t <= 1.0f)); 
140 
141 const tVector3 q0 = ControlVerts[0] + ((ControlVerts[1] - ControlVerts[0]) * t); 
142 const tVector3 q1 = ControlVerts[1] + ((ControlVerts[2] - ControlVerts[1]) * t); 
143 const tVector3 q2 = ControlVerts[2] + ((ControlVerts[3] - ControlVerts[2]) * t); 
144 
145 const tVector3 r0 = q0 + ((q1 - q0) * t); 
146 const tVector3 r1 = q1 + ((q2 - q1) * t); 
147 
148 v = r1 - r0; 
149} 
150 
151 
152float tMath::tBezierCurve::GetClosestParamRec(const tVector3& p, tComponents components, float start, float end, float threshold) const 
153{ 
154 float mid = (start + end)/2.0f; 
155 
156 // Base case for recursion. 
157 if ((end - start) < threshold) 
158 return mid; 
159 
160 // The two halves have param range [start, mid] and [mid, end]. We decide which one to use by using a midpoint param 
161 // calculation for each section. 
162 float paramA = (start+mid) / 2.0f; 
163 float paramB = (mid+end) / 2.0f; 
164  
165 tVector3 pos(p); 
166 pos.Zero(~components); 
167 
168 tVector3 posA; 
169 GetPoint(posA, paramA); 
170 posA.Zero(~components); 
171 
172 tVector3 posB; 
173 GetPoint(posB, paramB); 
174 posB.Zero(~components); 
175 
176 float distASq = (posA - pos).LengthSq(); 
177 float distBSq = (posB - pos).LengthSq(); 
178 
179 if (distASq < distBSq) 
180 end = mid; 
181 else 
182 start = mid; 
183 
184 // The (tail) recursive call. 
185 return GetClosestParamRec(p, components, start, end, threshold); 
186} 
187 
188 
189tMath::tComponents tMath::tBezierPath::tFastSectionState::CompareComponents = tMath::tComponent_All; 
190tMath::tVector3 tMath::tBezierPath::tFastSectionState::ComparePos = tVector3::zero; 
191const tMath::tVector3* tMath::tBezierPath::tFastSectionState::CompareControlVerts = nullptr; 
192 
193 
194tMath::tBezierPath::tBezierPath(const tBezierPath& src) : 
195 Mode(src.Mode), 
196 Type(src.Type), 
197 NumCurveSegments(src.NumCurveSegments), 
198 NumControlVerts(src.NumControlVerts) 
199{ 
200 if (!src.IsValid()) 
201 { 
202 ControlVerts = nullptr; 
203 return; 
204 } 
205 
206 switch (Mode) 
207 { 
208 case tMode::InternalCVs: 
209 ControlVerts = new tVector3[NumControlVerts]; 
210 tStd::tMemcpy(ControlVerts, src.ControlVerts, sizeof(tVector3) * NumControlVerts); 
211 break; 
212 
213 case tMode::ExternalCVs: 
214 ControlVerts = src.ControlVerts; 
215 break; 
216 } 
217} 
218 
219 
220void tMath::tBezierPath::Clear() 
221{ 
222 if (Mode == tMode::InternalCVs) 
223 delete[] ControlVerts; 
224 ControlVerts = 0; 
225 
226 Mode = tMode::InternalCVs; 
227 Type = tType::Open; 
228 NumCurveSegments = 0; 
229 NumControlVerts = 0; 
230} 
231 
232 
233float tMath::tBezierPath::ComputeApproxParamPerCoordinateUnit(tComponents components) const 
234{ 
235 if (!IsValid()) 
236 return 0.0f; 
237 
238 // For a closed path this still works if you consider the last point as separate from the first. That is, a closed 
239 // path is just like an open except the last interpolated point happens to match the first. 
240 int numInterpolatedPoints = NumCurveSegments + 1; 
241 if (numInterpolatedPoints < 2) 
242 return 0.0f; 
243 
244 float totalDist = 0.0f; 
245 for (int n = 1; n < numInterpolatedPoints; n++) 
246 { 
247 tVector3 a = ControlVerts[(n-1)*3]; 
248 tVector3 b = ControlVerts[n*3]; 
249 a.Zero(~components); 
250 b.Zero(~components); 
251 totalDist += tDistBetween(a, b); 
252 } 
253 
254 if (totalDist == 0.0f) 
255 return 0.0f; 
256 
257 return float(NumCurveSegments) / totalDist; 
258} 
259 
260 
261void tMath::tBezierPath::InterpolatePoints(const tVector3* knots, int numKnots, tType type) 
262{ 
263 tAssert(numKnots >= 2); 
264 Clear(); 
265 
266 Mode = tMode::InternalCVs; 
267 Type = type; 
268 switch (Type) 
269 { 
270 case tType::Open: 
271 { 
272 NumCurveSegments = numKnots - 1; 
273 NumControlVerts = 3*numKnots - 2; 
274 ControlVerts = new tVector3[NumControlVerts]; 
275 
276 // Place the interpolated CVs. 
277 for (int n = 0; n < numKnots; n++) 
278 ControlVerts[n*3] = knots[n]; 
279 
280 // Place the first and last non-interploated CVs. 
281 tVector3 initialPoint = (knots[1] - knots[0]) * 0.25f; 
282 
283 // Interp 1/4 away along first segment. 
284 ControlVerts[1] = knots[0] + initialPoint; 
285 tVector3 finalPoint = (knots[numKnots-2] - knots[numKnots-1]) * 0.25f; 
286 
287 // Interp 1/4 backward along last segment. 
288 ControlVerts[NumControlVerts-2] = knots[numKnots-1] + finalPoint; 
289 
290 // Now we'll do all the interior non-interpolated CVs. 
291 for (int k = 1; k < NumCurveSegments; k++) 
292 { 
293 tVector3 a = knots[k-1] - knots[k]; 
294 tVector3 b = knots[k+1] - knots[k]; 
295 
296 float aLen = a.Length(); 
297 float bLen = b.Length(); 
298 
299 if (aLen && bLen) 
300 { 
301 float abLen = (aLen + bLen) / 8.0f; 
302 tVector3 ab = (b/bLen) - (a/aLen); 
303 ab.Normalize(); 
304 ab *= abLen; 
305 
306 ControlVerts[k*3 - 1] = knots[k] - ab; 
307 ControlVerts[k*3 + 1] = knots[k] + ab; 
308 } 
309 else 
310 { 
311 ControlVerts[k*3 - 1] = knots[k]; 
312 ControlVerts[k*3 + 1] = knots[k]; 
313 } 
314 } 
315 break; 
316 } 
317 
318 case tType::Closed: 
319 { 
320 NumCurveSegments = numKnots; 
321 
322 // We duplicate the first point at the end so we have contiguous memory to look of the curve value. That's 
323 // what the +1 is for. 
324 NumControlVerts = 3*numKnots + 1; 
325 ControlVerts = new tVector3[NumControlVerts]; 
326 
327 // First lets place the interpolated CVs and duplicate the first into the last CV slot. 
328 for (int n = 0; n < numKnots; n++) 
329 ControlVerts[n*3] = knots[n]; 
330 
331 ControlVerts[NumControlVerts-1] = knots[0]; 
332 
333 // Now we'll do all the interior non-interpolated CVs. We go to k=NumCurveSegments which will compute the 
334 // two CVs around the zeroeth knot (points[0]). 
335 for (int k = 1; k <= NumCurveSegments; k++) 
336 { 
337 int modkm1 = k-1; 
338 int modkp1 = (k+1) % NumCurveSegments; 
339 int modk = k % NumCurveSegments; 
340 
341 tVector3 a = knots[modkm1] - knots[modk]; 
342 tVector3 b = knots[modkp1] - knots[modk]; 
343 
344 float aLen = a.Length(); 
345 float bLen = b.Length(); 
346 
347 int mod3km1 = 3*k - 1; 
348 
349 // Need the -1 so the end point is a duplicated start point. 
350 int mod3kp1 = (3*k + 1) % (NumControlVerts-1); 
351 if (aLen && bLen) 
352 { 
353 float abLen = (aLen + bLen) / 8.0f; 
354 tVector3 ab = (b/bLen) - (a/aLen); 
355 ab.Normalize(); 
356 ab *= abLen; 
357 
358 ControlVerts[mod3km1] = knots[modk] - ab; 
359 ControlVerts[mod3kp1] = knots[modk] + ab; 
360 } 
361 else 
362 { 
363 ControlVerts[mod3km1] = knots[modk]; 
364 ControlVerts[mod3kp1] = knots[modk]; 
365 } 
366 } 
367 break; 
368 } 
369 } 
370} 
371 
372 
373void tMath::tBezierPath::SetControlVerts(const tVector3* cvs, int numCVs, tMode mode, tType type) 
374{ 
375 tAssert(cvs); 
376 tAssert( ((type == tType::Open) && (numCVs >= 4)) || ((type == tType::Closed) && (numCVs >= 7)) ); 
377 tAssert(((numCVs-1) % 3) == 0); 
378 
379 Clear(); 
380 Mode = mode; 
381 Type = type; 
382 
383 NumControlVerts = numCVs; 
384 NumCurveSegments = ((numCVs - 1) / 3); 
385 switch (Mode) 
386 { 
387 case tMode::InternalCVs: 
388 ControlVerts = new tVector3[NumControlVerts]; 
389 tStd::tMemcpy(ControlVerts, cvs, sizeof(tVector3) * NumControlVerts); 
390 break; 
391 
392 case tMode::ExternalCVs: 
393 // Since the mode is external we know they won't be deleted so it's safe to cast the const away. 
394 ControlVerts = (tVector3*)cvs; 
395 break; 
396 } 
397} 
398 
399 
400void tMath::tBezierPath::GetPoint(tVector3& point, float t) const 
401{ 
402 if (!IsValid()) 
403 return; 
404 
405 // Only closed paths accept t values out of range. 
406 if (Type == tType::Closed) 
407 { 
408 while (t < 0.0f) 
409 t += float(NumCurveSegments); 
410 
411 while (t > float(NumCurveSegments)) 
412 t -= float(NumCurveSegments); 
413 } 
414 else 
415 { 
416 tiClamp(t, 0.0f, float(NumCurveSegments)); 
417 } 
418 
419 // Segment 0 is for t E [0, 1). The last segment is for t E [NumCurveSegments-1, NumCurveSegments]. 
420 // The following 'if' statement deals with the final inclusive bracket on the last segment. The cast must truncate. 
421 int segment = int(t); 
422 if (segment >= NumCurveSegments) 
423 segment = NumCurveSegments - 1; 
424 
425 tBezierCurve bc( &ControlVerts[3*segment] ); 
426 bc.GetPoint(point, t - float(segment)); 
427} 
428 
429 
430void tMath::tBezierPath::GetTangent(tVector3& v, float t) const 
431{ 
432 if (!IsValid()) 
433 return; 
434 
435 // Only closed paths accept t values out of range. 
436 if (Type == tType::Closed) 
437 { 
438 while (t < 0.0f) 
439 t += float(NumCurveSegments); 
440 
441 while (t > float(NumCurveSegments)) 
442 t -= float(NumCurveSegments); 
443 } 
444 else 
445 { 
446 tiClamp(t, 0.0f, float(NumCurveSegments)); 
447 } 
448 
449 // Segment 0 is for t E [0, 1). The last segment is for t E [NumCurveSegments-1, NumCurveSegments]. 
450 // The following 'if' statement deals with the final inclusive bracket on the last segment. The cast must truncate. 
451 int segment = int(t); 
452 if (segment >= NumCurveSegments) 
453 segment = NumCurveSegments - 1; 
454 
455 tBezierCurve bc( &ControlVerts[3*segment] ); 
456 bc.GetTangent(v, t - float(segment)); 
457} 
458 
459 
460bool tMath::tBezierPath::tFastSectionState::CompareSections(const tSectionInfo& a, const tSectionInfo& b) 
461{ 
462 // My attempt at a stable section sorting function. Note that the versions that used all 4 points individually 
463 // fail to be stable because they are dependent on the particular sections being compared. 
464 tVector3 approxAvgA = (CompareControlVerts[a.StartIndex] + CompareControlVerts[a.StartIndex + 3]) / 2.0f; 
465 tVector3 approxAvgB = (CompareControlVerts[b.StartIndex] + CompareControlVerts[b.StartIndex + 3]) / 2.0f; 
466 
467 approxAvgA.Zero(~CompareComponents); 
468 approxAvgB.Zero(~CompareComponents); 
469 
470 float distSqA = tDistBetweenSq(ComparePos, approxAvgA); 
471 float distSqB = tDistBetweenSq(ComparePos, approxAvgB); 
472 return distSqA < distSqB; 
473} 
474 
475 
476void tMath::tBezierPath::tFastSectionState::Set(const tVector3& pos, tComponents components, const tVector3* cvs, int numCVs) const 
477{ 
478 Clear(); 
479 Components = components; 
480 for (int p = 0; p < numCVs - 1; p += 3) 
481 Sections.Append(new tSectionInfo(p)); 
482 
483 // @todo There's gotta be a more modern way to do this. Maybe with a closure. 
484 CompareComponents = Components; 
485 ComparePos = pos; 
486 CompareControlVerts = cvs; 
487 Sections.Sort(CompareSections, tListSortAlgorithm::Merge); 
488} 
489 
490 
491void tMath::tBezierPath::tFastSectionState::Update(const tVector3& pos, const tVector3* cvs) const 
492{ 
493 CompareComponents = Components; 
494 ComparePos = pos; 
495 CompareControlVerts = cvs; 
496 Sections.Sort(CompareSections, tListSortAlgorithm::Bubble); 
497} 
498 
499 
500float tMath::tBezierPath::GetClosestParam(const tVector3& p, tComponents components, float paramThreshold, const tBezierPath::tFastSectionState& optObj) const 
501{ 
502 // Can we use the supplied FastSectionState if it has the correct components, number of sections, and CVs. If 
503 // anything is wrong, we simply clear the optimization object for next time. 
504 if (((optObj.Sections.GetNumItems() * 3) + 1) != NumControlVerts) 
505 optObj.Clear(); 
506 
507 if (optObj.Components != components) 
508 optObj.Clear(); 
509 
510 if (!optObj.Sections.GetNumItems()) 
511 optObj.Set(p, components, ControlVerts, NumControlVerts); 
512 else 
513 optObj.Update(p, ControlVerts); 
514 
515 // We try the first 3 sections. In most cases the closest will be in the first but due to the nature of the sort 
516 // function, it is prudent to check one on either side. 
517 tFastSectionState::tSectionInfo* section = optObj.Sections.First(); 
518 float minDistSq = MaxFloat; 
519 float closestParam = 0.0f; 
520 tVector3 pos(p); 
521 pos.Zero(~components); 
522 
523 for (int s = 0; (s < 3) && section; s++, section = section->Next()) 
524 { 
525 tBezierCurve curve(ControlVerts + section->StartIndex); 
526 float curveClosestParam = curve.GetClosestParam(pos, components, paramThreshold); 
527 
528 tVector3 curvePos; 
529 curve.GetPoint(curvePos, curveClosestParam); 
530 curvePos.Zero(~components); 
531 float distSq = (curvePos - pos).LengthSq(); 
532 if (distSq < minDistSq) 
533 { 
534 minDistSq = distSq; 
535 float startParam = float(section->StartIndex) / 3.0f; 
536 closestParam = startParam + curveClosestParam; 
537 } 
538 } 
539 
540 return closestParam; 
541} 
542 
543 
544tMath::tNNBSpline::tNNBSpline(int* knotValueSequence, tVector3* controlPoints, int numControlPoints, float paramRange) : 
545 KVS(knotValueSequence) 
546{ 
547 tAssert(!"tNNBSpline not implemented."); 
548} 
549 
550 
551tMath::tNNBSpline::~tNNBSpline() 
552{ 
553} 
554 
555 
556tMath::tVector3 tMath::tNNBSpline::Q(int i, float t) 
557{ 
558 tVector3 t1 = CPs[i-3] * B4(i-3, t); 
559 tVector3 t2 = CPs[i-2] * B4(i-2, t); 
560 tVector3 t3 = CPs[i-1] * B4(i-1, t); 
561 tVector3 t4 = CPs[i ] * B4(i , t); 
562 
563 return t1 + t2 + t3 + t4; 
564} 
565 
566 
567float tMath::tNNBSpline::B1(int i, float t) 
568{ 
569 if ( (KVS[i] <= t) && (t < KVS[i+1]) ) 
570 return 1.0f; 
571 else 
572 return 0.0f; 
573} 
574 
575 
576float tMath::tNNBSpline::B2(int i, float t) 
577{ 
578 int ti0 = KVS[i]; 
579 int ti1 = KVS[i+1]; 
580 int ti2 = KVS[i+2]; 
581 
582 int denom1 = ti1-ti0; 
583 int denom2 = ti2-ti1; 
584 
585 float term1 = 0; 
586 if (denom1) 
587 term1 = B1(i,t) * (t-ti0)/float(denom1); 
588 
589 float term2 = 0; 
590 if (denom2) 
591 term2 = B1(i+1, t) * (ti2-t)/float(denom2); 
592 
593 return term1 + term2; 
594} 
595 
596 
597float tMath::tNNBSpline::B3(int i, float t) 
598{ 
599 int ti0 = KVS[i]; 
600 int ti1 = KVS[i+1]; 
601 int ti2 = KVS[i+2]; 
602 int ti3 = KVS[i+3]; 
603 
604 int denom1 = ti2-ti0; 
605 int denom2 = ti3-ti1; 
606 
607 float term1 = 0; 
608 if (denom1) 
609 term1 = B2(i,t) * (t-ti0)/float(denom1); 
610 
611 float term2 = 0; 
612 if (denom2) 
613 term2 = B2(i+1, t) * (ti3-t)/float(denom2); 
614 
615 return term1 + term2; 
616} 
617 
618 
619float tMath::tNNBSpline::B4(int i, float t) 
620{ 
621 int ti0 = KVS[i]; 
622 int ti1 = KVS[i+1]; 
623 int ti3 = KVS[i+2]; 
624 int ti4 = KVS[i+3]; 
625 
626 int denom1 = ti3-ti0; 
627 int denom2 = ti4-ti1; 
628 
629 float term1 = 0; 
630 if (denom1) 
631 term1 = B3(i,t) * (t-ti0)/float(denom1); 
632 
633 float term2 = 0; 
634 if (denom2) 
635 term2 = B3(i+1, t) * (ti4-t)/float(denom2); 
636 
637 return term1 + term2; 
638} 
639