--- /dev/null
+/* -*- C++ -*- ------------------------------------------------------------
+
+Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
+
+The Configurable Math Library (CML) is distributed under the terms of the
+Boost Software License, v1.0 (see cml/LICENSE for details).
+
+ *-----------------------------------------------------------------------*/
+/** @file
+ * @brief
+ */
+
+#ifndef interpolation_h
+#define interpolation_h
+
+#include <cml/mathlib/matrix_rotation.h>
+
+/* Interpolation functions.
+ *
+ * @todo: This code works, but it needs a lot of cleanup.
+ */
+
+namespace cml {
+
+struct function_expects_args_of_same_type_error;
+
+namespace detail {
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper struct to promote vectors, quaternions, and matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class T1, class T2, class ResultT > struct TypePromote;
+
+template< class T >
+struct TypePromote< T,T,et::scalar_result_tag > {
+ typedef T temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::scalar_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+
+ typedef typename et::ScalarPromote<T1,T2>::type temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::vector_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+
+ /* @todo: This should be VectorPromote<> for symmetry with the other
+ * type promotions.
+ */
+ typedef typename CrossPromote<T1,T2>::promoted_vector temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::matrix_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+
+ typedef typename et::MatrixPromote2<T1,T2>::temporary_type temporary_type;
+};
+
+template< class T1, class T2 >
+struct TypePromote< T1,T2,et::quaternion_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+
+ typedef typename et::QuaternionPromote2<T1,T2>::temporary_type
+ temporary_type;
+};
+
+template< class T1, class T2, class T3, class ResultT > struct TypePromote3;
+
+template< class T1, class T2, class T3 >
+struct TypePromote3< T1,T2,T3,et::matrix_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef et::ExprTraits<T3> traits_3;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+ typedef typename traits_3::result_tag result_type_3;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_3>::is_true),
+ function_expects_args_of_same_type_error);
+
+ typedef typename et::MatrixPromote3<T1,T2,T3>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+};
+
+template< class T1, class T2, class T3 >
+struct TypePromote3< T1,T2,T3,et::quaternion_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef et::ExprTraits<T3> traits_3;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+ typedef typename traits_3::result_tag result_type_3;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_3>::is_true),
+ function_expects_args_of_same_type_error);
+
+ typedef typename et::QuaternionPromote3<T1,T2,T3>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+};
+
+template <
+ class T1, class T2, class T3, class T4, class ResultT
+> struct TypePromote4;
+
+template< class T1, class T2, class T3, class T4 >
+struct TypePromote4< T1,T2,T3,T4,et::matrix_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef et::ExprTraits<T3> traits_3;
+ typedef et::ExprTraits<T4> traits_4;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+ typedef typename traits_3::result_tag result_type_3;
+ typedef typename traits_4::result_tag result_type_4;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_3>::is_true),
+ function_expects_args_of_same_type_error);
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_4>::is_true),
+ function_expects_args_of_same_type_error);
+
+ typedef typename et::MatrixPromote4<T1,T2,T3,T4>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+};
+
+template< class T1, class T2, class T3, class T4 >
+struct TypePromote4< T1,T2,T3,T4,et::quaternion_result_tag > {
+ typedef et::ExprTraits<T1> traits_1;
+ typedef et::ExprTraits<T2> traits_2;
+ typedef et::ExprTraits<T3> traits_3;
+ typedef et::ExprTraits<T4> traits_4;
+ typedef typename traits_1::result_tag result_type_1;
+ typedef typename traits_2::result_tag result_type_2;
+ typedef typename traits_3::result_tag result_type_3;
+ typedef typename traits_4::result_tag result_type_4;
+
+ /* Check that results are of the same type */
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_2>::is_true),
+ function_expects_args_of_same_type_error);
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_3>::is_true),
+ function_expects_args_of_same_type_error);
+ CML_STATIC_REQUIRE_M(
+ (same_type<result_type_1, result_type_4>::is_true),
+ function_expects_args_of_same_type_error);
+
+ typedef typename et::QuaternionPromote4<T1,T2,T3,T4>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+};
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper functions to resize a vector, quaternion or matrix
+//////////////////////////////////////////////////////////////////////////////
+
+// Should be able to catch all no-ops with a generic function template...
+
+template < class T1, class T2, class SizeTag > void
+InterpResize(T1& t1, const T2& t2, SizeTag) {}
+
+// Catch vector and matrix resizes...
+
+template< typename E, class A, class VecT > void
+InterpResize(vector<E,A>& v, const VecT& target, dynamic_size_tag) {
+ v.resize(target.size());
+}
+
+template< typename E, class A, class B, class L, class MatT > void
+InterpResize(matrix<E,A,B,L>& m, const MatT& target, dynamic_size_tag) {
+ m.resize(target.rows(),target.cols());
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Construction of 'intermediate' quaternions and matrices for use with squad
+//////////////////////////////////////////////////////////////////////////////
+
+#if 0
+template < class QuatT_1, class QuatT_2 >
+typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
+concatenate_quaternions(
+ const QuatT_1& q1,
+ const QuatT_2& q2,
+ positive_cross)
+{
+ return q2 * q1;
+}
+
+template < class QuatT_1, class QuatT_2 >
+typename et::QuaternionPromote2<QuatT_1,QuatT_2>::temporary_type
+concatenate_quaternions(
+ const QuatT_1& q1,
+ const QuatT_2& q2,
+ negative_cross)
+{
+ return q1 * q2;
+}
+
+template< class T1, class T2, class T3, class SizeT >
+typename detail::TypePromote3<
+ T1,T2,T3,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+squad_intermediate(
+ const T1& t1,
+ const T2& t2,
+ const T3& t3,
+ typename detail::TypePromote3<
+ T1, T2, T3, typename et::ExprTraits<T1>::result_tag
+ >::value_type tolerance,
+ et::quaternion_result_tag,
+ SizeT)
+{
+ typedef et::ExprTraits<T1> traits_1;
+ typedef typename traits_1::result_tag result_type_1;
+
+ typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+ typedef typename temporary_type::cross_type cross_type;
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ /**
+ * NOTE: It seems that the equation for computing an intermediate
+ * quaternion produces the same results regardless of whether 'standard'
+ * or 'reverse' multiplication order is used (I haven't proved this -
+ * I've just observed it). Still, just to be sure I've used a pair of
+ * helper functions to ensure that the quaternions are multiplied in the
+ * right order.
+ */
+
+ temporary_type result;
+ detail::InterpResize(result, t1, size_tag());
+
+ temporary_type t2_inverse = conjugate(t2);
+ temporary_type temp1 = concatenate_quaternions(t1, t2_inverse, cross_type());
+ temporary_type temp2 = concatenate_quaternions(t3, t2_inverse, cross_type());
+ result = concatenate_quaternions(
+ exp(-(log(temp1) + log(temp2)) * value_type(.25)), t2, cross_type());
+ return result;
+}
+
+/**
+ * NOTE: Construction of intermediate rotation matrices for use with squad
+ * is currently implemented in terms of quaternions. This is pretty
+ * inefficient (especially so in the 2-d case, which involves jumping through
+ * a lot of hoops to get to 3-d and back), and is inelegant as well.
+ *
+ * I imagine this could be streamlined to work directly with the matrices, but
+ * I'd need to dig a bit first (figure out the matrix equivalents of
+ * quaternion exp() and log(), figure out what shortcuts can be taken in
+ * 2-d, etc.), so for now it'll just have to remain as-is.
+ *
+ * In future versions of the CML, it might also be worth reconsidering
+ * whether it's wise to support slerp and squad for matrices. Although it
+ * can be done, it's not efficient, and may give the user a false sense of
+ * security with respect to the efficiency of the underlying operations.
+ */
+
+template< class MatT_1, class MatT_2, class MatT_3, size_t N >
+struct squad_intermediate_f;
+
+template< class MatT_1, class MatT_2, class MatT_3 >
+struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>
+{
+ template< typename Real >
+ typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+ operator()(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ const MatT_3& m3,
+ Real tolerance)
+ {
+ typedef typename et::MatrixPromote3<
+ MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
+ typedef typename temporary_type::value_type value_type;
+ typedef quaternion< value_type > quaternion_type;
+
+ quaternion_type q1, q2, q3;
+ quaternion_rotation_matrix(q1, m1);
+ quaternion_rotation_matrix(q2, m2);
+ quaternion_rotation_matrix(q3, m3);
+
+ quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);
+
+ temporary_type m;
+ et::detail::Resize(m,3,3);
+
+ matrix_rotation_quaternion(m, q4);
+
+ return m;
+ }
+};
+
+template< class MatT_1, class MatT_2, class MatT_3 >
+struct squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>
+{
+ template< typename Real >
+ typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+ operator()(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ const MatT_3& m3,
+ Real tolerance)
+ {
+ typedef typename et::MatrixPromote3<
+ MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
+ typedef typename temporary_type::value_type value_type;
+ typedef quaternion< value_type > quaternion_type;
+ typedef vector< value_type, fixed<3> > vector_type;
+
+ value_type angle1 = matrix_to_rotation_2D(m1);
+ value_type angle2 = matrix_to_rotation_2D(m2);
+ value_type angle3 = matrix_to_rotation_2D(m3);
+ vector_type axis(value_type(0), value_type(0), value_type(1));
+
+ quaternion_type q1, q2, q3;
+ quaternion_rotation_axis_angle(q1, axis, angle1);
+ quaternion_rotation_axis_angle(q2, axis, angle2);
+ quaternion_rotation_axis_angle(q3, axis, angle3);
+
+ quaternion_type q4 = squad_intermediate(q1, q2, q3, tolerance);
+
+ value_type angle;
+ quaternion_to_axis_angle(q4, axis, angle);
+
+ temporary_type m;
+ et::detail::Resize(m,2,2);
+
+ matrix_rotation_2D(m, angle);
+
+ return m;
+ }
+};
+
+template< class MatT_1, class MatT_2, class MatT_3, typename Real >
+typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+squad_intermediate(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ const MatT_3& m3,
+ Real tolerance,
+ et::matrix_result_tag,
+ fixed_size_tag)
+{
+ return squad_intermediate_f<MatT_1,MatT_2,MatT_3,MatT_1::array_rows>()(
+ m1,m2,m3,tolerance);
+}
+
+template< class MatT_1, class MatT_2, class MatT_3, typename Real >
+typename et::MatrixPromote3< MatT_1,MatT_2,MatT_3 >::temporary_type
+squad_intermediate(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ const MatT_3& m3,
+ Real tolerance,
+ et::matrix_result_tag,
+ dynamic_size_tag)
+{
+ typedef typename et::MatrixPromote3<
+ MatT_1,MatT_2,MatT_3 >::temporary_type temporary_type;
+
+ temporary_type m;
+ et::detail::Resize(m,m1.rows(),m1.cols());
+
+ switch (m1.rows()) {
+ case 3:
+ m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,3>()(m1,m2,m3,tolerance);
+ break;
+ case 2:
+ m = squad_intermediate_f<MatT_1,MatT_2,MatT_3,2>()(m1,m2,m3,tolerance);
+ break;
+ default:
+ throw std::invalid_argument(
+ "matrix squad_intermediate_f() expects sizes 3x3 or 2x2");
+ break;
+ }
+ return m;
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two vectors of any size
+//////////////////////////////////////////////////////////////////////////////
+
+template< class VecT_1, class VecT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+ VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
+>::temporary_type
+slerp(
+ const VecT_1& v1,
+ const VecT_2& v2,
+ Real t,
+ Real tolerance,
+ et::vector_result_tag,
+ SizeT)
+{
+ typedef et::ExprTraits<VecT_1> type_traits;
+ typedef typename type_traits::result_tag result_type;
+ typedef typename
+ detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, v1, size_tag());
+
+ value_type omega = acos_safe(dot(v1,v2));
+ value_type s = std::sin(omega);
+ if (s < tolerance) {
+ result = nlerp(v1,v2,t);
+ } else {
+ result = (value_type(std::sin((value_type(1)-t)*omega))*v1 +
+ value_type(std::sin(t*omega))*v2) / s;
+ }
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two quaternions
+//////////////////////////////////////////////////////////////////////////////
+
+template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+ QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
+>::temporary_type
+slerp(
+ const QuatT_1& q1,
+ const QuatT_2& q2,
+ Real t,
+ Real tolerance,
+ et::quaternion_result_tag,
+ SizeT)
+{
+ typedef et::ExprTraits<QuatT_1> type_traits;
+ typedef typename type_traits::result_tag result_type;
+ typedef typename
+ detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+
+ temporary_type q3 = q2;
+ value_type c = dot(q1,q3);
+ if (c < value_type(0)) {
+ // Turning this off temporarily to test squad...
+ q3 = -q3;
+ c = -c;
+ }
+
+ value_type omega = acos_safe(c);
+ value_type s = std::sin(omega);
+
+ return (s < tolerance) ?
+ normalize(lerp(q1,q3,t)) :
+ (value_type(std::sin((value_type(1) - t) * omega)) * q1+
+ value_type(std::sin(t * omega)) * q3) / s;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper struct for spherical linear interpolation of 3x3 and 2x2 matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, size_t N > struct slerp_f;
+
+template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,3>
+{
+ template< typename Real >
+ typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type
+ operator()(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t,
+ Real tolerance)
+ {
+ typedef typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type temporary_type;
+
+ temporary_type m;
+ et::detail::Resize(m,3,3);
+ m = matrix_rotation_difference(m1,m2);
+ matrix_scale_rotation_angle(m,t,tolerance);
+ m = detail::matrix_concat_rotations(m1,m);
+ return m;
+ }
+};
+
+template< class MatT_1, class MatT_2 > struct slerp_f<MatT_1,MatT_2,2>
+{
+ template< typename Real >
+ typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type
+ operator()(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t,
+ Real tolerance)
+ {
+ typedef typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type temporary_type;
+
+ temporary_type m;
+ et::detail::Resize(m,2,2);
+ m = matrix_rotation_difference_2D(m1,m2);
+ matrix_scale_rotation_angle_2D(m,t,tolerance);
+ m = detail::matrix_concat_rotations_2D(m1,m);
+ return m;
+ }
+};
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two matrices of size 3x3 or 2x2
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+slerp(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t,
+ Real tolerance,
+ et::matrix_result_tag,
+ fixed_size_tag)
+{
+ return slerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t,tolerance);
+}
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+slerp(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t,
+ Real tolerance,
+ et::matrix_result_tag,
+ dynamic_size_tag)
+{
+ typedef typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type temporary_type;
+
+ temporary_type m;
+ et::detail::Resize(m,m1.rows(),m1.cols());
+
+ switch (m1.rows()) {
+ case 3:
+ m = slerp_f<MatT_1,MatT_2,3>()(m1,m2,t,tolerance);
+ break;
+ case 2:
+ m = slerp_f<MatT_1,MatT_2,2>()(m1,m2,t,tolerance);
+ break;
+ default:
+ throw std::invalid_argument(
+ "matrix slerp() expects sizes 3x3 or 2x2");
+ break;
+ }
+ return m;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two vectors of any size
+//////////////////////////////////////////////////////////////////////////////
+
+template< class VecT_1, class VecT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+ VecT_1,VecT_2,typename et::ExprTraits<VecT_1>::result_tag
+>::temporary_type
+nlerp(
+ const VecT_1& v1,
+ const VecT_2& v2,
+ Real t,
+ et::vector_result_tag,
+ SizeT)
+{
+ typedef et::ExprTraits<VecT_1> type_traits;
+ typedef typename type_traits::result_tag result_type;
+ typedef typename
+ detail::TypePromote<VecT_1,VecT_2,result_type>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, v1, size_tag());
+
+ result = (value_type(1)-t)*v1+t*v2;
+ result.normalize();
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two quaternions
+//////////////////////////////////////////////////////////////////////////////
+
+template< class QuatT_1, class QuatT_2, typename Real, class SizeT >
+typename detail::TypePromote<
+ QuatT_1,QuatT_2,typename et::ExprTraits<QuatT_1>::result_tag
+>::temporary_type
+nlerp(
+ const QuatT_1& q1,
+ const QuatT_2& q2,
+ Real t,
+ et::quaternion_result_tag,
+ SizeT)
+{
+ typedef et::ExprTraits<QuatT_1> type_traits;
+ typedef typename type_traits::result_tag result_type;
+ typedef typename
+ detail::TypePromote<QuatT_1,QuatT_2,result_type>::temporary_type
+ temporary_type;
+ typedef typename temporary_type::value_type value_type;
+
+ return normalize(lerp(q1, (dot(q1,q2) < value_type(0)) ? -q2 : q2, t));
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Helper struct for normalized linear interpolation of 3x3 and 2x2 matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, size_t N > struct nlerp_f;
+
+template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,3>
+{
+ template< typename Real >
+ typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type
+ operator()(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t)
+ {
+ typedef typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type temporary_type;
+ typedef typename temporary_type::value_type value_type;
+
+ temporary_type m;
+ et::detail::Resize(m,3,3);
+ m = lerp(m1,m2,t);
+ matrix_orthogonalize_3x3(m);
+ return m;
+ }
+};
+
+template< class MatT_1, class MatT_2 > struct nlerp_f<MatT_1,MatT_2,2>
+{
+ template< typename Real >
+ typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type
+ operator()(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t)
+ {
+ typedef typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type temporary_type;
+ typedef typename temporary_type::value_type value_type;
+
+ temporary_type m;
+ et::detail::Resize(m,2,2);
+ m = lerp(m1,m2,t);
+ matrix_orthogonalize_2x2(m);
+ return m;
+ }
+};
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two matrices of size 3x3 or 2x2
+//////////////////////////////////////////////////////////////////////////////
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+nlerp(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t,
+ et::matrix_result_tag,
+ fixed_size_tag)
+{
+ return nlerp_f<MatT_1,MatT_2,MatT_1::array_rows>()(m1,m2,t);
+}
+
+template< class MatT_1, class MatT_2, typename Real >
+typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+>::temporary_type
+nlerp(
+ const MatT_1& m1,
+ const MatT_2& m2,
+ Real t,
+ et::matrix_result_tag,
+ dynamic_size_tag)
+{
+ typedef typename detail::TypePromote<
+ MatT_1,MatT_2,typename et::ExprTraits<MatT_1>::result_tag
+ >::temporary_type temporary_type;
+
+ temporary_type m;
+ et::detail::Resize(m,m1.rows(),m1.cols());
+
+ switch (m1.rows()) {
+ case 3:
+ m = nlerp_f<MatT_1,MatT_2,3>()(m1,m2,t);
+ break;
+ case 2:
+ m = nlerp_f<MatT_1,MatT_2,2>()(m1,m2,t);
+ break;
+ default:
+ throw std::invalid_argument(
+ "matrix nlerp() expects sizes 3x3 or 2x2");
+ break;
+ }
+ return m;
+}
+
+} // namespace detail
+
+//////////////////////////////////////////////////////////////////////////////
+// Construction of 'intermediate' quaternions and matrices for use with squad
+//////////////////////////////////////////////////////////////////////////////
+
+/**
+ * NOTE: Computation of intermediate rotation matrices for matrix 'squad'
+ * doesn't seem to be working correctly. I'm not sure what the problem is
+ * (it might have to do with q and -q representing the same rotation), but
+ * in any case, I don't have time to get it sorted at the moment.
+ *
+ * In the meantime, I've just hacked in static assertions that will
+ * restrict squad usage to quats. For anyone reading these comments, don't
+ * worry: the quaternion verison of squad works just fine. However, you'll
+ * just have to live without matrix squad for the time being (which is
+ * probably just as well, given that matrix interpolation isn't terribly
+ * efficient).
+ */
+
+#if 0
+template< class T1, class T2, class T3 >
+typename detail::TypePromote3<
+ T1,T2,T3,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+squad_intermediate(
+ const T1& t1,
+ const T2& t2,
+ const T3& t3,
+ typename detail::TypePromote3<
+ T1, T2, T3, typename et::ExprTraits<T1>::result_tag
+ >::value_type tolerance =
+ epsilon <
+ typename detail::TypePromote3<
+ T1, T2, T3, typename et::ExprTraits<T1>::result_tag
+ >::value_type
+ >::placeholder())
+{
+ // HACK: See note above...
+ detail::CheckQuat(t1);
+ detail::CheckQuat(t2);
+ detail::CheckQuat(t3);
+
+ typedef et::ExprTraits<T1> traits_1;
+ typedef typename traits_1::result_tag result_type_1;
+
+ typedef typename detail::TypePromote3<T1,T2,T3,result_type_1>::temporary_type
+ temporary_type;
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, t1, size_tag());
+
+ result = detail::squad_intermediate(
+ t1,t2,t3,tolerance,result_type_1(),size_tag());
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical quadrangle interpolation of two quaternions or matrices
+//////////////////////////////////////////////////////////////////////////////
+
+/**
+ * NOTE: The squad() impelementation is unfinished. I'm leaving the code
+ * here (but preprocessor'ed out) for future reference.
+ *
+ * Currently, it seems that:
+ *
+ * 1. Computation of intermediate matrices is incorrect.
+ * 2. The interpolated orientation sometimes 'jumps' while between nodes.
+ *
+ * I've observed that removing the 'shortest path' negation from the slerp
+ * function eliminates the second problem. Also, in another implementation
+ * of squad that I've seen, q1 and q2 are interpolated over the shortest
+ * path, while the helper quaternions are not. I've never seen this
+ * mentioned as a requirement of squad, but maybe they know something I
+ * don't.
+ *
+ * For anyone who happens to read these comments, all of the other
+ * interpolation functions (lerp, nlerp, slerp, etc.) should work fine -
+ * it's just squad() that's on hold.
+ */
+
+template< class T1, class T2, class T3, class T4, typename Real >
+typename detail::TypePromote4<
+ T1,T2,T3,T4,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+squad(
+ const T1& t1,
+ const T2& t1_intermediate,
+ const T3& t2_intermediate,
+ const T4& t2,
+ Real t,
+ Real tolerance = epsilon<Real>::placeholder())
+{
+ // HACK: See note above...
+ detail::CheckQuat(t1);
+ detail::CheckQuat(t1_intermediate);
+ detail::CheckQuat(t2_intermediate);
+ detail::CheckQuat(t2);
+
+ typedef et::ExprTraits<T1> traits_1;
+ typedef typename traits_1::result_tag result_type_1;
+
+ typedef typename detail::TypePromote4<
+ T1,T2,T3,T4,result_type_1>::temporary_type temporary_type;
+ typedef typename temporary_type::value_type value_type;
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, t1, size_tag());
+
+ result = slerp(
+ slerp(t1, t2, t, tolerance),
+ slerp(t1_intermediate, t2_intermediate, t, tolerance),
+ value_type(2) * t * (value_type(1) - t),
+ tolerance
+ );
+
+ return result;
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+// Spherical linear interpolation of two vectors, quaternions or matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class T1, class T2, typename Real >
+typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+slerp(
+ const T1& t1,
+ const T2& t2,
+ Real t,
+ Real tolerance = epsilon<Real>::placeholder())
+{
+ typedef et::ExprTraits<T1> traits_1;
+ typedef typename traits_1::result_tag result_type_1;
+
+ typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
+ temporary_type;
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, t1, size_tag());
+
+ result = detail::slerp(t1,t2,t,tolerance,result_type_1(),size_tag());
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Normalized linear interpolation of two vectors, quaternions or matrices
+//////////////////////////////////////////////////////////////////////////////
+
+template< class T1, class T2, typename Real >
+typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+nlerp(const T1& t1, const T2& t2, Real t)
+{
+ typedef et::ExprTraits<T1> traits_1;
+ typedef typename traits_1::result_tag result_type_1;
+
+ typedef typename detail::TypePromote<T1,T2,result_type_1>::temporary_type
+ temporary_type;
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, t1, size_tag());
+
+ result = detail::nlerp(t1,t2,t,result_type_1(),size_tag());
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Linear interpolation of two values of any qualified type
+//////////////////////////////////////////////////////////////////////////////
+
+/** Linear interpolation of 2 values.
+ *
+ * @note The data points are assumed to be sampled at u = 0 and u = 1, so
+ * for interpolation u must lie between 0 and 1.
+ */
+template< class T1, class T2, typename Scalar >
+typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+lerp(const T1& val0, const T2& val1, Scalar u)
+{
+ typedef
+ typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type temporary_type;
+
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, val1, size_tag());
+
+ result = (Scalar(1) - u) * val0 + u * val1;
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Bilinear interpolation of four values of any qualified type
+//////////////////////////////////////////////////////////////////////////////
+
+template < class T1, class T2, class T3, class T4, typename Scalar >
+typename detail::TypePromote<
+ typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ T3,T4,typename et::ExprTraits<T3>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+bilerp(const T1& val00, const T2& val10,
+ const T3& val01, const T4& val11,
+ Scalar u, Scalar v)
+{
+ typedef
+ typename detail::TypePromote<
+ typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ T3,T4,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+ >::temporary_type temporary_type;
+
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, val00, size_tag());
+
+ Scalar uv = u * v;
+ result = (
+ (Scalar(1.0) - u - v + uv) * val00 +
+ (u - uv) * val10 +
+ (v - uv) * val01 +
+ uv * val11
+ );
+ return result;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Trilinear interpolation of eight values of any qualified type
+//////////////////////////////////////////////////////////////////////////////
+
+/** Trilinear interpolation of 8 values.
+ *
+ * @note The data values are assumed to be sampled at the corners of a unit
+ * cube, so for interpolation, u, v, and w must lie between 0 and 1.
+ */
+template < class T1, class T2, class T3, class T4,
+ class T5, class T6, class T7, class T8,
+ typename Scalar >
+typename detail::TypePromote<
+ typename detail::TypePromote<
+ typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ T3,T4,typename et::ExprTraits<T3>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ typename detail::TypePromote<
+ T5,T6,typename et::ExprTraits<T5>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ T7,T8,typename et::ExprTraits<T7>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+>::temporary_type
+trilerp(const T1& val000, const T2& val100,
+ const T3& val010, const T4& val110,
+ const T5& val001, const T6& val101,
+ const T7& val011, const T8& val111,
+ Scalar u, Scalar v, Scalar w)
+{
+ typedef
+ typename detail::TypePromote<
+ typename detail::TypePromote<
+ typename detail::TypePromote<
+ T1,T2,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ T3,T4,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ typename detail::TypePromote<
+ T5,T6,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename detail::TypePromote<
+ T7,T8,typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+ >::temporary_type,
+ typename et::ExprTraits<T1>::result_tag
+ >::temporary_type temporary_type;
+
+ typedef et::ExprTraits<temporary_type> result_traits;
+ typedef typename result_traits::size_tag size_tag;
+
+ temporary_type result;
+ detail::InterpResize(result, val000, size_tag());
+
+ Scalar uv = u * v;
+ Scalar vw = v * w;
+ Scalar wu = w * u;
+ Scalar uvw = uv * w;
+
+ result = (
+ (Scalar(1.0) - u - v - w + uv + vw + wu - uvw) * val000 +
+ (u - uv - wu + uvw) * val100 +
+ (v - uv - vw + uvw) * val010 +
+ (uv - uvw) * val110 +
+ (w - vw - wu + uvw) * val001 +
+ (wu - uvw) * val101 +
+ (vw - uvw) * val011 +
+ uvw * val111
+ );
+ return result;
+}
+
+} // namespace cml
+
+#endif
projects / chaz / yoink / blobdiff