1 /* -*- C++ -*- ------------------------------------------------------------
3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
5 The Configurable Math Library (CML) is distributed under the terms of the
6 Boost Software License, v1.0 (see cml/LICENSE for details).
8 *-----------------------------------------------------------------------*/
13 #ifndef interpolation_h
14 #define interpolation_h
16 #include <cml/mathlib/matrix_rotation.h>
18 /* Interpolation functions.
20 * @todo: This code works, but it needs a lot of cleanup.
25 struct function_expects_args_of_same_type_error
;
29 //////////////////////////////////////////////////////////////////////////////
30 // Helper struct to promote vectors, quaternions, and matrices
31 //////////////////////////////////////////////////////////////////////////////
33 template< class T1
, class T2
, class ResultT
> struct TypePromote
;
36 struct TypePromote
< T
,T
,et::scalar_result_tag
> {
37 typedef T temporary_type
;
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
;
47 /* Check that results are of the same type */
49 (same_type
<result_type_1
, result_type_2
>::is_true
),
50 function_expects_args_of_same_type_error
);
52 typedef typename
et::ScalarPromote
<T1
,T2
>::type temporary_type
;
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
;
62 /* Check that results are of the same type */
64 (same_type
<result_type_1
, result_type_2
>::is_true
),
65 function_expects_args_of_same_type_error
);
67 /* @todo: This should be VectorPromote<> for symmetry with the other
70 typedef typename CrossPromote
<T1
,T2
>::promoted_vector temporary_type
;
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
;
80 /* Check that results are of the same type */
82 (same_type
<result_type_1
, result_type_2
>::is_true
),
83 function_expects_args_of_same_type_error
);
85 typedef typename
et::MatrixPromote2
<T1
,T2
>::temporary_type temporary_type
;
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
;
95 /* Check that results are of the same type */
97 (same_type
<result_type_1
, result_type_2
>::is_true
),
98 function_expects_args_of_same_type_error
);
100 typedef typename
et::QuaternionPromote2
<T1
,T2
>::temporary_type
104 template< class T1
, class T2
, class T3
, class ResultT
> struct TypePromote3
;
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
;
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
);
123 typedef typename
et::MatrixPromote3
<T1
,T2
,T3
>::temporary_type
125 typedef typename
temporary_type::value_type value_type
;
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
;
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
);
145 typedef typename
et::QuaternionPromote3
<T1
,T2
,T3
>::temporary_type
147 typedef typename
temporary_type::value_type value_type
;
151 class T1
, class T2
, class T3
, class T4
, class ResultT
152 > struct TypePromote4
;
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
;
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
);
176 typedef typename
et::MatrixPromote4
<T1
,T2
,T3
,T4
>::temporary_type
178 typedef typename
temporary_type::value_type value_type
;
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
;
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
);
203 typedef typename
et::QuaternionPromote4
<T1
,T2
,T3
,T4
>::temporary_type
205 typedef typename
temporary_type::value_type value_type
;
208 //////////////////////////////////////////////////////////////////////////////
209 // Helper functions to resize a vector, quaternion or matrix
210 //////////////////////////////////////////////////////////////////////////////
212 // Should be able to catch all no-ops with a generic function template...
214 template < class T1
, class T2
, class SizeTag
> void
215 InterpResize(T1
& t1
, const T2
& t2
, SizeTag
) {}
217 // Catch vector and matrix resizes...
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());
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());
229 //////////////////////////////////////////////////////////////////////////////
230 // Construction of 'intermediate' quaternions and matrices for use with squad
231 //////////////////////////////////////////////////////////////////////////////
234 template < class QuatT_1
, class QuatT_2
>
235 typename
et::QuaternionPromote2
<QuatT_1
,QuatT_2
>::temporary_type
236 concatenate_quaternions(
244 template < class QuatT_1
, class QuatT_2
>
245 typename
et::QuaternionPromote2
<QuatT_1
,QuatT_2
>::temporary_type
246 concatenate_quaternions(
254 template< class T1
, class T2
, class T3
, class SizeT
>
255 typename
detail::TypePromote3
<
256 T1
,T2
,T3
,typename
et::ExprTraits
<T1
>::result_tag
262 typename
detail::TypePromote3
<
263 T1
, T2
, T3
, typename
et::ExprTraits
<T1
>::result_tag
264 >::value_type tolerance
,
265 et::quaternion_result_tag
,
268 typedef et::ExprTraits
<T1
> traits_1
;
269 typedef typename
traits_1::result_tag result_type_1
;
271 typedef typename
detail::TypePromote3
<T1
,T2
,T3
,result_type_1
>::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
;
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
287 temporary_type result
;
288 detail::InterpResize(result
, t1
, size_tag());
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());
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.
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.
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.
315 template< class MatT_1
, class MatT_2
, class MatT_3
, size_t N
>
316 struct squad_intermediate_f
;
318 template< class MatT_1
, class MatT_2
, class MatT_3
>
319 struct squad_intermediate_f
<MatT_1
,MatT_2
,MatT_3
,3>
321 template< typename Real
>
322 typename
et::MatrixPromote3
< MatT_1
,MatT_2
,MatT_3
>::temporary_type
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
;
334 quaternion_type q1
, q2
, q3
;
335 quaternion_rotation_matrix(q1
, m1
);
336 quaternion_rotation_matrix(q2
, m2
);
337 quaternion_rotation_matrix(q3
, m3
);
339 quaternion_type q4
= squad_intermediate(q1
, q2
, q3
, tolerance
);
342 et::detail::Resize(m
,3,3);
344 matrix_rotation_quaternion(m
, q4
);
350 template< class MatT_1
, class MatT_2
, class MatT_3
>
351 struct squad_intermediate_f
<MatT_1
,MatT_2
,MatT_3
,2>
353 template< typename Real
>
354 typename
et::MatrixPromote3
< MatT_1
,MatT_2
,MatT_3
>::temporary_type
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
;
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));
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
);
377 quaternion_type q4
= squad_intermediate(q1
, q2
, q3
, tolerance
);
380 quaternion_to_axis_angle(q4
, axis
, angle
);
383 et::detail::Resize(m
,2,2);
385 matrix_rotation_2D(m
, angle
);
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
398 et::matrix_result_tag
,
401 return squad_intermediate_f
<MatT_1
,MatT_2
,MatT_3
,MatT_1::array_rows
>()(
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
412 et::matrix_result_tag
,
415 typedef typename
et::MatrixPromote3
<
416 MatT_1
,MatT_2
,MatT_3
>::temporary_type temporary_type
;
419 et::detail::Resize(m
,m1
.rows(),m1
.cols());
423 m
= squad_intermediate_f
<MatT_1
,MatT_2
,MatT_3
,3>()(m1
,m2
,m3
,tolerance
);
426 m
= squad_intermediate_f
<MatT_1
,MatT_2
,MatT_3
,2>()(m1
,m2
,m3
,tolerance
);
429 throw std::invalid_argument(
430 "matrix squad_intermediate_f() expects sizes 3x3 or 2x2");
437 //////////////////////////////////////////////////////////////////////////////
438 // Spherical linear interpolation of two vectors of any size
439 //////////////////////////////////////////////////////////////////////////////
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
450 et::vector_result_tag
,
453 typedef et::ExprTraits
<VecT_1
> type_traits
;
454 typedef typename
type_traits::result_tag result_type
;
456 detail::TypePromote
<VecT_1
,VecT_2
,result_type
>::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
;
462 temporary_type result
;
463 detail::InterpResize(result
, v1
, size_tag());
465 value_type omega
= acos_safe(dot(v1
,v2
));
466 value_type s
= std::sin(omega
);
468 result
= nlerp(v1
,v2
,t
);
470 result
= (value_type(std::sin((value_type(1)-t
)*omega
))*v1
+
471 value_type(std::sin(t
*omega
))*v2
) / s
;
476 //////////////////////////////////////////////////////////////////////////////
477 // Spherical linear interpolation of two quaternions
478 //////////////////////////////////////////////////////////////////////////////
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
489 et::quaternion_result_tag
,
492 typedef et::ExprTraits
<QuatT_1
> type_traits
;
493 typedef typename
type_traits::result_tag result_type
;
495 detail::TypePromote
<QuatT_1
,QuatT_2
,result_type
>::temporary_type
497 typedef typename
temporary_type::value_type value_type
;
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...
507 value_type omega
= acos_safe(c
);
508 value_type s
= std::sin(omega
);
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
;
516 //////////////////////////////////////////////////////////////////////////////
517 // Helper struct for spherical linear interpolation of 3x3 and 2x2 matrices
518 //////////////////////////////////////////////////////////////////////////////
520 template< class MatT_1
, class MatT_2
, size_t N
> struct slerp_f
;
522 template< class MatT_1
, class MatT_2
> struct slerp_f
<MatT_1
,MatT_2
,3>
524 template< typename Real
>
525 typename
detail::TypePromote
<
526 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
534 typedef typename
detail::TypePromote
<
535 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
536 >::temporary_type temporary_type
;
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
);
547 template< class MatT_1
, class MatT_2
> struct slerp_f
<MatT_1
,MatT_2
,2>
549 template< typename Real
>
550 typename
detail::TypePromote
<
551 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
559 typedef typename
detail::TypePromote
<
560 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
561 >::temporary_type temporary_type
;
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
);
572 //////////////////////////////////////////////////////////////////////////////
573 // Spherical linear interpolation of two matrices of size 3x3 or 2x2
574 //////////////////////////////////////////////////////////////////////////////
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
585 et::matrix_result_tag
,
588 return slerp_f
<MatT_1
,MatT_2
,MatT_1::array_rows
>()(m1
,m2
,t
,tolerance
);
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
600 et::matrix_result_tag
,
603 typedef typename
detail::TypePromote
<
604 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
605 >::temporary_type temporary_type
;
608 et::detail::Resize(m
,m1
.rows(),m1
.cols());
612 m
= slerp_f
<MatT_1
,MatT_2
,3>()(m1
,m2
,t
,tolerance
);
615 m
= slerp_f
<MatT_1
,MatT_2
,2>()(m1
,m2
,t
,tolerance
);
618 throw std::invalid_argument(
619 "matrix slerp() expects sizes 3x3 or 2x2");
625 //////////////////////////////////////////////////////////////////////////////
626 // Normalized linear interpolation of two vectors of any size
627 //////////////////////////////////////////////////////////////////////////////
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
637 et::vector_result_tag
,
640 typedef et::ExprTraits
<VecT_1
> type_traits
;
641 typedef typename
type_traits::result_tag result_type
;
643 detail::TypePromote
<VecT_1
,VecT_2
,result_type
>::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
;
649 temporary_type result
;
650 detail::InterpResize(result
, v1
, size_tag());
652 result
= (value_type(1)-t
)*v1
+t
*v2
;
657 //////////////////////////////////////////////////////////////////////////////
658 // Normalized linear interpolation of two quaternions
659 //////////////////////////////////////////////////////////////////////////////
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
669 et::quaternion_result_tag
,
672 typedef et::ExprTraits
<QuatT_1
> type_traits
;
673 typedef typename
type_traits::result_tag result_type
;
675 detail::TypePromote
<QuatT_1
,QuatT_2
,result_type
>::temporary_type
677 typedef typename
temporary_type::value_type value_type
;
679 return normalize(lerp(q1
, (dot(q1
,q2
) < value_type(0)) ? -q2
: q2
, t
));
682 //////////////////////////////////////////////////////////////////////////////
683 // Helper struct for normalized linear interpolation of 3x3 and 2x2 matrices
684 //////////////////////////////////////////////////////////////////////////////
686 template< class MatT_1
, class MatT_2
, size_t N
> struct nlerp_f
;
688 template< class MatT_1
, class MatT_2
> struct nlerp_f
<MatT_1
,MatT_2
,3>
690 template< typename Real
>
691 typename
detail::TypePromote
<
692 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
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
;
705 et::detail::Resize(m
,3,3);
707 matrix_orthogonalize_3x3(m
);
712 template< class MatT_1
, class MatT_2
> struct nlerp_f
<MatT_1
,MatT_2
,2>
714 template< typename Real
>
715 typename
detail::TypePromote
<
716 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
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
;
729 et::detail::Resize(m
,2,2);
731 matrix_orthogonalize_2x2(m
);
736 //////////////////////////////////////////////////////////////////////////////
737 // Normalized linear interpolation of two matrices of size 3x3 or 2x2
738 //////////////////////////////////////////////////////////////////////////////
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
748 et::matrix_result_tag
,
751 return nlerp_f
<MatT_1
,MatT_2
,MatT_1::array_rows
>()(m1
,m2
,t
);
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
762 et::matrix_result_tag
,
765 typedef typename
detail::TypePromote
<
766 MatT_1
,MatT_2
,typename
et::ExprTraits
<MatT_1
>::result_tag
767 >::temporary_type temporary_type
;
770 et::detail::Resize(m
,m1
.rows(),m1
.cols());
774 m
= nlerp_f
<MatT_1
,MatT_2
,3>()(m1
,m2
,t
);
777 m
= nlerp_f
<MatT_1
,MatT_2
,2>()(m1
,m2
,t
);
780 throw std::invalid_argument(
781 "matrix nlerp() expects sizes 3x3 or 2x2");
787 } // namespace detail
789 //////////////////////////////////////////////////////////////////////////////
790 // Construction of 'intermediate' quaternions and matrices for use with squad
791 //////////////////////////////////////////////////////////////////////////////
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.
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
808 template< class T1
, class T2
, class T3
>
809 typename
detail::TypePromote3
<
810 T1
,T2
,T3
,typename
et::ExprTraits
<T1
>::result_tag
816 typename
detail::TypePromote3
<
817 T1
, T2
, T3
, typename
et::ExprTraits
<T1
>::result_tag
818 >::value_type tolerance
=
820 typename
detail::TypePromote3
<
821 T1
, T2
, T3
, typename
et::ExprTraits
<T1
>::result_tag
825 // HACK: See note above...
826 detail::CheckQuat(t1
);
827 detail::CheckQuat(t2
);
828 detail::CheckQuat(t3
);
830 typedef et::ExprTraits
<T1
> traits_1
;
831 typedef typename
traits_1::result_tag result_type_1
;
833 typedef typename
detail::TypePromote3
<T1
,T2
,T3
,result_type_1
>::temporary_type
835 typedef et::ExprTraits
<temporary_type
> result_traits
;
836 typedef typename
result_traits::size_tag size_tag
;
838 temporary_type result
;
839 detail::InterpResize(result
, t1
, size_tag());
841 result
= detail::squad_intermediate(
842 t1
,t2
,t3
,tolerance
,result_type_1(),size_tag());
846 //////////////////////////////////////////////////////////////////////////////
847 // Spherical quadrangle interpolation of two quaternions or matrices
848 //////////////////////////////////////////////////////////////////////////////
851 * NOTE: The squad() impelementation is unfinished. I'm leaving the code
852 * here (but preprocessor'ed out) for future reference.
854 * Currently, it seems that:
856 * 1. Computation of intermediate matrices is incorrect.
857 * 2. The interpolated orientation sometimes 'jumps' while between nodes.
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
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.
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
877 const T2
& t1_intermediate
,
878 const T3
& t2_intermediate
,
881 Real tolerance
= epsilon
<Real
>::placeholder())
883 // HACK: See note above...
884 detail::CheckQuat(t1
);
885 detail::CheckQuat(t1_intermediate
);
886 detail::CheckQuat(t2_intermediate
);
887 detail::CheckQuat(t2
);
889 typedef et::ExprTraits
<T1
> traits_1
;
890 typedef typename
traits_1::result_tag result_type_1
;
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
;
898 temporary_type result
;
899 detail::InterpResize(result
, t1
, size_tag());
902 slerp(t1
, t2
, t
, tolerance
),
903 slerp(t1_intermediate
, t2_intermediate
, t
, tolerance
),
904 value_type(2) * t
* (value_type(1) - t
),
912 //////////////////////////////////////////////////////////////////////////////
913 // Spherical linear interpolation of two vectors, quaternions or matrices
914 //////////////////////////////////////////////////////////////////////////////
916 template< class T1
, class T2
, typename Real
>
917 typename
detail::TypePromote
<
918 T1
,T2
,typename
et::ExprTraits
<T1
>::result_tag
924 Real tolerance
= epsilon
<Real
>::placeholder())
926 typedef et::ExprTraits
<T1
> traits_1
;
927 typedef typename
traits_1::result_tag result_type_1
;
929 typedef typename
detail::TypePromote
<T1
,T2
,result_type_1
>::temporary_type
931 typedef et::ExprTraits
<temporary_type
> result_traits
;
932 typedef typename
result_traits::size_tag size_tag
;
934 temporary_type result
;
935 detail::InterpResize(result
, t1
, size_tag());
937 result
= detail::slerp(t1
,t2
,t
,tolerance
,result_type_1(),size_tag());
941 //////////////////////////////////////////////////////////////////////////////
942 // Normalized linear interpolation of two vectors, quaternions or matrices
943 //////////////////////////////////////////////////////////////////////////////
945 template< class T1
, class T2
, typename Real
>
946 typename
detail::TypePromote
<
947 T1
,T2
,typename
et::ExprTraits
<T1
>::result_tag
949 nlerp(const T1
& t1
, const T2
& t2
, Real t
)
951 typedef et::ExprTraits
<T1
> traits_1
;
952 typedef typename
traits_1::result_tag result_type_1
;
954 typedef typename
detail::TypePromote
<T1
,T2
,result_type_1
>::temporary_type
956 typedef et::ExprTraits
<temporary_type
> result_traits
;
957 typedef typename
result_traits::size_tag size_tag
;
959 temporary_type result
;
960 detail::InterpResize(result
, t1
, size_tag());
962 result
= detail::nlerp(t1
,t2
,t
,result_type_1(),size_tag());
966 //////////////////////////////////////////////////////////////////////////////
967 // Linear interpolation of two values of any qualified type
968 //////////////////////////////////////////////////////////////////////////////
970 /** Linear interpolation of 2 values.
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.
975 template< class T1
, class T2
, typename Scalar
>
976 typename
detail::TypePromote
<
977 T1
,T2
,typename
et::ExprTraits
<T1
>::result_tag
979 lerp(const T1
& val0
, const T2
& val1
, Scalar u
)
982 typename
detail::TypePromote
<
983 T1
,T2
,typename
et::ExprTraits
<T1
>::result_tag
984 >::temporary_type temporary_type
;
986 typedef et::ExprTraits
<temporary_type
> result_traits
;
987 typedef typename
result_traits::size_tag size_tag
;
989 temporary_type result
;
990 detail::InterpResize(result
, val1
, size_tag());
992 result
= (Scalar(1) - u
) * val0
+ u
* val1
;
996 //////////////////////////////////////////////////////////////////////////////
997 // Bilinear interpolation of four values of any qualified type
998 //////////////////////////////////////////////////////////////////////////////
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
1005 typename
detail::TypePromote
<
1006 T3
,T4
,typename
et::ExprTraits
<T3
>::result_tag
1008 typename
et::ExprTraits
<T1
>::result_tag
1010 bilerp(const T1
& val00
, const T2
& val10
,
1011 const T3
& val01
, const T4
& val11
,
1015 typename
detail::TypePromote
<
1016 typename
detail::TypePromote
<
1017 T1
,T2
,typename
et::ExprTraits
<T1
>::result_tag
1019 typename
detail::TypePromote
<
1020 T3
,T4
,typename
et::ExprTraits
<T1
>::result_tag
1022 typename
et::ExprTraits
<T1
>::result_tag
1023 >::temporary_type temporary_type
;
1025 typedef et::ExprTraits
<temporary_type
> result_traits
;
1026 typedef typename
result_traits::size_tag size_tag
;
1028 temporary_type result
;
1029 detail::InterpResize(result
, val00
, size_tag());
1033 (Scalar(1.0) - u
- v
+ uv
) * val00
+
1041 //////////////////////////////////////////////////////////////////////////////
1042 // Trilinear interpolation of eight values of any qualified type
1043 //////////////////////////////////////////////////////////////////////////////
1045 /** Trilinear interpolation of 8 values.
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.
1050 template < class T1
, class T2
, class T3
, class T4
,
1051 class T5
, class T6
, class T7
, class T8
,
1053 typename
detail::TypePromote
<
1054 typename
detail::TypePromote
<
1055 typename
detail::TypePromote
<
1056 T1
,T2
,typename
et::ExprTraits
<T1
>::result_tag
1058 typename
detail::TypePromote
<
1059 T3
,T4
,typename
et::ExprTraits
<T3
>::result_tag
1061 typename
et::ExprTraits
<T1
>::result_tag
1063 typename
detail::TypePromote
<
1064 typename
detail::TypePromote
<
1065 T5
,T6
,typename
et::ExprTraits
<T5
>::result_tag
1067 typename
detail::TypePromote
<
1068 T7
,T8
,typename
et::ExprTraits
<T7
>::result_tag
1070 typename
et::ExprTraits
<T1
>::result_tag
1072 typename
et::ExprTraits
<T1
>::result_tag
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
)
1081 typename
detail::TypePromote
<
1082 typename
detail::TypePromote
<
1083 typename
detail::TypePromote
<
1084 T1
,T2
,typename
et::ExprTraits
<T1
>::result_tag
1086 typename
detail::TypePromote
<
1087 T3
,T4
,typename
et::ExprTraits
<T1
>::result_tag
1089 typename
et::ExprTraits
<T1
>::result_tag
1091 typename
detail::TypePromote
<
1092 typename
detail::TypePromote
<
1093 T5
,T6
,typename
et::ExprTraits
<T1
>::result_tag
1095 typename
detail::TypePromote
<
1096 T7
,T8
,typename
et::ExprTraits
<T1
>::result_tag
1098 typename
et::ExprTraits
<T1
>::result_tag
1100 typename
et::ExprTraits
<T1
>::result_tag
1101 >::temporary_type temporary_type
;
1103 typedef et::ExprTraits
<temporary_type
> result_traits
;
1104 typedef typename
result_traits::size_tag size_tag
;
1106 temporary_type result
;
1107 detail::InterpResize(result
, val000
, size_tag());
1112 Scalar uvw
= uv
* w
;
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
+