]> Dogcows Code - chaz/yoink/blob - src/cml/mathlib/interpolation.h
fixes for newer versions of g++
[chaz/yoink] / src / cml / mathlib / interpolation.h
1 /* -*- C++ -*- ------------------------------------------------------------
2
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
4
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
7
8 *-----------------------------------------------------------------------*/
9 /** @file
10 * @brief
11 */
12
13 #ifndef interpolation_h
14 #define interpolation_h
15
16 #include <cml/mathlib/matrix_rotation.h>
17
18 /* Interpolation functions.
19 *
20 * @todo: This code works, but it needs a lot of cleanup.
21 */
22
23 namespace cml {
24
25 struct function_expects_args_of_same_type_error;
26
27 namespace detail {
28
29 //////////////////////////////////////////////////////////////////////////////
30 // Helper struct to promote vectors, quaternions, and matrices
31 //////////////////////////////////////////////////////////////////////////////
32
33 template< class T1, class T2, class ResultT > struct TypePromote;
34
35 template< class T >
36 struct TypePromote< T,T,et::scalar_result_tag > {
37 typedef T temporary_type;
38 };
39
40 template< class T1, class T2 >
41 struct TypePromote< T1,T2,et::scalar_result_tag > {
42 typedef et::ExprTraits<T1> traits_1;
43 typedef et::ExprTraits<T2> traits_2;
44 typedef typename traits_1::result_tag result_type_1;
45 typedef typename traits_2::result_tag result_type_2;
46
47 /* Check that results are of the same type */
48 CML_STATIC_REQUIRE_M(
49 (same_type<result_type_1, result_type_2>::is_true),
50 function_expects_args_of_same_type_error);
51
52 typedef typename et::ScalarPromote<T1,T2>::type temporary_type;
53 };
54
55 template< class T1, class T2 >
56 struct TypePromote< T1,T2,et::vector_result_tag > {
57 typedef et::ExprTraits<T1> traits_1;
58 typedef et::ExprTraits<T2> traits_2;
59 typedef typename traits_1::result_tag result_type_1;
60 typedef typename traits_2::result_tag result_type_2;
61
62 /* Check that results are of the same type */
63 CML_STATIC_REQUIRE_M(
64 (same_type<result_type_1, result_type_2>::is_true),
65 function_expects_args_of_same_type_error);
66
67 /* @todo: This should be VectorPromote<> for symmetry with the other
68 * type promotions.
69 */
70 typedef typename CrossPromote<T1,T2>::promoted_vector temporary_type;
71 };
72
73 template< class T1, class T2 >
74 struct TypePromote< T1,T2,et::matrix_result_tag > {
75 typedef et::ExprTraits<T1> traits_1;
76 typedef et::ExprTraits<T2> traits_2;
77 typedef typename traits_1::result_tag result_type_1;
78 typedef typename traits_2::result_tag result_type_2;
79
80 /* Check that results are of the same type */
81 CML_STATIC_REQUIRE_M(
82 (same_type<result_type_1, result_type_2>::is_true),
83 function_expects_args_of_same_type_error);
84
85 typedef typename et::MatrixPromote2<T1,T2>::temporary_type temporary_type;
86 };
87
88 template< class T1, class T2 >
89 struct TypePromote< T1,T2,et::quaternion_result_tag > {
90 typedef et::ExprTraits<T1> traits_1;
91 typedef et::ExprTraits<T2> traits_2;
92 typedef typename traits_1::result_tag result_type_1;
93 typedef typename traits_2::result_tag result_type_2;
94
95 /* Check that results are of the same type */
96 CML_STATIC_REQUIRE_M(
97 (same_type<result_type_1, result_type_2>::is_true),
98 function_expects_args_of_same_type_error);
99
100 typedef typename et::QuaternionPromote2<T1,T2>::temporary_type
101 temporary_type;
102 };
103
104 template< class T1, class T2, class T3, class ResultT > struct TypePromote3;
105
106 template< class T1, class T2, class T3 >
107 struct TypePromote3< T1,T2,T3,et::matrix_result_tag > {
108 typedef et::ExprTraits<T1> traits_1;
109 typedef et::ExprTraits<T2> traits_2;
110 typedef et::ExprTraits<T3> traits_3;
111 typedef typename traits_1::result_tag result_type_1;
112 typedef typename traits_2::result_tag result_type_2;
113 typedef typename traits_3::result_tag result_type_3;
114
115 /* Check that results are of the same type */
116 CML_STATIC_REQUIRE_M(
117 (same_type<result_type_1, result_type_2>::is_true),
118 function_expects_args_of_same_type_error);
119 CML_STATIC_REQUIRE_M(
120 (same_type<result_type_1, result_type_3>::is_true),
121 function_expects_args_of_same_type_error);
122
123 typedef typename et::MatrixPromote3<T1,T2,T3>::temporary_type
124 temporary_type;
125 typedef typename temporary_type::value_type value_type;
126 };
127
128 template< class T1, class T2, class T3 >
129 struct TypePromote3< T1,T2,T3,et::quaternion_result_tag > {
130 typedef et::ExprTraits<T1> traits_1;
131 typedef et::ExprTraits<T2> traits_2;
132 typedef et::ExprTraits<T3> traits_3;
133 typedef typename traits_1::result_tag result_type_1;
134 typedef typename traits_2::result_tag result_type_2;
135 typedef typename traits_3::result_tag result_type_3;
136
137 /* Check that results are of the same type */
138 CML_STATIC_REQUIRE_M(
139 (same_type<result_type_1, result_type_2>::is_true),
140 function_expects_args_of_same_type_error);
141 CML_STATIC_REQUIRE_M(
142 (same_type<result_type_1, result_type_3>::is_true),
143 function_expects_args_of_same_type_error);
144
145 typedef typename et::QuaternionPromote3<T1,T2,T3>::temporary_type
146 temporary_type;
147 typedef typename temporary_type::value_type value_type;
148 };
149
150 template <
151 class T1, class T2, class T3, class T4, class ResultT
152 > struct TypePromote4;
153
154 template< class T1, class T2, class T3, class T4 >
155 struct TypePromote4< T1,T2,T3,T4,et::matrix_result_tag > {
156 typedef et::ExprTraits<T1> traits_1;
157 typedef et::ExprTraits<T2> traits_2;
158 typedef et::ExprTraits<T3> traits_3;
159 typedef et::ExprTraits<T4> traits_4;
160 typedef typename traits_1::result_tag result_type_1;
161 typedef typename traits_2::result_tag result_type_2;
162 typedef typename traits_3::result_tag result_type_3;
163 typedef typename traits_4::result_tag result_type_4;
164
165 /* Check that results are of the same type */
166 CML_STATIC_REQUIRE_M(
167 (same_type<result_type_1, result_type_2>::is_true),
168 function_expects_args_of_same_type_error);
169 CML_STATIC_REQUIRE_M(
170 (same_type<result_type_1, result_type_3>::is_true),
171 function_expects_args_of_same_type_error);
172 CML_STATIC_REQUIRE_M(
173 (same_type<result_type_1, result_type_4>::is_true),
174 function_expects_args_of_same_type_error);
175
176 typedef typename et::MatrixPromote4<T1,T2,T3,T4>::temporary_type
177 temporary_type;
178 typedef typename temporary_type::value_type value_type;
179 };
180
181 template< class T1, class T2, class T3, class T4 >
182 struct TypePromote4< T1,T2,T3,T4,et::quaternion_result_tag > {
183 typedef et::ExprTraits<T1> traits_1;
184 typedef et::ExprTraits<T2> traits_2;
185 typedef et::ExprTraits<T3> traits_3;
186 typedef et::ExprTraits<T4> traits_4;
187 typedef typename traits_1::result_tag result_type_1;
188 typedef typename traits_2::result_tag result_type_2;
189 typedef typename traits_3::result_tag result_type_3;
190 typedef typename traits_4::result_tag result_type_4;
191
192 /* Check that results are of the same type */
193 CML_STATIC_REQUIRE_M(
194 (same_type<result_type_1, result_type_2>::is_true),
195 function_expects_args_of_same_type_error);
196 CML_STATIC_REQUIRE_M(
197 (same_type<result_type_1, result_type_3>::is_true),
198 function_expects_args_of_same_type_error);
199 CML_STATIC_REQUIRE_M(
200 (same_type<result_type_1, result_type_4>::is_true),
201 function_expects_args_of_same_type_error);
202
203 typedef typename et::QuaternionPromote4<T1,T2,T3,T4>::temporary_type
204 temporary_type;
205 typedef typename temporary_type::value_type value_type;
206 };
207
208 //////////////////////////////////////////////////////////////////////////////
209 // Helper functions to resize a vector, quaternion or matrix
210 //////////////////////////////////////////////////////////////////////////////
211
212 // Should be able to catch all no-ops with a generic function template...
213
214 template < class T1, class T2, class SizeTag > void
215 InterpResize(T1& t1, const T2& t2, SizeTag) {}
216
217 // Catch vector and matrix resizes...
218
219 template< typename E, class A, class VecT > void
220 InterpResize(vector<E,A>& v, const VecT& target, dynamic_size_tag) {
221 v.resize(target.size());
222 }
223
224 template< typename E, class A, class B, class L, class MatT > void
225 InterpResize(matrix<E,A,B,L>& m, const MatT& target, dynamic_size_tag) {
226 m.resize(target.rows(),target.cols());
227 }
228
229 //////////////////////////////////////////////////////////////////////////////
230 // Construction of 'intermediate' quaternions and matrices for use with squad
231 //////////////////////////////////////////////////////////////////////////////
232
233 #if 0
234 template < class QuatT_1, class QuatT_2 >
235 typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
236 concatenate_quaternions(
237 const QuatT_1& q1,
238 const QuatT_2& q2,
239 positive_cross)
240 {
241 return q2 * q1;
242 }
243
244 template < class QuatT_1, class QuatT_2 >
245 typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
246 concatenate_quaternions(
247 const QuatT_1& q1,
248 const QuatT_2& q2,
249 negative_cross)
250 {
251 return q1 * q2;
252 }
253
254 template< class T1, class T2, class T3, class SizeT >
255 typename detail::TypePromote3<
256 T1,T2,T3,typename et::ExprTraits<T1>::result_tag
257 >::temporary_type
258 squad_intermediate(
259 const T1& t1,
260 const T2& t2,
261 const T3& t3,
262 typename detail::TypePromote3<
263 T1, T2, T3, typename et::ExprTraits<T1>::result_tag
264 >::value_type tolerance,
265 et::quaternion_result_tag,
266 SizeT)
267 {
268 typedef et::ExprTraits<T1> traits_1;
269 typedef typename traits_1::result_tag result_type_1;
270
271 typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
272 temporary_type;
273 typedef typename temporary_type::value_type value_type;
274 typedef typename temporary_type::cross_type cross_type;
275 typedef et::ExprTraits<temporary_type> result_traits;
276 typedef typename result_traits::size_tag size_tag;
277
278 /**
279 * NOTE: It seems that the equation for computing an intermediate
280 * quaternion produces the same results regardless of whether 'standard'
281 * or 'reverse' multiplication order is used (I haven't proved this -
282 * I've just observed it). Still, just to be sure I've used a pair of
283 * helper functions to ensure that the quaternions are multiplied in the
284 * right order.
285 */
286
287 temporary_type result;
288 detail::InterpResize(result, t1, size_tag());
289
290 temporary_type t2_inverse = conjugate(t2);
291 temporary_type temp1 = concatenate_quaternions(t1, t2_inverse, cross_type());
292 temporary_type temp2 = concatenate_quaternions(t3, t2_inverse, cross_type());
293 result = concatenate_quaternions(
294 exp(-(log(temp1) + log(temp2)) * value_type(.25)), t2, cross_type());
295 return result;
296 }
297
298 /**
299 * NOTE: Construction of intermediate rotation matrices for use with squad
300 * is currently implemented in terms of quaternions. This is pretty
301 * inefficient (especially so in the 2-d case, which involves jumping through
302 * a lot of hoops to get to 3-d and back), and is inelegant as well.
303 *
304 * I imagine this could be streamlined to work directly with the matrices, but
305 * I'd need to dig a bit first (figure out the matrix equivalents of
306 * quaternion exp() and log(), figure out what shortcuts can be taken in
307 * 2-d, etc.), so for now it'll just have to remain as-is.
308 *
309 * In future versions of the CML, it might also be worth reconsidering
310 * whether it's wise to support slerp and squad for matrices. Although it
311 * can be done, it's not efficient, and may give the user a false sense of
312 * security with respect to the efficiency of the underlying operations.
313 */
314
315 template< class MatT_1, class MatT_2, class MatT_3, size_t N >
316 struct squad_intermediate_f;
317
318 template< class MatT_1, class MatT_2, class MatT_3 >
319 struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>
320 {
321 template< typename Real >
322 typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
323 operator()(
324 const MatT_1& m1,
325 const MatT_2& m2,
326 const MatT_3& m3,
327 Real tolerance)
328 {
329 typedef typename et::MatrixPromote3<
330 MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
331 typedef typename temporary_type::value_type value_type;
332 typedef quaternion< value_type > quaternion_type;
333
334 quaternion_type q1, q2, q3;
335 quaternion_rotation_matrix(q1, m1);
336 quaternion_rotation_matrix(q2, m2);
337 quaternion_rotation_matrix(q3, m3);
338
339 quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);
340
341 temporary_type m;
342 et::detail::Resize(m,3,3);
343
344 matrix_rotation_quaternion(m, q4);
345
346 return m;
347 }
348 };
349
350 template< class MatT_1, class MatT_2, class MatT_3 >
351 struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>
352 {
353 template< typename Real >
354 typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
355 operator()(
356 const MatT_1& m1,
357 const MatT_2& m2,
358 const MatT_3& m3,
359 Real tolerance)
360 {
361 typedef typename et::MatrixPromote3<
362 MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
363 typedef typename temporary_type::value_type value_type;
364 typedef quaternion< value_type > quaternion_type;
365 typedef vector< value_type, fixed<3> > vector_type;
366
367 value_type angle1 = matrix_to_rotation_2D(m1);
368 value_type angle2 = matrix_to_rotation_2D(m2);
369 value_type angle3 = matrix_to_rotation_2D(m3);
370 vector_type axis(value_type(0), value_type(0), value_type(1));
371
372 quaternion_type q1, q2, q3;
373 quaternion_rotation_axis_angle(q1, axis, angle1);
374 quaternion_rotation_axis_angle(q2, axis, angle2);
375 quaternion_rotation_axis_angle(q3, axis, angle3);
376
377 quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);
378
379 value_type angle;
380 quaternion_to_axis_angle(q4, axis, angle);
381
382 temporary_type m;
383 et::detail::Resize(m,2,2);
384
385 matrix_rotation_2D(m, angle);
386
387 return m;
388 }
389 };
390
391 template< class MatT_1, class MatT_2, class MatT_3, typename Real >
392 typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
393 squad_intermediate(
394 const MatT_1& m1,
395 const MatT_2& m2,
396 const MatT_3& m3,
397 Real tolerance,
398 et::matrix_result_tag,
399 fixed_size_tag)
400 {
401 return squad_intermediate_f<MatT_1,MatT_2,MatT_3,MatT_1::array_rows>()(
402 m1,m2,m3,tolerance);
403 }
404
405 template< class MatT_1, class MatT_2, class MatT_3, typename Real >
406 typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
407 squad_intermediate(
408 const MatT_1& m1,
409 const MatT_2& m2,
410 const MatT_3& m3,
411 Real tolerance,
412 et::matrix_result_tag,
413 dynamic_size_tag)
414 {
415 typedef typename et::MatrixPromote3<
416 MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
417
418 temporary_type m;
419 et::detail::Resize(m,m1.rows(),m1.cols());
420
421 switch (m1.rows()) {
422 case 3:
423 m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>()(m1,m2,m3,tolerance);
424 break;
425 case 2:
426 m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>()(m1,m2,m3,tolerance);
427 break;
428 default:
429 throw std::invalid_argument(
430 "matrix squad_intermediate_f() expects sizes 3x3 or 2x2");
431 break;
432 }
433 return m;
434 }
435 #endif
436
437 //////////////////////////////////////////////////////////////////////////////
438 // Spherical linear interpolation of two vectors of any size
439 //////////////////////////////////////////////////////////////////////////////
440
441 template< class VecT_1, class VecT_2, typename Real, class SizeT >
442 typename detail::TypePromote<
443 VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
444 >::temporary_type
445 slerp(
446 const VecT_1& v1,
447 const VecT_2& v2,
448 Real t,
449 Real tolerance,
450 et::vector_result_tag,
451 SizeT)
452 {
453 typedef et::ExprTraits<VecT_1> type_traits;
454 typedef typename type_traits::result_tag result_type;
455 typedef typename
456 detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
457 temporary_type;
458 typedef typename temporary_type::value_type value_type;
459 typedef et::ExprTraits<temporary_type> result_traits;
460 typedef typename result_traits::size_tag size_tag;
461
462 temporary_type result;
463 detail::InterpResize(result, v1, size_tag());
464
465 value_type omega = acos_safe(dot(v1,v2));
466 value_type s = std::sin(omega);
467 if (s < tolerance) {
468 result = nlerp(v1,v2,t);
469 } else {
470 result = (value_type(std::sin((value_type(1)-t)*omega))*v1 +
471 value_type(std::sin(t*omega))*v2) / s;
472 }
473 return result;
474 }
475
476 //////////////////////////////////////////////////////////////////////////////
477 // Spherical linear interpolation of two quaternions
478 //////////////////////////////////////////////////////////////////////////////
479
480 template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
481 typename detail::TypePromote<
482 QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
483 >::temporary_type
484 slerp(
485 const QuatT_1& q1,
486 const QuatT_2& q2,
487 Real t,
488 Real tolerance,
489 et::quaternion_result_tag,
490 SizeT)
491 {
492 typedef et::ExprTraits<QuatT_1> type_traits;
493 typedef typename type_traits::result_tag result_type;
494 typedef typename
495 detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
496 temporary_type;
497 typedef typename temporary_type::value_type value_type;
498
499 temporary_type q3 = q2;
500 value_type c = dot(q1,q3);
501 if (c < value_type(0)) {
502 // Turning this off temporarily to test squad...
503 q3 = -q3;
504 c = -c;
505 }
506
507 value_type omega = acos_safe(c);
508 value_type s = std::sin(omega);
509
510 return (s < tolerance) ?
511 normalize(lerp(q1,q3,t)) :
512 (value_type(std::sin((value_type(1) - t) * omega)) * q1+
513 value_type(std::sin(t * omega)) * q3) / s;
514 }
515
516 //////////////////////////////////////////////////////////////////////////////
517 // Helper struct for spherical linear interpolation of 3x3 and 2x2 matrices
518 //////////////////////////////////////////////////////////////////////////////
519
520 template< class MatT_1, class MatT_2, size_t N > struct slerp_f;
521
522 template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,3>
523 {
524 template< typename Real >
525 typename detail::TypePromote<
526 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
527 >::temporary_type
528 operator()(
529 const MatT_1& m1,
530 const MatT_2& m2,
531 Real t,
532 Real tolerance)
533 {
534 typedef typename detail::TypePromote<
535 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
536 >::temporary_type temporary_type;
537
538 temporary_type m;
539 et::detail::Resize(m,3,3);
540 m = matrix_rotation_difference(m1,m2);
541 matrix_scale_rotation_angle(m,t,tolerance);
542 m = detail::matrix_concat_rotations(m1,m);
543 return m;
544 }
545 };
546
547 template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,2>
548 {
549 template< typename Real >
550 typename detail::TypePromote<
551 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
552 >::temporary_type
553 operator()(
554 const MatT_1& m1,
555 const MatT_2& m2,
556 Real t,
557 Real tolerance)
558 {
559 typedef typename detail::TypePromote<
560 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
561 >::temporary_type temporary_type;
562
563 temporary_type m;
564 et::detail::Resize(m,2,2);
565 m = matrix_rotation_difference_2D(m1,m2);
566 matrix_scale_rotation_angle_2D(m,t,tolerance);
567 m = detail::matrix_concat_rotations_2D(m1,m);
568 return m;
569 }
570 };
571
572 //////////////////////////////////////////////////////////////////////////////
573 // Spherical linear interpolation of two matrices of size 3x3 or 2x2
574 //////////////////////////////////////////////////////////////////////////////
575
576 template< class MatT_1, class MatT_2, typename Real >
577 typename detail::TypePromote<
578 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
579 >::temporary_type
580 slerp(
581 const MatT_1& m1,
582 const MatT_2& m2,
583 Real t,
584 Real tolerance,
585 et::matrix_result_tag,
586 fixed_size_tag)
587 {
588 return slerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t,tolerance);
589 }
590
591 template< class MatT_1, class MatT_2, typename Real >
592 typename detail::TypePromote<
593 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
594 >::temporary_type
595 slerp(
596 const MatT_1& m1,
597 const MatT_2& m2,
598 Real t,
599 Real tolerance,
600 et::matrix_result_tag,
601 dynamic_size_tag)
602 {
603 typedef typename detail::TypePromote<
604 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
605 >::temporary_type temporary_type;
606
607 temporary_type m;
608 et::detail::Resize(m,m1.rows(),m1.cols());
609
610 switch (m1.rows()) {
611 case 3:
612 m = slerp_f<MatT_1,MatT_2,3>()(m1,m2,t,tolerance);
613 break;
614 case 2:
615 m = slerp_f<MatT_1,MatT_2,2>()(m1,m2,t,tolerance);
616 break;
617 default:
618 throw std::invalid_argument(
619 "matrix slerp() expects sizes 3x3 or 2x2");
620 break;
621 }
622 return m;
623 }
624
625 //////////////////////////////////////////////////////////////////////////////
626 // Normalized linear interpolation of two vectors of any size
627 //////////////////////////////////////////////////////////////////////////////
628
629 template< class VecT_1, class VecT_2, typename Real, class SizeT >
630 typename detail::TypePromote<
631 VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
632 >::temporary_type
633 nlerp(
634 const VecT_1& v1,
635 const VecT_2& v2,
636 Real t,
637 et::vector_result_tag,
638 SizeT)
639 {
640 typedef et::ExprTraits<VecT_1> type_traits;
641 typedef typename type_traits::result_tag result_type;
642 typedef typename
643 detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
644 temporary_type;
645 typedef typename temporary_type::value_type value_type;
646 typedef et::ExprTraits<temporary_type> result_traits;
647 typedef typename result_traits::size_tag size_tag;
648
649 temporary_type result;
650 detail::InterpResize(result, v1, size_tag());
651
652 result = (value_type(1)-t)*v1+t*v2;
653 result.normalize();
654 return result;
655 }
656
657 //////////////////////////////////////////////////////////////////////////////
658 // Normalized linear interpolation of two quaternions
659 //////////////////////////////////////////////////////////////////////////////
660
661 template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
662 typename detail::TypePromote<
663 QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
664 >::temporary_type
665 nlerp(
666 const QuatT_1& q1,
667 const QuatT_2& q2,
668 Real t,
669 et::quaternion_result_tag,
670 SizeT)
671 {
672 typedef et::ExprTraits<QuatT_1> type_traits;
673 typedef typename type_traits::result_tag result_type;
674 typedef typename
675 detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
676 temporary_type;
677 typedef typename temporary_type::value_type value_type;
678
679 return normalize(lerp(q1, (dot(q1,q2) < value_type(0)) ? -q2 : q2, t));
680 }
681
682 //////////////////////////////////////////////////////////////////////////////
683 // Helper struct for normalized linear interpolation of 3x3 and 2x2 matrices
684 //////////////////////////////////////////////////////////////////////////////
685
686 template< class MatT_1, class MatT_2, size_t N > struct nlerp_f;
687
688 template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,3>
689 {
690 template< typename Real >
691 typename detail::TypePromote<
692 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
693 >::temporary_type
694 operator()(
695 const MatT_1& m1,
696 const MatT_2& m2,
697 Real t)
698 {
699 typedef typename detail::TypePromote<
700 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
701 >::temporary_type temporary_type;
702 typedef typename temporary_type::value_type value_type;
703
704 temporary_type m;
705 et::detail::Resize(m,3,3);
706 m = lerp(m1,m2,t);
707 matrix_orthogonalize_3x3(m);
708 return m;
709 }
710 };
711
712 template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,2>
713 {
714 template< typename Real >
715 typename detail::TypePromote<
716 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
717 >::temporary_type
718 operator()(
719 const MatT_1& m1,
720 const MatT_2& m2,
721 Real t)
722 {
723 typedef typename detail::TypePromote<
724 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
725 >::temporary_type temporary_type;
726 typedef typename temporary_type::value_type value_type;
727
728 temporary_type m;
729 et::detail::Resize(m,2,2);
730 m = lerp(m1,m2,t);
731 matrix_orthogonalize_2x2(m);
732 return m;
733 }
734 };
735
736 //////////////////////////////////////////////////////////////////////////////
737 // Normalized linear interpolation of two matrices of size 3x3 or 2x2
738 //////////////////////////////////////////////////////////////////////////////
739
740 template< class MatT_1, class MatT_2, typename Real >
741 typename detail::TypePromote<
742 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
743 >::temporary_type
744 nlerp(
745 const MatT_1& m1,
746 const MatT_2& m2,
747 Real t,
748 et::matrix_result_tag,
749 fixed_size_tag)
750 {
751 return nlerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t);
752 }
753
754 template< class MatT_1, class MatT_2, typename Real >
755 typename detail::TypePromote<
756 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
757 >::temporary_type
758 nlerp(
759 const MatT_1& m1,
760 const MatT_2& m2,
761 Real t,
762 et::matrix_result_tag,
763 dynamic_size_tag)
764 {
765 typedef typename detail::TypePromote<
766 MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
767 >::temporary_type temporary_type;
768
769 temporary_type m;
770 et::detail::Resize(m,m1.rows(),m1.cols());
771
772 switch (m1.rows()) {
773 case 3:
774 m = nlerp_f<MatT_1,MatT_2,3>()(m1,m2,t);
775 break;
776 case 2:
777 m = nlerp_f<MatT_1,MatT_2,2>()(m1,m2,t);
778 break;
779 default:
780 throw std::invalid_argument(
781 "matrix nlerp() expects sizes 3x3 or 2x2");
782 break;
783 }
784 return m;
785 }
786
787 } // namespace detail
788
789 //////////////////////////////////////////////////////////////////////////////
790 // Construction of 'intermediate' quaternions and matrices for use with squad
791 //////////////////////////////////////////////////////////////////////////////
792
793 /**
794 * NOTE: Computation of intermediate rotation matrices for matrix 'squad'
795 * doesn't seem to be working correctly. I'm not sure what the problem is
796 * (it might have to do with q and -q representing the same rotation), but
797 * in any case, I don't have time to get it sorted at the moment.
798 *
799 * In the meantime, I've just hacked in static assertions that will
800 * restrict squad usage to quats. For anyone reading these comments, don't
801 * worry: the quaternion verison of squad works just fine. However, you'll
802 * just have to live without matrix squad for the time being (which is
803 * probably just as well, given that matrix interpolation isn't terribly
804 * efficient).
805 */
806
807 #if 0
808 template< class T1, class T2, class T3 >
809 typename detail::TypePromote3<
810 T1,T2,T3,typename et::ExprTraits<T1>::result_tag
811 >::temporary_type
812 squad_intermediate(
813 const T1& t1,
814 const T2& t2,
815 const T3& t3,
816 typename detail::TypePromote3<
817 T1, T2, T3, typename et::ExprTraits<T1>::result_tag
818 >::value_type tolerance =
819 epsilon <
820 typename detail::TypePromote3<
821 T1, T2, T3, typename et::ExprTraits<T1>::result_tag
822 >::value_type
823 >::placeholder())
824 {
825 // HACK: See note above...
826 detail::CheckQuat(t1);
827 detail::CheckQuat(t2);
828 detail::CheckQuat(t3);
829
830 typedef et::ExprTraits<T1> traits_1;
831 typedef typename traits_1::result_tag result_type_1;
832
833 typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
834 temporary_type;
835 typedef et::ExprTraits<temporary_type> result_traits;
836 typedef typename result_traits::size_tag size_tag;
837
838 temporary_type result;
839 detail::InterpResize(result, t1, size_tag());
840
841 result = detail::squad_intermediate(
842 t1,t2,t3,tolerance,result_type_1(),size_tag());
843 return result;
844 }
845
846 //////////////////////////////////////////////////////////////////////////////
847 // Spherical quadrangle interpolation of two quaternions or matrices
848 //////////////////////////////////////////////////////////////////////////////
849
850 /**
851 * NOTE: The squad() impelementation is unfinished. I'm leaving the code
852 * here (but preprocessor'ed out) for future reference.
853 *
854 * Currently, it seems that:
855 *
856 * 1. Computation of intermediate matrices is incorrect.
857 * 2. The interpolated orientation sometimes 'jumps' while between nodes.
858 *
859 * I've observed that removing the 'shortest path' negation from the slerp
860 * function eliminates the second problem. Also, in another implementation
861 * of squad that I've seen, q1 and q2 are interpolated over the shortest
862 * path, while the helper quaternions are not. I've never seen this
863 * mentioned as a requirement of squad, but maybe they know something I
864 * don't.
865 *
866 * For anyone who happens to read these comments, all of the other
867 * interpolation functions (lerp, nlerp, slerp, etc.) should work fine -
868 * it's just squad() that's on hold.
869 */
870
871 template< class T1, class T2, class T3, class T4, typename Real >
872 typename detail::TypePromote4<
873 T1,T2,T3,T4,typename et::ExprTraits<T1>::result_tag
874 >::temporary_type
875 squad(
876 const T1& t1,
877 const T2& t1_intermediate,
878 const T3& t2_intermediate,
879 const T4& t2,
880 Real t,
881 Real tolerance = epsilon<Real>::placeholder())
882 {
883 // HACK: See note above...
884 detail::CheckQuat(t1);
885 detail::CheckQuat(t1_intermediate);
886 detail::CheckQuat(t2_intermediate);
887 detail::CheckQuat(t2);
888
889 typedef et::ExprTraits<T1> traits_1;
890 typedef typename traits_1::result_tag result_type_1;
891
892 typedef typename detail::TypePromote4<
893 T1,T2,T3,T4,result_type_1>::temporary_type temporary_type;
894 typedef typename temporary_type::value_type value_type;
895 typedef et::ExprTraits<temporary_type> result_traits;
896 typedef typename result_traits::size_tag size_tag;
897
898 temporary_type result;
899 detail::InterpResize(result, t1, size_tag());
900
901 result = slerp(
902 slerp(t1, t2, t, tolerance),
903 slerp(t1_intermediate, t2_intermediate, t, tolerance),
904 value_type(2) * t * (value_type(1) - t),
905 tolerance
906 );
907
908 return result;
909 }
910 #endif
911
912 //////////////////////////////////////////////////////////////////////////////
913 // Spherical linear interpolation of two vectors, quaternions or matrices
914 //////////////////////////////////////////////////////////////////////////////
915
916 template< class T1, class T2, typename Real >
917 typename detail::TypePromote<
918 T1,T2,typename et::ExprTraits<T1>::result_tag
919 >::temporary_type
920 slerp(
921 const T1& t1,
922 const T2& t2,
923 Real t,
924 Real tolerance = epsilon<Real>::placeholder())
925 {
926 typedef et::ExprTraits<T1> traits_1;
927 typedef typename traits_1::result_tag result_type_1;
928
929 typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
930 temporary_type;
931 typedef et::ExprTraits<temporary_type> result_traits;
932 typedef typename result_traits::size_tag size_tag;
933
934 temporary_type result;
935 detail::InterpResize(result, t1, size_tag());
936
937 result = detail::slerp(t1,t2,t,tolerance,result_type_1(),size_tag());
938 return result;
939 }
940
941 //////////////////////////////////////////////////////////////////////////////
942 // Normalized linear interpolation of two vectors, quaternions or matrices
943 //////////////////////////////////////////////////////////////////////////////
944
945 template< class T1, class T2, typename Real >
946 typename detail::TypePromote<
947 T1,T2,typename et::ExprTraits<T1>::result_tag
948 >::temporary_type
949 nlerp(const T1& t1, const T2& t2, Real t)
950 {
951 typedef et::ExprTraits<T1> traits_1;
952 typedef typename traits_1::result_tag result_type_1;
953
954 typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
955 temporary_type;
956 typedef et::ExprTraits<temporary_type> result_traits;
957 typedef typename result_traits::size_tag size_tag;
958
959 temporary_type result;
960 detail::InterpResize(result, t1, size_tag());
961
962 result = detail::nlerp(t1,t2,t,result_type_1(),size_tag());
963 return result;
964 }
965
966 //////////////////////////////////////////////////////////////////////////////
967 // Linear interpolation of two values of any qualified type
968 //////////////////////////////////////////////////////////////////////////////
969
970 /** Linear interpolation of 2 values.
971 *
972 * @note The data points are assumed to be sampled at u = 0 and u = 1, so
973 * for interpolation u must lie between 0 and 1.
974 */
975 template< class T1, class T2, typename Scalar >
976 typename detail::TypePromote<
977 T1,T2,typename et::ExprTraits<T1>::result_tag
978 >::temporary_type
979 lerp(const T1& val0, const T2& val1, Scalar u)
980 {
981 typedef
982 typename detail::TypePromote<
983 T1,T2,typename et::ExprTraits<T1>::result_tag
984 >::temporary_type temporary_type;
985
986 typedef et::ExprTraits<temporary_type> result_traits;
987 typedef typename result_traits::size_tag size_tag;
988
989 temporary_type result;
990 detail::InterpResize(result, val1, size_tag());
991
992 result = (Scalar(1) - u) * val0 + u * val1;
993 return result;
994 }
995
996 //////////////////////////////////////////////////////////////////////////////
997 // Bilinear interpolation of four values of any qualified type
998 //////////////////////////////////////////////////////////////////////////////
999
1000 template < class T1, class T2, class T3, class T4, typename Scalar >
1001 typename detail::TypePromote<
1002 typename detail::TypePromote<
1003 T1,T2,typename et::ExprTraits<T1>::result_tag
1004 >::temporary_type,
1005 typename detail::TypePromote<
1006 T3,T4,typename et::ExprTraits<T3>::result_tag
1007 >::temporary_type,
1008 typename et::ExprTraits<T1>::result_tag
1009 >::temporary_type
1010 bilerp(const T1& val00, const T2& val10,
1011 const T3& val01, const T4& val11,
1012 Scalar u, Scalar v)
1013 {
1014 typedef
1015 typename detail::TypePromote<
1016 typename detail::TypePromote<
1017 T1,T2,typename et::ExprTraits<T1>::result_tag
1018 >::temporary_type,
1019 typename detail::TypePromote<
1020 T3,T4,typename et::ExprTraits<T1>::result_tag
1021 >::temporary_type,
1022 typename et::ExprTraits<T1>::result_tag
1023 >::temporary_type temporary_type;
1024
1025 typedef et::ExprTraits<temporary_type> result_traits;
1026 typedef typename result_traits::size_tag size_tag;
1027
1028 temporary_type result;
1029 detail::InterpResize(result, val00, size_tag());
1030
1031 Scalar uv = u * v;
1032 result = (
1033 (Scalar(1.0) - u - v + uv) * val00 +
1034 (u - uv) * val10 +
1035 (v - uv) * val01 +
1036 uv * val11
1037 );
1038 return result;
1039 }
1040
1041 //////////////////////////////////////////////////////////////////////////////
1042 // Trilinear interpolation of eight values of any qualified type
1043 //////////////////////////////////////////////////////////////////////////////
1044
1045 /** Trilinear interpolation of 8 values.
1046 *
1047 * @note The data values are assumed to be sampled at the corners of a unit
1048 * cube, so for interpolation, u, v, and w must lie between 0 and 1.
1049 */
1050 template < class T1, class T2, class T3, class T4,
1051 class T5, class T6, class T7, class T8,
1052 typename Scalar >
1053 typename detail::TypePromote<
1054 typename detail::TypePromote<
1055 typename detail::TypePromote<
1056 T1,T2,typename et::ExprTraits<T1>::result_tag
1057 >::temporary_type,
1058 typename detail::TypePromote<
1059 T3,T4,typename et::ExprTraits<T3>::result_tag
1060 >::temporary_type,
1061 typename et::ExprTraits<T1>::result_tag
1062 >::temporary_type,
1063 typename detail::TypePromote<
1064 typename detail::TypePromote<
1065 T5,T6,typename et::ExprTraits<T5>::result_tag
1066 >::temporary_type,
1067 typename detail::TypePromote<
1068 T7,T8,typename et::ExprTraits<T7>::result_tag
1069 >::temporary_type,
1070 typename et::ExprTraits<T1>::result_tag
1071 >::temporary_type,
1072 typename et::ExprTraits<T1>::result_tag
1073 >::temporary_type
1074 trilerp(const T1& val000, const T2& val100,
1075 const T3& val010, const T4& val110,
1076 const T5& val001, const T6& val101,
1077 const T7& val011, const T8& val111,
1078 Scalar u, Scalar v, Scalar w)
1079 {
1080 typedef
1081 typename detail::TypePromote<
1082 typename detail::TypePromote<
1083 typename detail::TypePromote<
1084 T1,T2,typename et::ExprTraits<T1>::result_tag
1085 >::temporary_type,
1086 typename detail::TypePromote<
1087 T3,T4,typename et::ExprTraits<T1>::result_tag
1088 >::temporary_type,
1089 typename et::ExprTraits<T1>::result_tag
1090 >::temporary_type,
1091 typename detail::TypePromote<
1092 typename detail::TypePromote<
1093 T5,T6,typename et::ExprTraits<T1>::result_tag
1094 >::temporary_type,
1095 typename detail::TypePromote<
1096 T7,T8,typename et::ExprTraits<T1>::result_tag
1097 >::temporary_type,
1098 typename et::ExprTraits<T1>::result_tag
1099 >::temporary_type,
1100 typename et::ExprTraits<T1>::result_tag
1101 >::temporary_type temporary_type;
1102
1103 typedef et::ExprTraits<temporary_type> result_traits;
1104 typedef typename result_traits::size_tag size_tag;
1105
1106 temporary_type result;
1107 detail::InterpResize(result, val000, size_tag());
1108
1109 Scalar uv = u * v;
1110 Scalar vw = v * w;
1111 Scalar wu = w * u;
1112 Scalar uvw = uv * w;
1113
1114 result = (
1115 (Scalar(1.0) - u - v - w + uv + vw + wu - uvw) * val000 +
1116 (u - uv - wu + uvw) * val100 +
1117 (v - uv - vw + uvw) * val010 +
1118 (uv - uvw) * val110 +
1119 (w - vw - wu + uvw) * val001 +
1120 (wu - uvw) * val101 +
1121 (vw - uvw) * val011 +
1122 uvw * val111
1123 );
1124 return result;
1125 }
1126
1127 } // namespace cml
1128
1129 #endif
This page took 0.092631 seconds and 4 git commands to generate.