[Olena-patches] milena r1151: Add quaternion based on vector 4d

URL: https://svn.lrde.epita.fr/svn/oln/trunk/milena ChangeLog: 2007-09-21 Simon Nivault &lt;simon.nivault(a)lrde.epita.fr&gt; Add quaternion based on vector 4d * mln/metal/vec.hh: Add sprod, vprod and normalize. * mln/norm/l2.hh: New, Works on C-tabs and on metal::vec. * mln/value/quat.hh: New. * tests/norm_l2.cc: New. * tests/value_quat.cc: New. --- mln/metal/vec.hh | 117 +++++++++++++++++ mln/norm/l2.hh | 111 ++++++++++++++++ mln/value/quat.hh | 343 ++++++++++++++++++++++++++++++++++++++++++++++++++++ tests/norm_l2.cc | 54 ++++++++ tests/value_quat.cc | 72 ++++++++++ 5 files changed, 694 insertions(+), 3 deletions(-) Index: trunk/milena/tests/value_quat.cc =================================================================== --- trunk/milena/tests/value_quat.cc (revision 0) +++ trunk/milena/tests/value_quat.cc (revision 1151) @@ -0,0 +1,72 @@ +// Copyright (C) 2007 EPITA Research and Development Laboratory +// +// This file is part of the Olena Library. This library is free +// software; you can redistribute it and/or modify it under the terms +// of the GNU General Public License version 2 as published by the +// Free Software Foundation. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this library; see the file COPYING. If not, write to +// the Free Software Foundation, 51 Franklin Street, Fifth Floor, +// Boston, MA 02111-1307, USA. +// +// As a special exception, you may use this file as part of a free +// software library without restriction. Specifically, if other files +// instantiate templates or use macros or inline functions from this +// file, or you compile this file and link it with other files to +// produce an executable, this file does not by itself cause the +// resulting executable to be covered by the GNU General Public +// License. This exception does not however invalidate any other +// reasons why the executable file might be covered by the GNU General +// Public License. + +/*! \file tests/value_int_u8.cc + * + * \brief Tests on mln::value::int_u8. + */ + +#include <iostream> + +#include <mln/value/quat.hh> + + +int main() +{ + using namespace mln; + + value::quat q1(3.f, 4.f, 1.6f, 0.5f); + value::quat q2(1.2, make::vec(3, 6, 4)); + value::quat q3(make::vec(1.3, 3., -6., 4.)); + + std::cout << q1 << std::endl; + std::cout << q2 << std::endl; + std::cout << q3 << std::endl; + + std::cout << q1.scal() << std::endl; + + q1.set_scal(2.6); + std::cout << q1 << std::endl; + + std::cout << q1.vect() << std::endl; + + q2.set_vect(make::vec(1.4, 5.9, 3.1)); + std::cout << q2 << std::endl; + + std::cout << q2 * q3 << std::endl; + + mln_assertion(!q3.is_unit()); + q3.set_unit(); + std::cout << q3 << std::endl; + mln_assertion(q3.is_unit()); + + std::cout << q2.conj() << std::endl; + std::cout << q2.inv() << std::endl; + std::cout << norm::l2(q2) << ' ' << norm::l2(q2.inv()) << std::endl; + std::cout << q2.inv().inv() << std::endl; + +} Index: trunk/milena/tests/norm_l2.cc =================================================================== --- trunk/milena/tests/norm_l2.cc (revision 0) +++ trunk/milena/tests/norm_l2.cc (revision 1151) @@ -0,0 +1,54 @@ +// Copyright (C) 2007 EPITA Research and Development Laboratory +// +// This file is part of the Olena Library. This library is free +// software; you can redistribute it and/or modify it under the terms +// of the GNU General Public License version 2 as published by the +// Free Software Foundation. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this library; see the file COPYING. If not, write to +// the Free Software Foundation, 51 Franklin Street, Fifth Floor, +// Boston, MA 02111-1307, USA. +// +// As a special exception, you may use this file as part of a free +// software library without restriction. Specifically, if other files +// instantiate templates or use macros or inline functions from this +// file, or you compile this file and link it with other files to +// produce an executable, this file does not by itself cause the +// resulting executable to be covered by the GNU General Public +// License. This exception does not however invalidate any other +// reasons why the executable file might be covered by the GNU General +// Public License. + +/*! \file tests/norm_l2.cc + * + * \brief Tests on mln::norm::l2. + */ + +#include <iostream> + +#include <mln/norm/l2.hh> + +#include <mln/metal/vec.hh> + +int main() +{ + using namespace mln; + + int t1[] = {2, 6, -1, 2}; + + std::cout << norm::l2(t1) << std::endl; + + int t2[] = {2, -6, -1, 2}; + + std::cout << norm::l2_distance(t1, t2) << std::endl; + + metal::vec<4,float> v = make::vec(2, 6, -1, 2); + + std::cout << norm::l2(v); +} Index: trunk/milena/mln/metal/vec.hh =================================================================== --- trunk/milena/mln/metal/vec.hh (revision 1150) +++ trunk/milena/mln/metal/vec.hh (revision 1151) @@ -29,6 +29,7 @@ # define MLN_METAL_VEC_HH # include <iostream> +# include <cmath> # include <mln/core/contract.hh> # include <mln/metal/binary_arith_trait.hh> @@ -140,10 +141,84 @@ void set_all(const T& val); + T sprod(const vec<n, T>& rhs) const; + unsigned size() const; + const vec<n, T>& normalize(); }; + // eq + + template <unsigned n, typename T, typename U> + bool operator==(const vec<n,T>& lhs, const vec<n,U>& rhs); + + template <unsigned n, typename T, typename U> + bool operator!=(const vec<n,T>& lhs, const vec<n,U>& rhs); + + // + + + template <unsigned n, typename T, typename U> + vec<n,T>& + operator+=(vec<n,T>& lhs, const vec<n,U>& rhs); + + template <unsigned n, typename T, typename U> + vec<n, typename binary_arith_trait<T,U>::ret > + operator+(const vec<n,T>& lhs, const vec<n,U>& rhs); + + // - + + template <unsigned n, typename T, typename U> + vec<n,T>& + operator-=(vec<n,T>& lhs, const vec<n,U>& rhs); + + template <unsigned n, typename T, typename U> + vec<n, typename binary_arith_trait<T,U>::ret> + operator-(const vec<n,T>& lhs, const vec<n,U>& rhs); + + template <unsigned n, typename T> + vec<n, T> + operator-(const vec<n,T>& lhs); + + // * + + template <unsigned n, typename T, typename S> + vec<n,T>& + operator*=(vec<n,T>& lhs, const S& scalar); + + template <unsigned n, typename T, typename S> + vec<n, typename binary_arith_trait<T,S>::ret> + operator*(const S& scalar, const vec<n,T>& lhs); + + // / + + template <unsigned n, typename T, typename S> + vec<n,T>& + operator/=(vec<n,T>& lhs, const S& scalar); + + template <unsigned n, typename T, typename S> + vec<n, typename binary_arith_trait<T,S>::ret> + operator/(const vec<n,T>& lhs, const S& scalar); + + // << + + template <unsigned n, typename T> + std::ostream& + operator<<(std::ostream& ostr, const vec<n,T>& v); + + template <unsigned n> + std::ostream& + operator<<(std::ostream& ostr, const vec<n,unsigned char>& v); + + template <unsigned n> + std::ostream& + operator<<(std::ostream& ostr, const vec<n,signed char>& v); + + // vprod + + template <typename T, typename U> + vec<3, typename binary_arith_trait<T, U>::ret> + vprod(const vec<3, T>& lhs, const vec<3, U>& rhs); # ifndef MLN_INCLUDE_ONLY @@ -193,11 +268,33 @@ } template <unsigned n, typename T> + T vec<n,T>::sprod(const vec<n, T>& rhs) const + { + T tmp = 0; + + for (unsigned i = 0; i < n; ++i) + tmp += data_[i] * rhs.data_[i]; + return tmp; + } + + template <unsigned n, typename T> unsigned vec<n,T>::size() const { return n; } + template <unsigned n, typename T> + const vec<n, T>& vec<n, T>::normalize() + { + float n_l2 = 0; + for (unsigned i = 0; i < n; ++i) + n_l2 += data_[i] * data_[i]; + n_l2 = sqrt(n_l2); + for (unsigned i = 0; i < n; ++i) + data_[i] = T(data_[i] / n_l2); + return *this; + } + // eq @@ -284,7 +381,7 @@ template <unsigned n, typename T, typename S> vec<n, typename binary_arith_trait<T,S>::ret> - operator*(const vec<n,T>& lhs, const S& scalar) + operator*(const S& scalar, const vec<n,T>& lhs) { vec<n, typename binary_arith_trait<T,S>::ret> tmp; for (unsigned i = 0; i < n; ++i) @@ -299,7 +396,7 @@ vec<n,T>& operator/=(vec<n,T>& lhs, const S& scalar) { - precondition(scalar != 0); + mln_precondition(scalar != 0); for (unsigned i = 0; i < n; ++i) lhs[i] /= scalar; return lhs; @@ -309,7 +406,7 @@ vec<n, typename binary_arith_trait<T,S>::ret> operator/(const vec<n,T>& lhs, const S& scalar) { - precondition(scalar != 0); + mln_precondition(scalar != 0); vec<n, typename binary_arith_trait<T,S>::ret> tmp; for (unsigned i = 0; i < n; ++i) tmp[i] = lhs[i] / scalar; @@ -349,6 +446,20 @@ return ostr; } + // vprod + + template <typename T, typename U> + vec<3, typename binary_arith_trait<T, U>::ret> + vprod(const vec<3, T>& lhs, const vec<3, U>& rhs) + { + vec<3, typename binary_arith_trait<T, U>::ret> tmp; + tmp[0] = lhs[1] * rhs[2] - lhs[2] * rhs[1]; + tmp[1] = lhs[2] * rhs[0] - lhs[0] * rhs[2]; + tmp[2] = lhs[0] * rhs[1] - lhs[1] * rhs[0]; + return tmp; + } + + # endif // MLN_INCLUDE_ONLY Index: trunk/milena/mln/norm/l2.hh =================================================================== --- trunk/milena/mln/norm/l2.hh (revision 0) +++ trunk/milena/mln/norm/l2.hh (revision 1151) @@ -0,0 +1,111 @@ +// Copyright (C) 2007 EPITA Research and Development Laboratory +// +// This file is part of the Olena Library. This library is free +// software; you can redistribute it and/or modify it under the terms +// of the GNU General Public License version 2 as published by the +// Free Software Foundation. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this library; see the file COPYING. If not, write to +// the Free Software Foundation, 51 Franklin Street, Fifth Floor, +// Boston, MA 02111-1307, USA. +// +// As a special exception, you may use this file as part of a free +// software library without restriction. Specifically, if other files +// instantiate templates or use macros or inline functions from this +// file, or you compile this file and link it with other files to +// produce an executable, this file does not by itself cause the +// resulting executable to be covered by the GNU General Public +// License. This exception does not however invalidate any other +// reasons why the executable file might be covered by the GNU General +// Public License. + +#ifndef MLN_NORM_L2_HH +# define MLN_NORM_L2_HH + +/*! \file mln/norm/l2.hh + * + * \brief Define some infinity-norm related routines. + */ + +# include <cmath> + +# include <mln/metal/vec.hh> + +namespace mln +{ + + namespace norm + { + + /// Infinity-norm of a vector \p vec. + template <unsigned n, typename C> + float l2(const C (&vec)[n]); + + /// Infinity-norm distance between vectors \p v1 and \p v2. + template <unsigned n, typename C> + float l2_distance(const C (&v1)[n], const C (&v2)[n]); + + template <unsigned n, typename C> + float l2(const metal::vec<n,C>& vec); + + template <unsigned n, typename C> + float l2_distance(const metal::vec<n,C>& vec1, const metal::vec<n,C>& vec2); + +# ifndef MLN_INCLUDE_ONLY + + template <unsigned n, typename C> + float l2(const C (&vec)[n]) + { + C c = 0; + for (unsigned i = 0; i < n; ++i) + c += vec[i] * vec[i]; + return sqrt(c); + } + + template <unsigned n, typename C> + float l2_distance(const C (&v1)[n], const C (&v2)[n]) + { + C d = 0; + for (unsigned i = 0; i < n; ++i) + { + C dd = v1[i] - v2[i]; + d += dd * dd; + } + return sqrt(d); + } + + template <unsigned n, typename C> + float l2(const metal::vec<n,C>& vec) + { + C c = 0; + for (unsigned i = 0; i < n; ++i) + c += vec[i] * vec[i]; + return sqrt(c); + } + + template <unsigned n, typename C> + float l2(const metal::vec<n,C>& vec1, const metal::vec<n,C>& vec2) + { + C d = 0; + for (unsigned i = 0; i < n; ++i) + { + C dd = vec1[i] - vec2[i]; + d += dd * dd; + } + return sqrt(d); + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace mln::norm + +} // end of namespace mln + + +#endif // ! MLN_NORM_L2_HH Index: trunk/milena/mln/value/quat.hh =================================================================== --- trunk/milena/mln/value/quat.hh (revision 0) +++ trunk/milena/mln/value/quat.hh (revision 1151) @@ -0,0 +1,343 @@ +// Copyright (C) 2007 EPITA Research and Development Laboratory +// +// This file is part of the Olena Library. This library is free +// software; you can redistribute it and/or modify it under the terms +// of the GNU General Public License version 2 as published by the +// Free Software Foundation. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this library; see the file COPYING. If not, write to +// the Free Software Foundation, 51 Franklin Street, Fifth Floor, +// Boston, MA 02111-1307, USA. +// +// As a special exception, you may use this file as part of a free +// software library without restriction. Specifically, if other files +// instantiate templates or use macros or inline functions from this +// file, or you compile this file and link it with other files to +// produce an executable, this file does not by itself cause the +// resulting executable to be covered by the GNU General Public +// License. This exception does not however invalidate any other +// reasons why the executable file might be covered by the GNU General +// Public License. + +#ifndef MLN_VALUE_QUAT_HH +# define MLN_VALUE_QUAT_HH + +# include <cmath> + +# include <mln/metal/vec.hh> +# include <mln/norm/l2.hh> +# include <mln/value/props.hh> + +namespace mln +{ + + namespace value + { + + class quat : public metal::vec<4, float> + { + public: + + // ctors + + quat(); + quat(float s, float x, float y, float z); + + template <typename T> + quat(float s, const metal::vec<3,T>& v); + + template <typename T> + quat(const metal::vec<4,T>& v); + + // accessors/modifiers as a 'scalar+metal::vec<3>' + + float scal() const; + void set_scal(float s); + + metal::vec<3,float> vect() const; + void set_vect(float x, float y, float z); + template <typename T> + void set_vect(const metal::vec<3,T>& v); + + // multiplication + + quat operator*(const quat& rhs) const; + + // tests + + bool is_unit() const; + bool is_null() const; + + // conjugate and invert + + quat conj() const; + quat inv() const; + + // transform into unit quaternion + + quat& set_unit(); + template <typename T> + void set_unit(float theta, const metal::vec<3,T>& uv); + + // only for unit quaternions described by theta and uv such as: + // q = ( cos(theta), sin(theta) * uv ) + + template <typename T> + quat(unsigned one, float theta, const metal::vec<3, T>& uv); + + float theta() const; + void set_theta(float theta); + + metal::vec<3, float> uvect() const; + template <typename T> + void set_uvect(const metal::vec<3,T>& uv); + + }; + + // overloaded math procs + + quat log(const quat& q); + quat exp(const quat& q); + quat pow(const quat& q, double t); + template <typename T> + bool about_equal(const T& f, const T& q); + bool about_equal(const quat& p, const quat& q); + + + // meths and procs bodies... + +# ifndef MLN_INCLUDE_ONLY + + //ctors + + + quat::quat() + { + } + + quat::quat(float s, float x, float y, float z) + { + set_scal(s); + set_vect(x, y, z); + } + + template <typename T> + quat::quat(float s, const metal::vec<3,T>& v) + { + set_scal(s); + set_vect(v); + } + + template <typename T> + quat::quat(const metal::vec<4,T>& v) + : metal::vec<4,float>(v) + { + } + + + // accessors/modifiers as a 'scalar+vec<3>' + + + float quat::scal() const + { + return data_[0]; + } + + + void quat::set_scal(float s) + { + data_[0] = s; + } + + + metal::vec<3, float> quat::vect() const + { + return make::vec(data_[1], data_[2], data_[3]); + } + + + void quat::set_vect(float x, float y, float z) + { + data_[1] = x; + data_[2] = y; + data_[3] = z; + } + + template <typename T> + void quat::set_vect(const metal::vec<3,T>& v) + { + set_vect(v[0], v[1], v[2]); + } + + + // multiplication + + + quat quat::operator*(const quat& rhs) const + { + float + s1 = scal(), + s2 = rhs.scal(); + + metal::vec<3,float> + v1 = vect(), + v2 = rhs.vect(); + + return quat(s1 * s2 - v1.sprod(v2), + metal::vprod(v1, v2) + + s1 * v2 + s2 * v1); + } + + + // tests + + + bool quat::is_unit() const + { + return about_equal(norm::l2(*this), 1.f); + } + + + bool quat::is_null() const + { + return about_equal(norm::l2(*this), 0.f); + } + + + // conjugate and invert + + + quat quat::conj() const + { + return quat(scal(), - vect()); + } + + + quat quat::inv() const + { + assert(! is_null()); + float f = norm::l2(*this); + + return conj() / (f * f); + } + + + // transform into unit quaternion + + + quat& quat::set_unit() + { + normalize(); + return *this; + } + + + template <typename T> + void quat::set_unit(float theta, const metal::vec<3,T>& uv) + { + static const float pi = 3.14159265358979323846; + + mln_precondition(theta > - pi - props<float>::epsilon() + && theta < pi + props<float>::epsilon()); + mln_precondition(about_equal(norm::l2(uv), 1.f)); + + data_[0] = cos(theta); + float sint = sin(theta); + data_[1] = uv[0] * sint; + data_[2] = uv[1] * sint; + data_[3] = uv[2] * sint; + } + + // only for unit quaternions described by theta and uv such as: + // q = ( cos(theta), sin(theta) * uv ) + + + template <typename T> + quat::quat(unsigned one, float theta, const metal::vec<3,T>& uv) + { + mln_precondition(one == 1); + set_unit(theta, uv); + } + + + float quat::theta() const + { + mln_precondition(is_unit()); + return acos(scal()); + } + + + void quat::set_theta(float theta) + { + mln_precondition(is_unit()); + set_unit(theta, uvect()); + } + + metal::vec<3, float> quat::uvect() const + { + mln_precondition(is_unit()); + metal::vec<3, float> v = vect(); + return v.normalize(); + } + + template <typename T> + void quat::set_uvect(const metal::vec<3,T>& uv) + { + mln_precondition(is_unit()); + set_unit(theta(), uv); + } + + + // overloaded math procs + + + quat log(const quat& q) + { + mln_precondition(q.is_unit()); + return quat(0.f, q.theta() * q.uvect()); + } + + + quat exp(const quat& q) + { + mln_precondition(about_equal(q.scal(), 0.f)); + metal::vec<3, float> v = q.vect(); + float theta = norm::l2(v); + mln_precondition(!about_equal(theta, 0.f)); + metal::vec<3, float> uv = v / theta; + return quat(cos(theta), sin(theta) * uv); + } + + + quat pow(const quat& q, double t) + { + mln_precondition(q.is_unit()); + return exp(t * log(q)); + } + + template <typename T> + bool about_equal(const T& f, const T& q) + { + if (f > q) + return (f - q ) < props<T>::epsilon(); + return (q - f) < props<T>::epsilon(); + } + + bool about_equal(const quat& p, const quat& q) + { + return about_equal<float>(norm::l2(p - q), 0); + } + +# endif // ! MLN_INCLUDE_ONLY + + } // end of namespace mln::value + +} // end of namespace mln + +#endif // ! MLN_VALUE_QUAT_HH