Milena (Olena)
User documentation 2.0a Id
|
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