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 (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
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
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
00103 typedef algebra::vec<3,C> vec_t;
00104
00105
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 }
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
00185
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
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
00293
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 }
00322
00323 }
00324
00325 }
00326
00327
00328 #endif // ! MLN_FUN_X2X_ROTATION_HH