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 #ifndef MLN_FUN_X2X_ROTATION_HH
00027 # define MLN_FUN_X2X_ROTATION_HH
00028
00038
00039 # include <cmath>
00040
00041 # include <mln/core/concept/function.hh>
00042 # include <mln/fun/internal/x2x_linear_impl.hh>
00043 # include <mln/algebra/vec.hh>
00044 # include <mln/algebra/mat.hh>
00045 # include <mln/algebra/quat.hh>
00046 # include <mln/make/h_mat.hh>
00047
00048 # include <mln/norm/l2.hh>
00049
00050
00051 namespace mln
00052 {
00053
00054 namespace fun
00055 {
00056
00057 namespace x2x
00058 {
00059
00060 namespace internal
00061 {
00062 template < unsigned n, typename C >
00063 algebra::h_mat<n, C>
00064 get_rot_h_mat(const C alpha_, const algebra::vec<n,C>& axis_)
00065 {
00066 mln_assertion(!"get_h_mat : n not implemented");
00067 }
00068
00069 template <typename C >
00070 algebra::h_mat<3, C>
00071 get_rot_h_mat(const C alpha_, const algebra::vec<3,C>& axis_)
00072 {
00073
00074 typedef algebra::vec<3,C> vec_t;
00075
00076 mln_precondition(!(axis_ == vec_t(literal::zero)));
00077
00079 algebra::vec<3,C> axis = axis_;
00080 axis.normalize();
00081
00082 const C cos_a = cos(alpha_);
00083 const C sin_a = sin(alpha_);
00084 const C u = axis_[0];
00085 const C v = axis_[1];
00086 const C w = axis_[2];
00087 const C u2 = u * u;
00088 const C v2 = v * v;
00089 const C w2 = w * w;
00090
00091 algebra::h_mat<3, C> m_;
00092
00093 m_(0,0) = u2 + (1 - u2) * cos_a;
00094 m_(0,1) = u*v * (1 - cos_a) - w * sin_a;
00095 m_(0,2) = u*w * (1 - cos_a) + v * sin_a;
00096 m_(0,3) = 0;
00097
00098 m_(1,0) = u*v * (1 - cos_a) + w * sin_a;
00099 m_(1,1) = v2 + (1 - v2) * cos_a;
00100 m_(1,2) = v * w * (1 - cos_a) - u * sin_a;
00101 m_(1,3) = 0;
00102
00103 m_(2,0) = u * w * (1 - cos_a) - v * sin_a;
00104 m_(2,1) = u * w * (1 - cos_a) + u * sin_a;
00105 m_(2,2) = w2 + (1 - w2) * cos_a;
00106 m_(2,3) = 0;
00107
00108 m_(3,0) = 0;
00109 m_(3,1) = 0;
00110 m_(3,2) = 0;
00111 m_(3,3) = 1;
00112
00113 return m_;
00114 }
00115
00116 template <typename C >
00117 algebra::h_mat<2, C>
00118 get_rot_h_mat(const C alpha_, const algebra::vec<2,C>&)
00119 {
00120 const C cos_a = cos(alpha_);
00121 const C sin_a = sin(alpha_);
00122
00123 algebra::h_mat<2, C> m_;
00124
00125 m_(0,0) = cos_a; m_(0,1) = -sin_a; m_(0,2) = 0;
00126 m_(1,0) = sin_a; m_(1,1) = cos_a; m_(1,2) = 0;
00127 m_(2,0) = 0; m_(2,1) = 0; m_(2,2) = 1;
00128
00129 return m_;
00130 }
00131
00132 }
00133
00134
00136 template <unsigned n, typename C>
00137 struct rotation
00138 :
00139 fun::internal::x2x_linear_impl_< algebra::vec<n,C>, rotation<n,C> >
00140 ,
00141 public Function_v2v< rotation<n,C> >
00142 {
00144 typedef rotation<n,C> invert;
00146 invert inv() const;
00147
00149 rotation();
00151 rotation(C alpha, const algebra::vec<n,C>& axis);
00153 rotation(const algebra::quat& q);
00155 rotation(const algebra::h_mat<n,C>& m);
00156
00158 algebra::vec<n,C> operator()(const algebra::vec<n,C>& v) const;
00159
00161 void set_alpha(C alpha);
00163 void set_axis(const algebra::vec<n,C>& axis);
00164
00165 protected:
00166 void update();
00167 bool check_rotation(const algebra::quat& q);
00168
00169
00171 C alpha_;
00172 algebra::vec<n,C> axis_;
00173 };
00174
00175
00176 # ifndef MLN_INCLUDE_ONLY
00177
00178 template <unsigned n, typename C>
00179 inline
00180 rotation<n,C>::rotation()
00181 {
00182 }
00183
00184 template <unsigned n, typename C>
00185 inline
00186 rotation<n,C>::rotation(C alpha, const algebra::vec<n,C>& axis)
00187 :alpha_(alpha),
00188 axis_(axis)
00189 {
00190 this->m_ = algebra::h_mat<n,C>::Id;
00191 update();
00192 }
00193
00194 template <unsigned n, typename C>
00195 inline
00196 rotation<n,C>::rotation(const algebra::quat& q)
00197 {
00198
00199 mlc_bool(n == 3)::check();
00200 mln_precondition(q.is_unit());
00201
00202 C
00203 w = q.to_vec()[0],
00204 x = q.to_vec()[1], x2 = 2*x*x, xw = 2*x*w,
00205 y = q.to_vec()[2], y2 = 2*y*y, xy = 2*x*y, yw = 2*y*w,
00206 z = q.to_vec()[3], z2 = 2*z*z, xz = 2*x*z, yz = 2*y*z, zw = 2*z*w;
00207
00208 C t[9] = {1.f - y2 - z2, xy - zw, xz + yw,
00209 xy + zw, 1.f - x2 - z2, yz - xw,
00210 xz - yw, yz + xw, 1.f - x2 - y2};
00211
00212 this->m_ = mln::make::h_mat(t);
00213 mln_assertion(check_rotation(q));
00214
00216
00217 alpha_ = acos(w) * 2;
00218 axis_[0] = x;
00219 axis_[1] = y;
00220 axis_[2] = z;
00221 axis_.normalize();
00222 }
00223
00224
00225 template <unsigned n, typename C>
00226 inline
00227 rotation<n,C>::rotation(const algebra::h_mat<n,C>& m)
00228 {
00229 this->m_ = m;
00230 }
00231
00232
00233 template <unsigned n, typename C>
00234 inline
00235 algebra::vec<n,C>
00236 rotation<n,C>::operator()(const algebra::vec<n,C>& v) const
00237 {
00238 algebra::mat<n+1,1,C> hmg;
00239 algebra::mat<n+1,1,C> tmp;
00240 algebra::vec<n,C> res;
00241 for (unsigned i = 0; i < n; ++i)
00242 hmg(i,0) = v[i];
00243 hmg(n,0) = 1;
00244 tmp = this->m_ * hmg;
00245 mln_assertion(tmp(n,0) == 1);
00246 for (unsigned i = 0; i < n; ++i)
00247 res[i] = tmp(i,0);
00248 return res;
00249 }
00250
00251 template <unsigned n, typename C>
00252 inline
00253 rotation<n,C>
00254 rotation<n,C>::inv() const
00255 {
00256 typename rotation::invert res(-alpha_, axis_);
00257 return res;
00258 }
00259
00260 template <unsigned n, typename C>
00261 inline
00262 void
00263 rotation<n,C>::set_alpha(C alpha)
00264 {
00265 alpha_ = alpha;
00266 update();
00267 }
00268
00269 template <unsigned n, typename C>
00270 inline
00271 void
00272 rotation<n,C>::set_axis(const algebra::vec<n,C>& axis)
00273 {
00274 axis_ = axis;
00275 update();
00276 }
00277
00278
00279
00280 template <unsigned n, typename C>
00281 inline
00282 void
00283 rotation<n,C>::update()
00284 {
00285 this->m_ = internal::get_rot_h_mat(alpha_, axis_);
00286 }
00287
00288 template <unsigned n, typename C>
00289 inline
00290 bool
00291 rotation<n,C>::check_rotation(const algebra::quat& q)
00292 {
00293 srand(time(0));
00294 assert(q.is_unit());
00295
00296 algebra::vec<n,C>
00297 tmp = make::vec(rand(), rand(), rand()),
00298 p = tmp / norm::l2(tmp),
00299 p_rot_1 = q.rotate(p),
00300 p_rot_2 = (*this)(p);
00301 return norm::l2(p_rot_1 - p_rot_2) < mln_epsilon(C);
00302 }
00303
00304 # endif // ! MLN_INCLUDE_ONLY
00305
00306
00307 }
00308
00309 }
00310
00311 }
00312
00313
00314 #endif // ! MLN_FUN_X2X_ROTATION_HH