[Olena-patches] olena-2.0-115-gd9deb1a Improve conversion between algebra::h_mat and algebra::quat.

* mln/algebra/h_mat.hh, * mln/algebra/quat.hh: New from_to_ overloads. * mln/fun/x2x/rotation.hh: Make use of new make::h_mat overload. * mln/make/h_mat.hh: New overload using quaternion as argument. * mln/value/builtin/integers.hh: Add epsilon. * tests/algebra/h_mat.cc, * tests/make/h_mat.cc: Improve tests. --- milena/ChangeLog | 16 ++++++ milena/mln/algebra/h_mat.hh | 94 +++++++++++++++++++++++++++++++++- milena/mln/algebra/quat.hh | 71 +++++++++++++++++++++++++- milena/mln/fun/x2x/rotation.hh | 12 +---- milena/mln/make/h_mat.hh | 38 +++++++++++++- milena/mln/value/builtin/integers.hh | 4 +- milena/tests/algebra/h_mat.cc | 52 +++++++++++++----- milena/tests/make/h_mat.cc | 38 +++++++++++++- 8 files changed, 291 insertions(+), 34 deletions(-) diff --git a/milena/ChangeLog b/milena/ChangeLog index de897eb..d8d0804 100644 --- a/milena/ChangeLog +++ b/milena/ChangeLog @@ -1,3 +1,19 @@ +2012-05-25 Guillaume Lazzara &lt;z(a)lrde.epita.fr&gt; + + Improve conversion between algebra::h_mat and algebra::quat. + + * mln/algebra/h_mat.hh, + * mln/algebra/quat.hh: New from_to_ overloads. + + * mln/fun/x2x/rotation.hh: Make use of new make::h_mat overload. + + * mln/make/h_mat.hh: New overload using quaternion as argument. + + * mln/value/builtin/integers.hh: Add epsilon. + + * tests/algebra/h_mat.cc, + * tests/make/h_mat.cc: Improve tests. + 2012-05-10 Guillaume Lazzara &lt;z(a)lrde.epita.fr&gt; Rely on Argument-Dependent Lookup (ADL) in from_to_ overloads. diff --git a/milena/mln/algebra/h_mat.hh b/milena/mln/algebra/h_mat.hh index 3d75a66..e2ee67f 100644 --- a/milena/mln/algebra/h_mat.hh +++ b/milena/mln/algebra/h_mat.hh @@ -1,4 +1,5 @@ -// Copyright (C) 2007, 2008, 2009 EPITA Research and Development Laboratory (LRDE) +// Copyright (C) 2007, 2008, 2009, 2012 EPITA Research and Development +// Laboratory (LRDE) // // This file is part of Olena. // @@ -31,10 +32,21 @@ * \brief Definition of a matrix with homogeneous coordinates. * * \todo Add traits. + * + * FIXME: there is a big issue with this class. exact() does not + * return the correct type since the exact type is NOT correctly + * propagated to Object. mat and h_mat should inherit from a base + * class and the operator traits should be updated so that + * interoperability between mat and h_mat is preserved which is not + * obvious. + * */ # include <mln/algebra/mat.hh> +# include <mln/algebra/quat.hh> +# include <mln/math/pi.hh> +# include <mln/util/couple.hh> namespace mln { @@ -57,9 +69,15 @@ namespace mln h_mat(); /// Constructor with the underlying matrix. h_mat(const mat<d+1, d+1, T>& x); + }; + /// \internal Conversion: h_mat -> quat + template <typename C> + void from_to_(const algebra::h_mat<3,C>& from, algebra::quat& to); + + # ifndef MLN_INCLUDE_ONLY template <unsigned d, typename T> @@ -76,6 +94,80 @@ namespace mln { } + + // Conversions + + template <typename C> + void from_to_(const algebra::h_mat<3,C>& from, algebra::quat& to) + { + C tr = from(0, 0) + from(1, 1) + from(2, 2) + 1; + + if (tr > 0.005f) // Actually, greater than 0 + { + C s = 0.5 / sqrt(tr), + w = 0.25 / s, + x = (from(2, 1) - from(1, 2)) * s, + y = (from(0, 2) - from(2, 0)) * s, + z = (from(1, 0) - from(0, 1)) * s; + + to = algebra::quat(w, x, y, z); + return; + } + + // If the trace of the matrix is less than or equal to zero + // then identify which major diagonal element has the greatest + // value. + + C max = 0; + unsigned c = 0; + for (unsigned d = 0; d <= 3; ++d) + if (from(d, d) > max) + { + max = from(d, d); + c = d; + } + + // Depending on this value, calculate the following: + C s, w, x, y, z; + switch(c) + { + case 0: + s = sqrt(1.0 + from(0, 0) - from(1, 1) - from(2, 2)) * 2; + x = 0.5 / s; + y = (from(0, 1) + from(1, 0)) / s; + z = (from(0, 2) + from(2, 0)) / s; + w = (from(1, 2) + from(2, 1)) / s; + break; + + case 1: + s = sqrt(1.0 + from(1, 1) - from(0, 0) - from(2, 2)) * 2; + x = (from(0, 1) + from(1, 0)) / s; + y = 0.5 / s; + z = (from(1, 2) + from(2, 1)) / s; + w = (from(0, 2) + from(2, 0)) / s; + break; + + case 2: + s = sqrt(1.0 + from(2, 2) - from(0, 0) - from(1, 1)) * 2; + x = (from(0, 2) + from(2, 0)) / s; + y = (from(1, 2) + from(2, 1)) / s; + z = 0.5 / s; + w = (from(0, 1) + from(1, 0) ) / s; + break; + + // Error case + default: + x = 0; + y = 0; + z = 0; + w = 0; + } + + to = algebra::quat(w, x, y, z); + return; + } + + # endif // ! MLN_INCLUDE_ONLY } // end of namespace mln::algebra diff --git a/milena/mln/algebra/quat.hh b/milena/mln/algebra/quat.hh index 9ec725d..20dc9db 100644 --- a/milena/mln/algebra/quat.hh +++ b/milena/mln/algebra/quat.hh @@ -1,4 +1,5 @@ -// Copyright (C) 2007, 2008, 2009 EPITA Research and Development Laboratory (LRDE) +// Copyright (C) 2007, 2008, 2009, 2012 EPITA Research and Development +// Laboratory (LRDE) // // This file is part of Olena. // @@ -44,6 +45,8 @@ # include <mln/math/abs.hh> # include <mln/norm/l2.hh> +# include <mln/util/couple.hh> +# include <mln/math/pi.hh> // FIXME: pow, exp etc... are def here and in value::... @@ -260,8 +263,28 @@ namespace mln quat slerp_5(const quat& p, const quat& q, float h); + /// \internal Conversion: quaternion -> (angle_degrees, axis). + template <typename C> + void from_to_(const quat& from, mln::util::couple<C, algebra::vec<3,C> >& to); + + } // end of namespace mln::algebra + + + namespace make + { + + template <typename C> + mln::algebra::quat + quat(double angle, const mln::algebra::vec<3,C>& axis); + + } + # ifndef MLN_INCLUDE_ONLY + + namespace algebra + { + // Constructors. inline @@ -673,10 +696,54 @@ namespace mln return tmp; } -# endif // ! MLN_INCLUDE_ONLY + + // Conversions. + + template <typename C> + void from_to_(const quat& from, mln::util::couple<C, algebra::vec<3,C> >& to) + { + quat tmp = from; + tmp.set_unit(); + + C w = tmp.to_vec()[0], + angle = std::acos(w) * 2 * 180/math::pi; + + C sa = std::sin(std::acos(w)); + if (std::fabs( sa ) < 0.0005 ) + sa = 1; + + to.first() = angle; + to.second()[0] = tmp.to_vec()[1] / sa; + to.second()[1] = tmp.to_vec()[2] / sa; + to.second()[2] = tmp.to_vec()[3] / sa; + } } // end of namespace mln::algebra + + namespace make + { + + template <typename C> + mln::algebra::quat + quat(double angle, const mln::algebra::vec<3,C>& axis) + { + angle *= mln::math::pi/180; + C s = std::sin(angle / 2); + + C x = axis[0] * s, + y = axis[1] * s, + z = axis[2] * s, + w = std::cos(angle / 2); + + return mln::algebra::quat(w, x, y, z); + } + + } // end of namespace mln::make + + +# endif // ! MLN_INCLUDE_ONLY + } // end of namespace mln #endif // ! MLN_ALGEBRA_QUAT_HH diff --git a/milena/mln/fun/x2x/rotation.hh b/milena/mln/fun/x2x/rotation.hh index ed48ebf..da0e1fc 100644 --- a/milena/mln/fun/x2x/rotation.hh +++ b/milena/mln/fun/x2x/rotation.hh @@ -214,17 +214,7 @@ namespace mln mlc_bool(n == 3)::check(); mln_precondition(q.is_unit()); - C - w = q.to_vec()[0], - x = q.to_vec()[1], x2 = 2*x*x, xw = 2*x*w, - y = q.to_vec()[2], y2 = 2*y*y, xy = 2*x*y, yw = 2*y*w, - z = q.to_vec()[3], z2 = 2*z*z, xz = 2*x*z, yz = 2*y*z, zw = 2*z*w; - - C t[9] = {1.f - y2 - z2, xy - zw, xz + yw, - xy + zw, 1.f - x2 - z2, yz - xw, - xz - yw, yz + xw, 1.f - x2 - y2}; - - this->m_ = mln::make::h_mat(t); + this->m_ = mln::make::h_mat(q); mln_assertion(check_rotation(q)); /// Update attributes. diff --git a/milena/mln/make/h_mat.hh b/milena/mln/make/h_mat.hh index d124ccf..1ab2dc0 100644 --- a/milena/mln/make/h_mat.hh +++ b/milena/mln/make/h_mat.hh @@ -1,4 +1,5 @@ -// Copyright (C) 2009 EPITA Research and Development Laboratory (LRDE) +// Copyright (C) 2009, 2012 EPITA Research and Development Laboratory +// (LRDE) // // This file is part of Olena. // @@ -41,7 +42,7 @@ namespace mln { - /// Create an mln::algebra::mat<n,n,T>. + /// Create an mln::algebra::h_mat<n,T>. /* * \param[in] tab C-array of values. * @@ -51,6 +52,14 @@ namespace mln algebra::h_mat<mlc_sqrt_int(N), T> h_mat(const T (&tab)[N]); + /*! \brief Create a rotation matrix as mln::algebra::h_mat<n,T>. + + \param[in] q A quaternion. + + \return A rotation matrix based on \p q. + */ + template <typename C> + algebra::h_mat<3, C> h_mat(const C& v, const algebra::quat& q); # ifndef MLN_INCLUDE_ONLY @@ -67,6 +76,31 @@ namespace mln return tmp; } + + template <typename C> + inline + algebra::h_mat<3, C> + h_mat(const C& v, const algebra::quat& q) + { + mln_precondition(q.is_unit()); + (void) v; + + algebra::h_mat<3, C> m; + C + w = q.to_vec()[0], + x = q.to_vec()[1], x2 = 2*x*x, xw = 2*x*w, + y = q.to_vec()[2], y2 = 2*y*y, xy = 2*x*y, yw = 2*y*w, + z = q.to_vec()[3], z2 = 2*z*z, xz = 2*x*z, yz = 2*y*z, zw = 2*z*w; + + C t[9] = {1.f - y2 - z2, xy - zw, xz + yw, + xy + zw, 1.f - x2 - z2, yz - xw, + xz - yw, yz + xw, 1.f - x2 - y2}; + + m = mln::make::h_mat(t); + return m; + } + + # endif // ! MLN_INCLUDE_ONLY } // end of namespace mln::make diff --git a/milena/mln/value/builtin/integers.hh b/milena/mln/value/builtin/integers.hh index 05cbede..8cc173c 100644 --- a/milena/mln/value/builtin/integers.hh +++ b/milena/mln/value/builtin/integers.hh @@ -1,4 +1,5 @@ -// Copyright (C) 2007, 2008, 2009 EPITA Research and Development Laboratory (LRDE) +// Copyright (C) 2007, 2008, 2009, 2012 EPITA Research and Development +// Laboratory (LRDE) // // This file is part of Olena. // @@ -81,6 +82,7 @@ namespace mln static const E min() { return mln::value::internal::limits<E>::min(); } static const E max() { return mln::value::internal::limits<E>::max(); } + static const E epsilon() { return mln::value::internal::limits<E>::epsilon(); } typedef float sum; }; diff --git a/milena/tests/algebra/h_mat.cc b/milena/tests/algebra/h_mat.cc index a5bb31c..7a30f6b 100644 --- a/milena/tests/algebra/h_mat.cc +++ b/milena/tests/algebra/h_mat.cc @@ -1,4 +1,5 @@ -// Copyright (C) 2007, 2009 EPITA Research and Development Laboratory (LRDE) +// Copyright (C) 2007, 2009, 2012 EPITA Research and Development +// Laboratory (LRDE) // // This file is part of Olena. // @@ -26,34 +27,55 @@ #include <mln/fun/i2v/all_to.hh> #include <mln/algebra/mat.hh> #include <mln/algebra/h_mat.hh> +#include <mln/make/h_mat.hh> +template <typename T> +bool about_equal(const T& f, const T& q) +{ + return mln::math::abs(q - f) <= 0.000001; +} + int main() { using namespace mln; - algebra::mat<1,3,float> m1; - m1.set_all(4); - algebra::mat<2,2,float> m2 = literal::identity; + // Reading in h_mat. + algebra::h_mat<3,float> hm2(all_to(2.5)); + for (int i = 0; i < 4; ++i) + for (int j = 0; j < 4; ++j) + mln_assertion(hm2(i, j) == 2.5f); + // Conversion from mat to h_mat + algebra::mat<2,2,float> m2 = literal::identity; algebra::h_mat<1,float> hm1(m2); - algebra::h_mat<2,float> hm2; - algebra::h_mat<3,float> hm3(all_to(1.5)); + mln_assertion(m2.size() == hm1.size()); + for (int i = 0; i < 2; ++i) + for (int j = 0; j < 2; ++j) + mln_assertion(m2(i, j) == hm1(i, j)); + // Conversion from h_mat to mat. + algebra::h_mat<3,float> hm3(all_to(1.5)); algebra::mat<4,4,float> m4 = hm3; + mln_assertion(m4.size() == hm3.size()); + for (int i = 0; i < 4; ++i) + for (int j = 0; j < 4; ++j) + mln_assertion(m4(i, j) == hm3(i, j)); - std::cout << "m1 = " << m1 << ";" << std::endl; - std::cout << "m2 = " << m2 << ";" << std::endl; - std::cout << "m4 = " << m4 << ";" << std::endl; - std::cout << "hm1 = " << hm1 << ";" << std::endl; - std::cout << "hm2 = " << hm2 << ";" << std::endl; - std::cout << "hm3 = " << hm3 << ";" << std::endl; + // Check from_to_ overloads. { - algebra::h_mat<2,float> m, m2; - m = m2; - // FIXME: Test *many* => runs ok... + algebra::quat q_ref(0.92388, 0.186238, 0.310397, 0.124159); + double vals[9] = { 0.776477, -0.113801, 0.619785, + 0.345031, 0.8998, -0.267046, + -0.527293, 0.4212, 0.737938 }; + algebra::h_mat<3,double> m = make::h_mat(vals); + algebra::quat q2; + algebra::from_to_(m, q2); + + for (int i = 0; i < 4; ++i) + mln_assertion(about_equal(q_ref.to_vec()[i], q2.to_vec()[i])); } } diff --git a/milena/tests/make/h_mat.cc b/milena/tests/make/h_mat.cc index a737252..eaf232e 100644 --- a/milena/tests/make/h_mat.cc +++ b/milena/tests/make/h_mat.cc @@ -1,4 +1,5 @@ -// Copyright (C) 2009 EPITA Research and Development Laboratory (LRDE) +// Copyright (C) 2009, 2012 EPITA Research and Development Laboratory +// (LRDE) // // This file is part of Olena. // @@ -24,6 +25,7 @@ // executable file might be covered by the GNU General Public License. #include <mln/make/h_mat.hh> +#include <mln/math/abs.hh> static const int result[4][4] = { { 2, 3, 4, 0 }, @@ -31,6 +33,19 @@ static const int result[4][4] = { { 2, 3, 4, 0 }, { 8, 9, 10, 0 }, { 0, 0, 0, 1 } }; +static const double result_q[4][4] = { { 0.776477, -0.113801, 0.619785, 0 }, + { 0.345031, 0.8998, -0.267046, 0 }, + { -0.527293, 0.4212, 0.737938, 0 }, + { 0, 0, 0, 1 } }; + + +template <typename T> +bool about_equal(const T& f, const T& q) +{ + return mln::math::abs(q - f) <= 0.000001; +} + + int main() { using namespace mln; @@ -39,8 +54,27 @@ int main() 5, 6, 7, 8, 9, 10 }; algebra::h_mat<3,int> m = make::h_mat(vals); - + for (unsigned i = 0; i < 4; ++i) for (unsigned j = 0; j < 4; ++j) mln_assertion(m(i,j) == result[i][j]); + + + // Checking creation from quaternions. + { + algebra::vec<3,double> v; + v[0] = 1; + v[1] = 2; + v[2] = 3; + v.normalize(); + + algebra::quat q(0.92388, 0.186238, 0.310397, 0.124159); + q.set_unit(); + + algebra::h_mat<3,double> mat = make::h_mat(double(), q); + + for (unsigned i = 0; i < 4; ++i) + for (unsigned j = 0; j < 4; ++j) + mln_assertion(about_equal(mat(i,j), result_q[i][j])); + } } -- 1.7.2.5