Milena (Olena)  User documentation 2.0a Id
 All Classes Namespaces Functions Variables Typedefs Enumerator Groups Pages
rotation.hh
1 // Copyright (C) 2007, 2008, 2009, 2010, 2011 EPITA Research and
2 // Development Laboratory (LRDE)
3 //
4 // This file is part of Olena.
5 //
6 // Olena is free software: you can redistribute it and/or modify it under
7 // the terms of the GNU General Public License as published by the Free
8 // Software Foundation, version 2 of the License.
9 //
10 // Olena is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Public License for more details.
14 //
15 // You should have received a copy of the GNU General Public License
16 // along with Olena. If not, see <http://www.gnu.org/licenses/>.
17 //
18 // As a special exception, you may use this file as part of a free
19 // software project without restriction. Specifically, if other files
20 // instantiate templates or use macros or inline functions from this
21 // file, or you compile this file and link it with other files to produce
22 // an executable, this file does not by itself cause the resulting
23 // executable to be covered by the GNU General Public License. This
24 // exception does not however invalidate any other reasons why the
25 // executable file might be covered by the GNU General Public License.
26 
27 #ifndef MLN_FUN_X2X_ROTATION_HH
28 # define MLN_FUN_X2X_ROTATION_HH
29 
39 
40 # include <cstdlib>
41 # include <cmath>
42 
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>
49 
50 # include <mln/norm/l2.hh>
51 
52 
53 namespace mln
54 {
55 
56  namespace fun
57  {
58 
59  namespace x2x
60  {
61 
62  namespace internal
63  {
64  // (Axis, angle)-based rotation: general case (not implemented).
65  template < unsigned n, typename C >
66  algebra::h_mat<n, C>
67  get_rot_h_mat(const C alpha_, const algebra::vec<n,C>& axis_)
68  {
69  (void) alpha_;
70  (void) axis_;
71 
72  std::cerr
73  << __FILE__ << ":" << __LINE__ << ": error:"
74  << " generic mln::fun::x2x::internal::get_rot_h_mat<n, C>"
75  << " not implemented."
76  << std::endl;
77  std::abort();
78  }
79 
80  // (Axis, angle)-based rotation: 2D case.
81  template <typename C >
82  algebra::h_mat<2, C>
83  get_rot_h_mat(const C alpha, const algebra::vec<2,C>&)
84  {
85  const C cos_a = cos(alpha);
86  const C sin_a = sin(alpha);
87 
88  algebra::h_mat<2, C> m;
89 
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;
93 
94  return m;
95  }
96 
97  // (Axis, angle)-based rotation: 3D case.
98  template <typename C >
99  algebra::h_mat<3, C>
100  get_rot_h_mat(const C alpha, const algebra::vec<3,C>& axis)
101  {
102  // Ensure axis is valid.
103  typedef algebra::vec<3,C> vec_t;
104  // FIXME: This check is not precise enought when the vector
105  // holds floating point values.
106  mln_precondition(axis != vec_t(literal::zero));
107 
108  algebra::vec<3,C> normed_axis = axis;
109  normed_axis.normalize();
110 
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];
116  const C u2 = u * u;
117  const C v2 = v * v;
118  const C w2 = w * w;
119 
120  algebra::h_mat<3, C> m;
121 
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;
125  m(0,3) = 0;
126 
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;
130  m(1,3) = 0;
131 
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;
135  m(2,3) = 0;
136 
137  m(3,0) = 0;
138  m(3,1) = 0;
139  m(3,2) = 0;
140  m(3,3) = 1;
141 
142  return m;
143  }
144 
145  } // end of namespace internal
146 
147 
149  template <unsigned n, typename C>
150  struct rotation
151  : fun::internal::x2x_linear_impl_< algebra::vec<n,C>, C, rotation<n,C> >,
152  public Function_v2v< rotation<n,C> >
153  {
155  typedef C data_t;
156 
160  invert inv() const;
161 
163  rotation();
166  rotation(C alpha, const algebra::vec<n,C>& axis);
168  rotation(const algebra::quat& q);
170  rotation(const algebra::h_mat<n,C>& m);
171 
173  algebra::vec<n,C> operator()(const algebra::vec<n,C>& v) const;
174 
176  void set_alpha(C alpha);
178  void set_axis(const algebra::vec<n,C>& axis);
179 
180  protected:
181  void update();
182  bool check_rotation(const algebra::quat& q);
183 
184  /* FIXME: Is it useful to keep these values, since they are
185  primarily used to build the matrix `m_'? */
186  C alpha_;
187  algebra::vec<n,C> axis_;
188  };
189 
190 
191 # ifndef MLN_INCLUDE_ONLY
192 
193  template <unsigned n, typename C>
194  inline
196  {
197  }
198 
199  template <unsigned n, typename C>
200  inline
201  rotation<n,C>::rotation(C alpha, const algebra::vec<n,C>& axis)
202  : alpha_(alpha),
203  axis_(axis)
204  {
205  this->m_ = algebra::h_mat<n,C>::Id;
206  update();
207  }
208 
209  template <unsigned n, typename C>
210  inline
211  rotation<n,C>::rotation(const algebra::quat& q)
212  {
213  // FIXME: Should also work for 2D.
214  mlc_bool(n == 3)::check();
215  mln_precondition(q.is_unit());
216 
217  C
218  w = q.to_vec()[0],
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;
222 
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};
226 
227  this->m_ = mln::make::h_mat(t);
228  mln_assertion(check_rotation(q));
229 
231  alpha_ = acos(w) * 2;
232  axis_[0] = x;
233  axis_[1] = y;
234  axis_[2] = z;
235  axis_.normalize();
236  }
237 
238 
239  template <unsigned n, typename C>
240  inline
242  {
243  this->m_ = m;
244  }
245 
246 
247  template <unsigned n, typename C>
248  inline
249  algebra::vec<n,C>
250  rotation<n,C>::operator()(const algebra::vec<n,C>& v) const
251  {
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)
256  hmg(i,0) = v[i];
257  hmg(n,0) = 1;
258  tmp = this->m_ * hmg;
259  mln_assertion(tmp(n,0) == 1);
260  for (unsigned i = 0; i < n; ++i)
261  res[i] = tmp(i,0);
262  return res;
263  }
264 
265  template <unsigned n, typename C>
266  inline
269  {
270  typename rotation::invert res(-alpha_, axis_);
271  return res;
272  }
273 
274  template <unsigned n, typename C>
275  inline
276  void
278  {
279  alpha_ = alpha;
280  update();
281  }
282 
283  template <unsigned n, typename C>
284  inline
285  void
286  rotation<n,C>::set_axis(const algebra::vec<n,C>& axis)
287  {
288  axis_ = axis;
289  update();
290  }
291 
292  // Homogenous matrix for a rotation of a point (x,y,z)
293  // about the vector (u,v,w) by the angle alpha.
294  template <unsigned n, typename C>
295  inline
296  void
298  {
299  this->m_ = internal::get_rot_h_mat(alpha_, axis_);
300  }
301 
302  template <unsigned n, typename C>
303  inline
304  bool
305  rotation<n,C>::check_rotation(const algebra::quat& q)
306  {
307  srand(time(0));
308  assert(q.is_unit());
309 
310  algebra::vec<n,C>
311  tmp = make::vec(rand(), rand(), rand()),
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);
316  }
317 
318 # endif // ! MLN_INCLUDE_ONLY
319 
320 
321  } // end of namespace mln::fun::x2x
322 
323  } // end of namespace mln::fun
324 
325 } // end of namespace mln
326 
327 
328 #endif // ! MLN_FUN_X2X_ROTATION_HH