Dogcows Code - chaz/yoink/blob - src/Moof/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 = val0 * (Scalar(1) - u) + val1 * u;
 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