$extrastylesheet
Olena  User documentation 2.1
An Image Processing Platform
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
rotation.hh
1 // Copyright (C) 2007, 2008, 2009, 2010, 2011, 2012 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 
40 
41 # include <cstdlib>
42 # include <cmath>
43 
44 # include <mln/core/concept/function.hh>
45 # include <mln/fun/internal/x2x_linear_impl.hh>
46 # include <mln/algebra/vec.hh>
47 # include <mln/algebra/mat.hh>
48 # include <mln/algebra/quat.hh>
49 # include <mln/make/h_mat.hh>
50 
51 # include <mln/norm/l2.hh>
52 
53 
54 namespace mln
55 {
56 
57  namespace fun
58  {
59 
60  namespace x2x
61  {
62 
63  namespace internal
64  {
65  // (Axis, angle)-based rotation: general case (not implemented).
66  template < unsigned n, typename C >
67  algebra::h_mat<n, C>
68  get_rot_h_mat(const C alpha_, const algebra::vec<n,C>& axis_)
69  {
70  (void) alpha_;
71  (void) axis_;
72 
73  std::cerr
74  << __FILE__ << ":" << __LINE__ << ": error:"
75  << " generic mln::fun::x2x::internal::get_rot_h_mat<n, C>"
76  << " not implemented."
77  << std::endl;
78  std::abort();
79  }
80 
81  // (Angle)-based rotation: 2D case.
82  template <typename C >
84  get_rot_h_mat(const C alpha, const algebra::vec<2,C>& /* unused */)
85  {
86  const C cos_a = cos(alpha);
87  const C sin_a = sin(alpha);
88 
90 
91  m(0,0) = cos_a; m(0,1) = -sin_a; m(0,2) = 0;
92  m(1,0) = sin_a; m(1,1) = cos_a; m(1,2) = 0;
93  m(2,0) = 0; m(2,1) = 0; m(2,2) = 1;
94 
95  return m;
96  }
97 
98  // (Axis, angle)-based rotation: 3D case.
99  template <typename C >
101  get_rot_h_mat(const C alpha, const algebra::vec<3,C>& axis)
102  {
103  // Ensure axis is valid.
104  typedef algebra::vec<3,C> vec_t;
105  // FIXME: This check is not precise enough when the vector
106  // holds floating point values.
107  mln_precondition(axis != vec_t(literal::zero));
108 
109  algebra::vec<3,C> normed_axis = axis;
110  normed_axis.normalize();
111 
112  const C cos_a = cos(alpha);
113  const C sin_a = sin(alpha);
114  const C u = normed_axis[0];
115  const C v = normed_axis[1];
116  const C w = normed_axis[2];
117  const C u2 = u * u;
118  const C v2 = v * v;
119  const C w2 = w * w;
120 
122 
123  m(0,0) = u2 + (1 - u2) * cos_a;
124  m(0,1) = u * v * (1 - cos_a) - w * sin_a;
125  m(0,2) = u * w * (1 - cos_a) + v * sin_a;
126  m(0,3) = 0;
127 
128  m(1,0) = u * v * (1 - cos_a) + w * sin_a;
129  m(1,1) = v2 + (1 - v2) * cos_a;
130  m(1,2) = v * w * (1 - cos_a) - u * sin_a;
131  m(1,3) = 0;
132 
133  m(2,0) = u * w * (1 - cos_a) - v * sin_a;
134  m(2,1) = v * w * (1 - cos_a) + u * sin_a;
135  m(2,2) = w2 + (1 - w2) * cos_a;
136  m(2,3) = 0;
137 
138  m(3,0) = 0;
139  m(3,1) = 0;
140  m(3,2) = 0;
141  m(3,3) = 1;
142 
143  return m;
144  }
145 
146  } // end of namespace internal
147 
148 
150  template <unsigned n, typename C>
151  struct rotation
152  : fun::internal::x2x_linear_impl_< algebra::vec<n,C>, C, rotation<n,C> >,
153  public Function_v2v< rotation<n,C> >
154  {
156  typedef C data_t;
157 
161  invert inv() const;
162 
164  rotation();
167  rotation(const C& alpha, const algebra::vec<n,C>& axis = literal::zero);
169  rotation(const algebra::quat& q);
171  rotation(const algebra::h_mat<n,C>& m);
172 
175 
176  protected:
177  void update(const C& alpha, const algebra::vec<n,C>& axis);
178  bool check_rotation(const algebra::quat& q);
179 
180  };
181 
182 
183 # ifndef MLN_INCLUDE_ONLY
184 
185 
186  template <unsigned n, typename C>
187  inline
189  {
190  }
191 
192  template <unsigned n, typename C>
193  inline
194  rotation<n,C>::rotation(const C& alpha, const algebra::vec<n,C>& axis)
195  {
196  this->m_ = algebra::h_mat<n,C>::Id;
197  update(alpha, axis);
198  }
199 
200  template <unsigned n, typename C>
201  inline
203  {
204  // FIXME: Should also work for 2D.
205  mlc_bool(n == 3)::check();
206  mln_precondition(q.is_unit());
207 
208  this->m_ = mln::make::h_mat(C(), q);
209  mln_assertion(check_rotation(q));
210  }
211 
212 
213  template <unsigned n, typename C>
214  inline
215  rotation<n,C>::rotation(const algebra::h_mat<n,C>& m)
216  {
217  this->m_ = m;
218  }
219 
220 
221  template <unsigned n, typename C>
222  inline
223  algebra::vec<n,C>
224  rotation<n,C>::operator()(const algebra::vec<n,C>& v) const
225  {
226  algebra::mat<n+1,1,C> hmg;
227  algebra::mat<n+1,1,C> tmp;
228  algebra::vec<n,C> res;
229  for (unsigned i = 0; i < n; ++i)
230  hmg(i,0) = v[i];
231  hmg(n,0) = 1;
232  tmp = this->m_ * hmg;
233  mln_assertion(tmp(n,0) == 1);
234  for (unsigned i = 0; i < n; ++i)
235  res[i] = tmp(i,0);
236  return res;
237  }
238 
239  template <unsigned n, typename C>
240  inline
241  rotation<n,C>
242  rotation<n,C>::inv() const
243  {
244  typename rotation::invert res(this->m_._1());
245  return res;
246  }
247 
248  // Homogenous matrix for a rotation of a point (x,y,z)
249  // about the vector (u,v,w) by the angle alpha.
250  template <unsigned n, typename C>
251  inline
252  void
253  rotation<n,C>::update(const C& alpha, const algebra::vec<n,C>& axis)
254  {
255  this->m_ = internal::get_rot_h_mat(alpha, axis);
256  }
257 
258  template <unsigned n, typename C>
259  inline
260  bool
261  rotation<n,C>::check_rotation(const algebra::quat& q)
262  {
263  srand(time(0));
264  assert(q.is_unit());
265 
266  algebra::vec<n,C>
267  tmp = make::vec(rand(), rand(), rand()),
268  p = tmp / norm::l2(tmp),
269  p_rot_1 = q.rotate(p),
270  p_rot_2 = (*this)(p);
271  return norm::l2(p_rot_1 - p_rot_2) < mln_epsilon(C);
272  }
273 
274 # endif // ! MLN_INCLUDE_ONLY
275 
276 
277  } // end of namespace mln::fun::x2x
278 
279  } // end of namespace mln::fun
280 
281 } // end of namespace mln
282 
283 
284 #endif // ! MLN_FUN_X2X_ROTATION_HH