1use super::{SampleDerivative, SampleTwoDerivatives};
5use crate::common_traits::{HasTangent, Sum, VectorSpace, WithDerivative, WithTwoDerivatives};
6use crate::curve::{
7 adaptors::{
8 ChainCurve, ConstantCurve, ContinuationCurve, CurveReparamCurve, ForeverCurve, GraphCurve,
9 LinearReparamCurve, PingPongCurve, RepeatCurve, ReverseCurve, ZipCurve,
10 },
11 Curve,
12};
13
14impl<T> SampleDerivative<T> for ConstantCurve<T>
17where
18 T: HasTangent + Clone,
19{
20 fn sample_with_derivative_unchecked(&self, _t: f32) -> WithDerivative<T> {
21 WithDerivative {
22 value: self.value.clone(),
23 derivative: VectorSpace::ZERO,
24 }
25 }
26}
27
28impl<T> SampleTwoDerivatives<T> for ConstantCurve<T>
29where
30 T: HasTangent + Clone,
31{
32 fn sample_with_two_derivatives_unchecked(&self, _t: f32) -> WithTwoDerivatives<T> {
33 WithTwoDerivatives {
34 value: self.value.clone(),
35 derivative: VectorSpace::ZERO,
36 second_derivative: VectorSpace::ZERO,
37 }
38 }
39}
40
41impl<T, C, D> SampleDerivative<T> for ChainCurve<T, C, D>
44where
45 T: HasTangent,
46 C: SampleDerivative<T>,
47 D: SampleDerivative<T>,
48{
49 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
50 if t > self.first.domain().end() {
51 self.second.sample_with_derivative_unchecked(
52 t - self.first.domain().end() + self.second.domain().start(),
54 )
55 } else {
56 self.first.sample_with_derivative_unchecked(t)
57 }
58 }
59}
60
61impl<T, C, D> SampleTwoDerivatives<T> for ChainCurve<T, C, D>
62where
63 T: HasTangent,
64 C: SampleTwoDerivatives<T>,
65 D: SampleTwoDerivatives<T>,
66{
67 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
68 if t > self.first.domain().end() {
69 self.second.sample_with_two_derivatives_unchecked(
70 t - self.first.domain().end() + self.second.domain().start(),
72 )
73 } else {
74 self.first.sample_with_two_derivatives_unchecked(t)
75 }
76 }
77}
78
79impl<T, C, D> SampleDerivative<T> for ContinuationCurve<T, C, D>
82where
83 T: VectorSpace,
84 C: SampleDerivative<T>,
85 D: SampleDerivative<T>,
86{
87 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
88 if t > self.first.domain().end() {
89 let mut output = self.second.sample_with_derivative_unchecked(
90 t - self.first.domain().end() + self.second.domain().start(),
92 );
93 output.value = output.value + self.offset;
94 output
95 } else {
96 self.first.sample_with_derivative_unchecked(t)
97 }
98 }
99}
100
101impl<T, C, D> SampleTwoDerivatives<T> for ContinuationCurve<T, C, D>
102where
103 T: VectorSpace,
104 C: SampleTwoDerivatives<T>,
105 D: SampleTwoDerivatives<T>,
106{
107 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
108 if t > self.first.domain().end() {
109 let mut output = self.second.sample_with_two_derivatives_unchecked(
110 t - self.first.domain().end() + self.second.domain().start(),
112 );
113 output.value = output.value + self.offset;
114 output
115 } else {
116 self.first.sample_with_two_derivatives_unchecked(t)
117 }
118 }
119}
120
121impl<T, C> SampleDerivative<T> for RepeatCurve<T, C>
124where
125 T: HasTangent,
126 C: SampleDerivative<T>,
127{
128 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
129 let t = self.base_curve_sample_time(t);
130 self.curve.sample_with_derivative_unchecked(t)
131 }
132}
133
134impl<T, C> SampleTwoDerivatives<T> for RepeatCurve<T, C>
135where
136 T: HasTangent,
137 C: SampleTwoDerivatives<T>,
138{
139 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
140 let t = self.base_curve_sample_time(t);
141 self.curve.sample_with_two_derivatives_unchecked(t)
142 }
143}
144
145impl<T, C> SampleDerivative<T> for ForeverCurve<T, C>
148where
149 T: HasTangent,
150 C: SampleDerivative<T>,
151{
152 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
153 let t = self.base_curve_sample_time(t);
154 self.curve.sample_with_derivative_unchecked(t)
155 }
156}
157
158impl<T, C> SampleTwoDerivatives<T> for ForeverCurve<T, C>
159where
160 T: HasTangent,
161 C: SampleTwoDerivatives<T>,
162{
163 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
164 let t = self.base_curve_sample_time(t);
165 self.curve.sample_with_two_derivatives_unchecked(t)
166 }
167}
168
169impl<T, C> SampleDerivative<T> for PingPongCurve<T, C>
172where
173 T: HasTangent,
174 C: SampleDerivative<T>,
175{
176 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
177 if t > self.curve.domain().end() {
178 let t = self.curve.domain().end() * 2.0 - t;
179 let mut output = self.curve.sample_with_derivative_unchecked(t);
182 output.derivative = -output.derivative;
183 output
184 } else {
185 self.curve.sample_with_derivative_unchecked(t)
186 }
187 }
188}
189
190impl<T, C> SampleTwoDerivatives<T> for PingPongCurve<T, C>
191where
192 T: HasTangent,
193 C: SampleTwoDerivatives<T>,
194{
195 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
196 if t > self.curve.domain().end() {
197 let t = self.curve.domain().end() * 2.0 - t;
198 let mut output = self.curve.sample_with_two_derivatives_unchecked(t);
201 output.derivative = -output.derivative;
202 output
203 } else {
204 self.curve.sample_with_two_derivatives_unchecked(t)
205 }
206 }
207}
208
209impl<U, V, S, T, C, D> SampleDerivative<(S, T)> for ZipCurve<S, T, C, D>
212where
213 U: VectorSpace<Scalar = f32>,
214 V: VectorSpace<Scalar = f32>,
215 S: HasTangent<Tangent = U>,
216 T: HasTangent<Tangent = V>,
217 C: SampleDerivative<S>,
218 D: SampleDerivative<T>,
219{
220 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<(S, T)> {
221 let first_output = self.first.sample_with_derivative_unchecked(t);
222 let second_output = self.second.sample_with_derivative_unchecked(t);
223 WithDerivative {
224 value: (first_output.value, second_output.value),
225 derivative: Sum(first_output.derivative, second_output.derivative),
226 }
227 }
228}
229
230impl<U, V, S, T, C, D> SampleTwoDerivatives<(S, T)> for ZipCurve<S, T, C, D>
231where
232 U: VectorSpace<Scalar = f32>,
233 V: VectorSpace<Scalar = f32>,
234 S: HasTangent<Tangent = U>,
235 T: HasTangent<Tangent = V>,
236 C: SampleTwoDerivatives<S>,
237 D: SampleTwoDerivatives<T>,
238{
239 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<(S, T)> {
240 let first_output = self.first.sample_with_two_derivatives_unchecked(t);
241 let second_output = self.second.sample_with_two_derivatives_unchecked(t);
242 WithTwoDerivatives {
243 value: (first_output.value, second_output.value),
244 derivative: Sum(first_output.derivative, second_output.derivative),
245 second_derivative: Sum(
246 first_output.second_derivative,
247 second_output.second_derivative,
248 ),
249 }
250 }
251}
252
253impl<V, T, C> SampleDerivative<(f32, T)> for GraphCurve<T, C>
256where
257 V: VectorSpace<Scalar = f32>,
258 T: HasTangent<Tangent = V>,
259 C: SampleDerivative<T>,
260{
261 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<(f32, T)> {
262 let output = self.base.sample_with_derivative_unchecked(t);
263 WithDerivative {
264 value: (t, output.value),
265 derivative: Sum(1.0, output.derivative),
266 }
267 }
268}
269
270impl<V, T, C> SampleTwoDerivatives<(f32, T)> for GraphCurve<T, C>
271where
272 V: VectorSpace<Scalar = f32>,
273 T: HasTangent<Tangent = V>,
274 C: SampleTwoDerivatives<T>,
275{
276 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<(f32, T)> {
277 let output = self.base.sample_with_two_derivatives_unchecked(t);
278 WithTwoDerivatives {
279 value: (t, output.value),
280 derivative: Sum(1.0, output.derivative),
281 second_derivative: Sum(0.0, output.second_derivative),
282 }
283 }
284}
285
286impl<T, C> SampleDerivative<T> for ReverseCurve<T, C>
289where
290 T: HasTangent,
291 C: SampleDerivative<T>,
292{
293 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
294 let mut output = self
297 .curve
298 .sample_with_derivative_unchecked(self.domain().end() - (t - self.domain().start()));
299
300 output.derivative = -output.derivative;
301
302 output
303 }
304}
305
306impl<T, C> SampleTwoDerivatives<T> for ReverseCurve<T, C>
307where
308 T: HasTangent,
309 C: SampleTwoDerivatives<T>,
310{
311 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
312 let mut output = self.curve.sample_with_two_derivatives_unchecked(
315 self.domain().end() - (t - self.domain().start()),
316 );
317
318 output.derivative = -output.derivative;
319
320 output
325 }
326}
327
328impl<V, T, C, D> SampleDerivative<T> for CurveReparamCurve<T, C, D>
331where
332 V: VectorSpace<Scalar = f32>,
333 T: HasTangent<Tangent = V>,
334 C: SampleDerivative<T>,
335 D: SampleDerivative<f32>,
336{
337 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
338 let reparam_output = self.reparam_curve.sample_with_derivative_unchecked(t);
343
344 let mut output = self
348 .base
349 .sample_with_derivative_unchecked(reparam_output.value);
350
351 output.derivative = output.derivative * reparam_output.derivative;
353
354 output
356 }
357}
358
359impl<V, T, C, D> SampleTwoDerivatives<T> for CurveReparamCurve<T, C, D>
360where
361 V: VectorSpace<Scalar = f32>,
362 T: HasTangent<Tangent = V>,
363 C: SampleTwoDerivatives<T>,
364 D: SampleTwoDerivatives<f32>,
365{
366 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
367 let reparam_output = self.reparam_curve.sample_with_two_derivatives_unchecked(t);
372
373 let mut output = self
378 .base
379 .sample_with_two_derivatives_unchecked(reparam_output.value);
380
381 output.second_derivative = (output.second_derivative
384 * (reparam_output.derivative * reparam_output.derivative))
385 + (output.derivative * reparam_output.second_derivative);
386
387 output.derivative = output.derivative * reparam_output.derivative;
390
391 output
392 }
393}
394
395impl<V, T, C> SampleDerivative<T> for LinearReparamCurve<T, C>
398where
399 V: VectorSpace<Scalar = f32>,
400 T: HasTangent<Tangent = V>,
401 C: SampleDerivative<T>,
402{
403 fn sample_with_derivative_unchecked(&self, t: f32) -> WithDerivative<T> {
404 let g = self.new_domain.linear_map_to(self.base.domain()).unwrap();
409
410 let g_derivative = self.base.domain().length() / self.new_domain.length();
412
413 let mut output = self.base.sample_with_derivative_unchecked(g(t));
417
418 output.derivative = output.derivative * g_derivative;
420
421 output
422 }
423}
424
425impl<V, T, C> SampleTwoDerivatives<T> for LinearReparamCurve<T, C>
426where
427 V: VectorSpace<Scalar = f32>,
428 T: HasTangent<Tangent = V>,
429 C: SampleTwoDerivatives<T>,
430{
431 fn sample_with_two_derivatives_unchecked(&self, t: f32) -> WithTwoDerivatives<T> {
432 let g = self.new_domain.linear_map_to(self.base.domain()).unwrap();
437
438 let g_derivative = self.base.domain().length() / self.new_domain.length();
440
441 let mut output = self.base.sample_with_two_derivatives_unchecked(g(t));
446
447 output.second_derivative = output.second_derivative * (g_derivative * g_derivative);
450
451 output.derivative = output.derivative * g_derivative;
454
455 output
456 }
457}
458
459#[cfg(test)]
460mod tests {
461
462 use approx::assert_abs_diff_eq;
463
464 use super::*;
465 use crate::cubic_splines::{CubicBezier, CubicCardinalSpline, CubicCurve, CubicGenerator};
466 use crate::curve::{Curve, CurveExt, Interval};
467 use crate::{vec2, Vec2, Vec3};
468
469 fn test_curve() -> CubicCurve<Vec2> {
470 let control_pts = [[
471 vec2(0.0, 0.0),
472 vec2(1.0, 0.0),
473 vec2(0.0, 1.0),
474 vec2(1.0, 1.0),
475 ]];
476
477 CubicBezier::new(control_pts).to_curve().unwrap()
478 }
479
480 fn other_test_curve() -> CubicCurve<Vec2> {
481 let control_pts = [
482 vec2(1.0, 1.0),
483 vec2(2.0, 1.0),
484 vec2(2.0, 0.0),
485 vec2(1.0, 0.0),
486 ];
487
488 CubicCardinalSpline::new(0.5, control_pts)
489 .to_curve()
490 .unwrap()
491 }
492
493 fn reparam_curve() -> CubicCurve<f32> {
494 let control_pts = [[0.0, 0.25, 0.75, 1.0]];
495
496 CubicBezier::new(control_pts).to_curve().unwrap()
497 }
498
499 #[test]
500 fn constant_curve() {
501 let curve = ConstantCurve::new(Interval::UNIT, Vec3::new(0.2, 1.5, -2.6));
502 let jet = curve.sample_with_derivative(0.5).unwrap();
503 assert_abs_diff_eq!(jet.derivative, Vec3::ZERO);
504 }
505
506 #[test]
507 fn chain_curve() {
508 let curve1 = test_curve();
509 let curve2 = other_test_curve();
510 let curve = curve1.by_ref().chain(&curve2).unwrap();
511
512 let jet = curve.sample_with_derivative(0.65).unwrap();
513 let true_jet = curve1.sample_with_derivative(0.65).unwrap();
514 assert_abs_diff_eq!(jet.value, true_jet.value);
515 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
516
517 let jet = curve.sample_with_derivative(1.1).unwrap();
518 let true_jet = curve2.sample_with_derivative(0.1).unwrap();
519 assert_abs_diff_eq!(jet.value, true_jet.value);
520 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
521 }
522
523 #[test]
524 fn continuation_curve() {
525 let curve1 = test_curve();
526 let curve2 = other_test_curve();
527 let curve = curve1.by_ref().chain_continue(&curve2).unwrap();
528
529 let jet = curve.sample_with_derivative(0.99).unwrap();
530 let true_jet = curve1.sample_with_derivative(0.99).unwrap();
531 assert_abs_diff_eq!(jet.value, true_jet.value);
532 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
533
534 let jet = curve.sample_with_derivative(1.3).unwrap();
535 let true_jet = curve2.sample_with_derivative(0.3).unwrap();
536 assert_abs_diff_eq!(jet.value, true_jet.value);
537 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
538 }
539
540 #[test]
541 fn repeat_curve() {
542 let curve1 = test_curve();
543 let curve = curve1.by_ref().repeat(3).unwrap();
544
545 let jet = curve.sample_with_derivative(0.73).unwrap();
546 let true_jet = curve1.sample_with_derivative(0.73).unwrap();
547 assert_abs_diff_eq!(jet.value, true_jet.value);
548 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
549
550 let jet = curve.sample_with_derivative(3.5).unwrap();
551 let true_jet = curve1.sample_with_derivative(0.5).unwrap();
552 assert_abs_diff_eq!(jet.value, true_jet.value);
553 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
554 }
555
556 #[test]
557 fn forever_curve() {
558 let curve1 = test_curve();
559 let curve = curve1.by_ref().forever().unwrap();
560
561 let jet = curve.sample_with_derivative(0.12).unwrap();
562 let true_jet = curve1.sample_with_derivative(0.12).unwrap();
563 assert_abs_diff_eq!(jet.value, true_jet.value);
564 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
565
566 let jet = curve.sample_with_derivative(500.5).unwrap();
567 let true_jet = curve1.sample_with_derivative(0.5).unwrap();
568 assert_abs_diff_eq!(jet.value, true_jet.value);
569 assert_abs_diff_eq!(jet.derivative, true_jet.derivative);
570 }
571
572 #[test]
573 fn ping_pong_curve() {
574 let curve1 = test_curve();
575 let curve = curve1.by_ref().ping_pong().unwrap();
576
577 let jet = curve.sample_with_derivative(0.99).unwrap();
578 let comparison_jet = curve1.sample_with_derivative(0.99).unwrap();
579 assert_abs_diff_eq!(jet.value, comparison_jet.value);
580 assert_abs_diff_eq!(jet.derivative, comparison_jet.derivative);
581
582 let jet = curve.sample_with_derivative(1.3).unwrap();
583 let comparison_jet = curve1.sample_with_derivative(0.7).unwrap();
584 assert_abs_diff_eq!(jet.value, comparison_jet.value);
585 assert_abs_diff_eq!(jet.derivative, -comparison_jet.derivative, epsilon = 1.0e-5);
586 }
587
588 #[test]
589 fn zip_curve() {
590 let curve1 = test_curve();
591 let curve2 = other_test_curve();
592 let curve = curve1.by_ref().zip(&curve2).unwrap();
593
594 let jet = curve.sample_with_derivative(0.7).unwrap();
595 let comparison_jet1 = curve1.sample_with_derivative(0.7).unwrap();
596 let comparison_jet2 = curve2.sample_with_derivative(0.7).unwrap();
597 assert_abs_diff_eq!(jet.value.0, comparison_jet1.value);
598 assert_abs_diff_eq!(jet.value.1, comparison_jet2.value);
599 let Sum(derivative1, derivative2) = jet.derivative;
600 assert_abs_diff_eq!(derivative1, comparison_jet1.derivative);
601 assert_abs_diff_eq!(derivative2, comparison_jet2.derivative);
602 }
603
604 #[test]
605 fn graph_curve() {
606 let curve1 = test_curve();
607 let curve = curve1.by_ref().graph();
608
609 let jet = curve.sample_with_derivative(0.25).unwrap();
610 let comparison_jet = curve1.sample_with_derivative(0.25).unwrap();
611 assert_abs_diff_eq!(jet.value.0, 0.25);
612 assert_abs_diff_eq!(jet.value.1, comparison_jet.value);
613 let Sum(one, derivative) = jet.derivative;
614 assert_abs_diff_eq!(one, 1.0);
615 assert_abs_diff_eq!(derivative, comparison_jet.derivative);
616 }
617
618 #[test]
619 fn reverse_curve() {
620 let curve1 = test_curve();
621 let curve = curve1.by_ref().reverse().unwrap();
622
623 let jet = curve.sample_with_derivative(0.23).unwrap();
624 let comparison_jet = curve1.sample_with_derivative(0.77).unwrap();
625 assert_abs_diff_eq!(jet.value, comparison_jet.value);
626 assert_abs_diff_eq!(jet.derivative, -comparison_jet.derivative);
627 }
628
629 #[test]
630 fn curve_reparam_curve() {
631 let reparam_curve = reparam_curve();
632 let reparam_jet = reparam_curve.sample_with_derivative(0.6).unwrap();
633
634 let curve1 = test_curve();
635 let curve = curve1.by_ref().reparametrize_by_curve(&reparam_curve);
636
637 let jet = curve.sample_with_derivative(0.6).unwrap();
638 let base_jet = curve1
639 .sample_with_derivative(reparam_curve.sample(0.6).unwrap())
640 .unwrap();
641 assert_abs_diff_eq!(jet.value, base_jet.value);
642 assert_abs_diff_eq!(jet.derivative, base_jet.derivative * reparam_jet.derivative);
643 }
644
645 #[test]
646 fn linear_reparam_curve() {
647 let curve1 = test_curve();
648 let curve = curve1
649 .by_ref()
650 .reparametrize_linear(Interval::new(0.0, 0.5).unwrap())
651 .unwrap();
652
653 let jet = curve.sample_with_derivative(0.23).unwrap();
654 let comparison_jet = curve1.sample_with_derivative(0.46).unwrap();
655 assert_abs_diff_eq!(jet.value, comparison_jet.value);
656 assert_abs_diff_eq!(jet.derivative, comparison_jet.derivative * 2.0);
657 }
658}