00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
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 std::cerr
00070 << __FILE__ << ":" << __LINE__ << ": error:"
00071 << " generic mln::fun::x2x::internal::get_rot_h_mat<n, C>"
00072 << " not implemented."
00073 << std::endl;
00074 std::abort();
00075 }
00076
00077
00078 template <typename C >
00079 algebra::h_mat<2, C>
00080 get_rot_h_mat(const C alpha, const algebra::vec<2,C>&)
00081 {
00082 const C cos_a = cos(alpha);
00083 const C sin_a = sin(alpha);
00084
00085 algebra::h_mat<2, C> m;
00086
00087 m(0,0) = cos_a; m(0,1) = -sin_a; m(0,2) = 0;
00088 m(1,0) = sin_a; m(1,1) = cos_a; m(1,2) = 0;
00089 m(2,0) = 0; m(2,1) = 0; m(2,2) = 1;
00090
00091 return m;
00092 }
00093
00094
00095 template <typename C >
00096 algebra::h_mat<3, C>
00097 get_rot_h_mat(const C alpha, const algebra::vec<3,C>& axis)
00098 {
00099
00100 typedef algebra::vec<3,C> vec_t;
00101
00102
00103 mln_precondition(axis != vec_t(literal::zero));
00104
00105 algebra::vec<3,C> normed_axis = axis;
00106 normed_axis.normalize();
00107
00108 const C cos_a = cos(alpha);
00109 const C sin_a = sin(alpha);
00110 const C u = normed_axis[0];
00111 const C v = normed_axis[1];
00112 const C w = normed_axis[2];
00113 const C u2 = u * u;
00114 const C v2 = v * v;
00115 const C w2 = w * w;
00116
00117 algebra::h_mat<3, C> m;
00118
00119 m(0,0) = u2 + (1 - u2) * cos_a;
00120 m(0,1) = u * v * (1 - cos_a) - w * sin_a;
00121 m(0,2) = u * w * (1 - cos_a) + v * sin_a;
00122 m(0,3) = 0;
00123
00124 m(1,0) = u * v * (1 - cos_a) + w * sin_a;
00125 m(1,1) = v2 + (1 - v2) * cos_a;
00126 m(1,2) = v * w * (1 - cos_a) - u * sin_a;
00127 m(1,3) = 0;
00128
00129 m(2,0) = u * w * (1 - cos_a) - v * sin_a;
00130 m(2,1) = v * w * (1 - cos_a) + u * sin_a;
00131 m(2,2) = w2 + (1 - w2) * cos_a;
00132 m(2,3) = 0;
00133
00134 m(3,0) = 0;
00135 m(3,1) = 0;
00136 m(3,2) = 0;
00137 m(3,3) = 1;
00138
00139 return m;
00140 }
00141
00142 }
00143
00144
00146 template <unsigned n, typename C>
00147 struct rotation
00148 : fun::internal::x2x_linear_impl_< algebra::vec<n,C>, C, rotation<n,C> >,
00149 public Function_v2v< rotation<n,C> >
00150 {
00152 typedef C data_t;
00153
00155 typedef rotation<n,C> invert;
00157 invert inv() const;
00158
00160 rotation();
00163 rotation(C alpha, const algebra::vec<n,C>& axis);
00165 rotation(const algebra::quat& q);
00167 rotation(const algebra::h_mat<n,C>& m);
00168
00170 algebra::vec<n,C> operator()(const algebra::vec<n,C>& v) const;
00171
00173 void set_alpha(C alpha);
00175 void set_axis(const algebra::vec<n,C>& axis);
00176
00177 protected:
00178 void update();
00179 bool check_rotation(const algebra::quat& q);
00180
00181
00182
00183 C alpha_;
00184 algebra::vec<n,C> axis_;
00185 };
00186
00187
00188 # ifndef MLN_INCLUDE_ONLY
00189
00190 template <unsigned n, typename C>
00191 inline
00192 rotation<n,C>::rotation()
00193 {
00194 }
00195
00196 template <unsigned n, typename C>
00197 inline
00198 rotation<n,C>::rotation(C alpha, const algebra::vec<n,C>& axis)
00199 : alpha_(alpha),
00200 axis_(axis)
00201 {
00202 this->m_ = algebra::h_mat<n,C>::Id;
00203 update();
00204 }
00205
00206 template <unsigned n, typename C>
00207 inline
00208 rotation<n,C>::rotation(const algebra::quat& q)
00209 {
00210
00211 mlc_bool(n == 3)::check();
00212 mln_precondition(q.is_unit());
00213
00214 C
00215 w = q.to_vec()[0],
00216 x = q.to_vec()[1], x2 = 2*x*x, xw = 2*x*w,
00217 y = q.to_vec()[2], y2 = 2*y*y, xy = 2*x*y, yw = 2*y*w,
00218 z = q.to_vec()[3], z2 = 2*z*z, xz = 2*x*z, yz = 2*y*z, zw = 2*z*w;
00219
00220 C t[9] = {1.f - y2 - z2, xy - zw, xz + yw,
00221 xy + zw, 1.f - x2 - z2, yz - xw,
00222 xz - yw, yz + xw, 1.f - x2 - y2};
00223
00224 this->m_ = mln::make::h_mat(t);
00225 mln_assertion(check_rotation(q));
00226
00228 alpha_ = acos(w) * 2;
00229 axis_[0] = x;
00230 axis_[1] = y;
00231 axis_[2] = z;
00232 axis_.normalize();
00233 }
00234
00235
00236 template <unsigned n, typename C>
00237 inline
00238 rotation<n,C>::rotation(const algebra::h_mat<n,C>& m)
00239 {
00240 this->m_ = m;
00241 }
00242
00243
00244 template <unsigned n, typename C>
00245 inline
00246 algebra::vec<n,C>
00247 rotation<n,C>::operator()(const algebra::vec<n,C>& v) const
00248 {
00249 algebra::mat<n+1,1,C> hmg;
00250 algebra::mat<n+1,1,C> tmp;
00251 algebra::vec<n,C> res;
00252 for (unsigned i = 0; i < n; ++i)
00253 hmg(i,0) = v[i];
00254 hmg(n,0) = 1;
00255 tmp = this->m_ * hmg;
00256 mln_assertion(tmp(n,0) == 1);
00257 for (unsigned i = 0; i < n; ++i)
00258 res[i] = tmp(i,0);
00259 return res;
00260 }
00261
00262 template <unsigned n, typename C>
00263 inline
00264 rotation<n,C>
00265 rotation<n,C>::inv() const
00266 {
00267 typename rotation::invert res(-alpha_, axis_);
00268 return res;
00269 }
00270
00271 template <unsigned n, typename C>
00272 inline
00273 void
00274 rotation<n,C>::set_alpha(C alpha)
00275 {
00276 alpha_ = alpha;
00277 update();
00278 }
00279
00280 template <unsigned n, typename C>
00281 inline
00282 void
00283 rotation<n,C>::set_axis(const algebra::vec<n,C>& axis)
00284 {
00285 axis_ = axis;
00286 update();
00287 }
00288
00289
00290
00291 template <unsigned n, typename C>
00292 inline
00293 void
00294 rotation<n,C>::update()
00295 {
00296 this->m_ = internal::get_rot_h_mat(alpha_, axis_);
00297 }
00298
00299 template <unsigned n, typename C>
00300 inline
00301 bool
00302 rotation<n,C>::check_rotation(const algebra::quat& q)
00303 {
00304 srand(time(0));
00305 assert(q.is_unit());
00306
00307 algebra::vec<n,C>
00308 tmp = make::vec(rand(), rand(), rand()),
00309 p = tmp / norm::l2(tmp),
00310 p_rot_1 = q.rotate(p),
00311 p_rot_2 = (*this)(p);
00312 return norm::l2(p_rot_1 - p_rot_2) < mln_epsilon(C);
00313 }
00314
00315 # endif // ! MLN_INCLUDE_ONLY
00316
00317
00318 }
00319
00320 }
00321
00322 }
00323
00324
00325 #endif // ! MLN_FUN_X2X_ROTATION_HH