Milena (Olena)  User documentation 2.0a Id
rotation.hh
00001 // Copyright (C) 2007, 2008, 2009, 2010, 2011 EPITA Research and
00002 // Development Laboratory (LRDE)
00003 //
00004 // This file is part of Olena.
00005 //
00006 // Olena is free software: you can redistribute it and/or modify it under
00007 // the terms of the GNU General Public License as published by the Free
00008 // Software Foundation, version 2 of the License.
00009 //
00010 // Olena is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013 // General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU General Public License
00016 // along with Olena.  If not, see <http://www.gnu.org/licenses/>.
00017 //
00018 // As a special exception, you may use this file as part of a free
00019 // software project without restriction.  Specifically, if other files
00020 // instantiate templates or use macros or inline functions from this
00021 // file, or you compile this file and link it with other files to produce
00022 // an executable, this file does not by itself cause the resulting
00023 // executable to be covered by the GNU General Public License.  This
00024 // exception does not however invalidate any other reasons why the
00025 // executable file might be covered by the GNU General Public License.
00026 
00027 #ifndef MLN_FUN_X2X_ROTATION_HH
00028 # define MLN_FUN_X2X_ROTATION_HH
00029 
00039 
00040 # include <cstdlib>
00041 # include <cmath>
00042 
00043 # include <mln/core/concept/function.hh>
00044 # include <mln/fun/internal/x2x_linear_impl.hh>
00045 # include <mln/algebra/vec.hh>
00046 # include <mln/algebra/mat.hh>
00047 # include <mln/algebra/quat.hh>
00048 # include <mln/make/h_mat.hh>
00049 
00050 # include <mln/norm/l2.hh>
00051 
00052 
00053 namespace mln
00054 {
00055 
00056   namespace fun
00057   {
00058 
00059     namespace x2x
00060     {
00061 
00062       namespace internal
00063       {
00064         // (Axis, angle)-based rotation: general case (not implemented).
00065         template < unsigned n, typename C >
00066         algebra::h_mat<n, C>
00067         get_rot_h_mat(const C alpha_, const algebra::vec<n,C>& axis_)
00068         {
00069           (void) alpha_;
00070           (void) axis_;
00071 
00072           std::cerr
00073             << __FILE__ << ":" << __LINE__ << ": error:"
00074             << " generic mln::fun::x2x::internal::get_rot_h_mat<n, C>"
00075             << " not implemented."
00076             << std::endl;
00077           std::abort();
00078         }
00079 
00080         // (Axis, angle)-based rotation: 2D case.
00081         template <typename C >
00082         algebra::h_mat<2, C>
00083         get_rot_h_mat(const C alpha, const algebra::vec<2,C>&)
00084         {
00085           const C cos_a = cos(alpha);
00086           const C sin_a = sin(alpha);
00087 
00088           algebra::h_mat<2, C> m;
00089 
00090           m(0,0) = cos_a; m(0,1) = -sin_a; m(0,2) = 0;
00091           m(1,0) = sin_a; m(1,1) = cos_a;  m(1,2) = 0;
00092           m(2,0) = 0;     m(2,1) = 0;      m(2,2) = 1;
00093 
00094           return m;
00095         }
00096 
00097         // (Axis, angle)-based rotation: 3D case.
00098         template <typename C >
00099         algebra::h_mat<3, C>
00100         get_rot_h_mat(const C alpha, const algebra::vec<3,C>& axis)
00101         {
00102           // Ensure axis is valid.
00103           typedef algebra::vec<3,C> vec_t;
00104           // FIXME: This check is not precise enought when the vector
00105           // holds floating point values.
00106           mln_precondition(axis != vec_t(literal::zero));
00107 
00108           algebra::vec<3,C> normed_axis = axis;
00109           normed_axis.normalize();
00110 
00111           const C cos_a = cos(alpha);
00112           const C sin_a = sin(alpha);
00113           const C u = normed_axis[0];
00114           const C v = normed_axis[1];
00115           const C w = normed_axis[2];
00116           const C u2 = u * u;
00117           const C v2 = v * v;
00118           const C w2 = w * w;
00119 
00120           algebra::h_mat<3, C> m;
00121 
00122           m(0,0) = u2 + (1 - u2) * cos_a;
00123           m(0,1) = u * v * (1 - cos_a) - w * sin_a;
00124           m(0,2) = u * w * (1 - cos_a) + v * sin_a;
00125           m(0,3) = 0;
00126 
00127           m(1,0) = u * v * (1 - cos_a) + w * sin_a;
00128           m(1,1) = v2 + (1 - v2) * cos_a;
00129           m(1,2) = v * w * (1 - cos_a) - u * sin_a;
00130           m(1,3) = 0;
00131 
00132           m(2,0) = u * w * (1 - cos_a) - v * sin_a;
00133           m(2,1) = v * w * (1 - cos_a) + u * sin_a;
00134           m(2,2) = w2 + (1 - w2) * cos_a;
00135           m(2,3) = 0;
00136 
00137           m(3,0) = 0;
00138           m(3,1) = 0;
00139           m(3,2) = 0;
00140           m(3,3) = 1;
00141 
00142           return m;
00143         }
00144 
00145       } // end of namespace internal
00146 
00147 
00149       template <unsigned n, typename C>
00150       struct rotation
00151         : fun::internal::x2x_linear_impl_< algebra::vec<n,C>, C, rotation<n,C> >,
00152           public Function_v2v< rotation<n,C> >
00153       {
00155         typedef C data_t;
00156 
00158         typedef rotation<n,C> invert;
00160         invert inv() const;
00161 
00163         rotation();
00166         rotation(C alpha, const algebra::vec<n,C>& axis);
00168         rotation(const algebra::quat& q);
00170         rotation(const algebra::h_mat<n,C>& m);
00171 
00173         algebra::vec<n,C> operator()(const algebra::vec<n,C>& v) const;
00174 
00176         void set_alpha(C alpha);
00178         void set_axis(const algebra::vec<n,C>& axis);
00179 
00180       protected:
00181         void update();
00182         bool check_rotation(const algebra::quat& q);
00183 
00184         /* FIXME: Is it useful to keep these values, since they are
00185            primarily used to build the matrix `m_'?  */
00186         C alpha_;
00187         algebra::vec<n,C> axis_;
00188       };
00189 
00190 
00191 # ifndef MLN_INCLUDE_ONLY
00192 
00193       template <unsigned n, typename C>
00194       inline
00195       rotation<n,C>::rotation()
00196       {
00197       }
00198 
00199       template <unsigned n, typename C>
00200       inline
00201       rotation<n,C>::rotation(C alpha, const algebra::vec<n,C>& axis)
00202         : alpha_(alpha),
00203           axis_(axis)
00204       {
00205         this->m_ = algebra::h_mat<n,C>::Id;
00206         update();
00207       }
00208 
00209       template <unsigned n, typename C>
00210       inline
00211       rotation<n,C>::rotation(const algebra::quat& q)
00212       {
00213         // FIXME: Should also work for 2D.
00214         mlc_bool(n == 3)::check();
00215         mln_precondition(q.is_unit());
00216 
00217         C
00218           w = q.to_vec()[0],
00219           x = q.to_vec()[1],  x2 = 2*x*x,  xw = 2*x*w,
00220           y = q.to_vec()[2],  y2 = 2*y*y,  xy = 2*x*y,  yw = 2*y*w,
00221           z = q.to_vec()[3],  z2 = 2*z*z,  xz = 2*x*z,  yz = 2*y*z,  zw = 2*z*w;
00222 
00223         C t[9] = {1.f - y2 - z2,  xy - zw,  xz + yw,
00224                       xy + zw,  1.f - x2 - z2,  yz - xw,
00225                       xz - yw,  yz + xw,  1.f - x2 - y2};
00226 
00227         this->m_ = mln::make::h_mat(t);
00228         mln_assertion(check_rotation(q));
00229 
00231         alpha_ = acos(w) * 2;
00232         axis_[0] = x;
00233         axis_[1] = y;
00234         axis_[2] = z;
00235         axis_.normalize();
00236       }
00237 
00238 
00239       template <unsigned n, typename C>
00240       inline
00241       rotation<n,C>::rotation(const algebra::h_mat<n,C>& m)
00242       {
00243         this->m_ = m;
00244       }
00245 
00246 
00247       template <unsigned n, typename C>
00248       inline
00249       algebra::vec<n,C>
00250       rotation<n,C>::operator()(const algebra::vec<n,C>& v) const
00251       {
00252         algebra::mat<n+1,1,C> hmg;
00253         algebra::mat<n+1,1,C> tmp;
00254         algebra::vec<n,C> res;
00255         for (unsigned i = 0; i < n; ++i)
00256           hmg(i,0) = v[i];
00257         hmg(n,0) = 1;
00258         tmp = this->m_ * hmg;
00259         mln_assertion(tmp(n,0) == 1);
00260         for (unsigned i = 0; i < n; ++i)
00261           res[i] = tmp(i,0);
00262         return res;
00263       }
00264 
00265       template <unsigned n, typename C>
00266       inline
00267       rotation<n,C>
00268       rotation<n,C>::inv() const
00269       {
00270         typename rotation::invert res(-alpha_, axis_);
00271         return res;
00272       }
00273 
00274       template <unsigned n, typename C>
00275       inline
00276       void
00277       rotation<n,C>::set_alpha(C alpha)
00278       {
00279         alpha_ = alpha;
00280         update();
00281       }
00282 
00283       template <unsigned n, typename C>
00284       inline
00285       void
00286       rotation<n,C>::set_axis(const algebra::vec<n,C>& axis)
00287       {
00288         axis_ = axis;
00289         update();
00290       }
00291 
00292       // Homogenous matrix for a rotation of a point (x,y,z)
00293       // about the vector (u,v,w) by the angle alpha.
00294       template <unsigned n, typename C>
00295       inline
00296       void
00297       rotation<n,C>::update()
00298       {
00299         this->m_ = internal::get_rot_h_mat(alpha_, axis_);
00300       }
00301 
00302       template <unsigned n, typename C>
00303       inline
00304       bool
00305       rotation<n,C>::check_rotation(const algebra::quat& q)
00306       {
00307         srand(time(0));
00308         assert(q.is_unit());
00309 
00310         algebra::vec<n,C>
00311           tmp = make::vec(rand(), rand(), rand()),
00312               p = tmp / norm::l2(tmp),
00313               p_rot_1 = q.rotate(p),
00314               p_rot_2 = (*this)(p);
00315         return norm::l2(p_rot_1 - p_rot_2) < mln_epsilon(C);
00316       }
00317 
00318 # endif // ! MLN_INCLUDE_ONLY
00319 
00320 
00321     } // end of namespace mln::fun::x2x
00322 
00323   } // end of namespace mln::fun
00324 
00325 } // end of namespace mln
00326 
00327 
00328 #endif // ! MLN_FUN_X2X_ROTATION_HH
 All Classes Namespaces Functions Variables Typedefs Enumerator