27 #ifndef MLN_FUN_X2X_ROTATION_HH
28 # define MLN_FUN_X2X_ROTATION_HH
43 # include <mln/core/concept/function.hh>
44 # include <mln/fun/internal/x2x_linear_impl.hh>
45 # include <mln/algebra/vec.hh>
46 # include <mln/algebra/mat.hh>
47 # include <mln/algebra/quat.hh>
48 # include <mln/make/h_mat.hh>
50 # include <mln/norm/l2.hh>
65 template <
unsigned n,
typename C >
67 get_rot_h_mat(
const C alpha_,
const algebra::vec<n,C>& axis_)
73 << __FILE__ <<
":" << __LINE__ <<
": error:"
74 <<
" generic mln::fun::x2x::internal::get_rot_h_mat<n, C>"
75 <<
" not implemented."
81 template <
typename C >
83 get_rot_h_mat(
const C alpha,
const algebra::vec<2,C>&)
85 const C cos_a = cos(alpha);
86 const C sin_a = sin(alpha);
88 algebra::h_mat<2, C> m;
90 m(0,0) = cos_a; m(0,1) = -sin_a; m(0,2) = 0;
91 m(1,0) = sin_a; m(1,1) = cos_a; m(1,2) = 0;
92 m(2,0) = 0; m(2,1) = 0; m(2,2) = 1;
98 template <
typename C >
100 get_rot_h_mat(
const C alpha,
const algebra::vec<3,C>& axis)
103 typedef algebra::vec<3,C> vec_t;
108 algebra::vec<3,C> normed_axis = axis;
109 normed_axis.normalize();
111 const C cos_a = cos(alpha);
112 const C sin_a = sin(alpha);
113 const C u = normed_axis[0];
114 const C v = normed_axis[1];
115 const C w = normed_axis[2];
120 algebra::h_mat<3, C> m;
122 m(0,0) = u2 + (1 - u2) * cos_a;
123 m(0,1) = u * v * (1 - cos_a) - w * sin_a;
124 m(0,2) = u * w * (1 - cos_a) + v * sin_a;
127 m(1,0) = u * v * (1 - cos_a) + w * sin_a;
128 m(1,1) = v2 + (1 - v2) * cos_a;
129 m(1,2) = v * w * (1 - cos_a) - u * sin_a;
132 m(2,0) = u * w * (1 - cos_a) - v * sin_a;
133 m(2,1) = v * w * (1 - cos_a) + u * sin_a;
134 m(2,2) = w2 + (1 - w2) * cos_a;
149 template <
unsigned n,
typename C>
151 : fun::internal::x2x_linear_impl_< algebra::vec<n,C>, C, rotation<n,C> >,
166 rotation(C alpha,
const algebra::vec<n,C>& axis);
173 algebra::vec<n,C>
operator()(
const algebra::vec<n,C>& v)
const;
178 void set_axis(
const algebra::vec<n,C>& axis);
182 bool check_rotation(
const algebra::quat& q);
187 algebra::vec<n,C> axis_;
191 # ifndef MLN_INCLUDE_ONLY
193 template <
unsigned n,
typename C>
199 template <
unsigned n,
typename C>
209 template <
unsigned n,
typename C>
214 mlc_bool(n == 3)::check();
215 mln_precondition(q.is_unit());
219 x = q.to_vec()[1], x2 = 2*x*x, xw = 2*x*w,
220 y = q.to_vec()[2], y2 = 2*y*y, xy = 2*x*y, yw = 2*y*w,
221 z = q.to_vec()[3], z2 = 2*z*z, xz = 2*x*z, yz = 2*y*z, zw = 2*z*w;
223 C t[9] = {1.f - y2 - z2, xy - zw, xz + yw,
224 xy + zw, 1.f - x2 - z2, yz - xw,
225 xz - yw, yz + xw, 1.f - x2 - y2};
228 mln_assertion(check_rotation(q));
231 alpha_ = acos(w) * 2;
239 template <
unsigned n,
typename C>
247 template <
unsigned n,
typename C>
252 algebra::mat<n+1,1,C> hmg;
253 algebra::mat<n+1,1,C> tmp;
254 algebra::vec<n,C> res;
255 for (
unsigned i = 0; i < n; ++i)
258 tmp = this->m_ * hmg;
259 mln_assertion(tmp(n,0) == 1);
260 for (
unsigned i = 0; i < n; ++i)
265 template <
unsigned n,
typename C>
274 template <
unsigned n,
typename C>
283 template <
unsigned n,
typename C>
294 template <
unsigned n,
typename C>
299 this->m_ = internal::get_rot_h_mat(alpha_, axis_);
302 template <
unsigned n,
typename C>
305 rotation<n,C>::check_rotation(
const algebra::quat& q)
312 p = tmp / norm::l2(tmp),
313 p_rot_1 = q.rotate(p),
314 p_rot_2 = (*this)(p);
315 return norm::l2(p_rot_1 - p_rot_2) < mln_epsilon(C);
318 # endif // ! MLN_INCLUDE_ONLY
328 #endif // ! MLN_FUN_X2X_ROTATION_HH