rotation.hh 7.25 KB
Newer Older
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
7
8
9
10
// Olena is free software: you can redistribute it and/or modify it under
// the terms of the GNU General Public License as published by the Free
// Software Foundation, version 2 of the License.
//
// Olena is distributed in the hope that it will be useful,
11
12
13
14
15
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// 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
22
23
24
25
// file, or you compile this file and link it with other files to produce
// an executable, this file does not by itself cause the resulting
// executable to be covered by the GNU General Public License.  This
// exception does not however invalidate any other reasons why the
// executable file might be covered by the GNU General Public License.
26
27
28
29

#ifndef MLN_FUN_X2X_ROTATION_HH
# define MLN_FUN_X2X_ROTATION_HH

30
/// \file
31
///
32
/// \brief Define a rotation function.
Guillaume Lazzara's avatar
Guillaume Lazzara committed
33
34
35
///
/// \todo store the quaternion instead of (axis, alpha)
///	  => better precision while composing two rotation matrices.
Guillaume Lazzara's avatar
Guillaume Lazzara committed
36
///
37
/// FIXME: Find a better reference...
Guillaume Lazzara's avatar
Guillaume Lazzara committed
38
39
/// Conversion from Quaternion to (angle,axis), Source:
///   http://jeux.developpez.com/faq/matquat/?page=quaternions#Q56
40

41
# include <cstdlib>
42
# include <cmath>
43

Simon Nivault's avatar
Simon Nivault committed
44
# include <mln/core/concept/function.hh>
45
# include <mln/fun/internal/x2x_linear_impl.hh>
46
47
# include <mln/algebra/vec.hh>
# include <mln/algebra/mat.hh>
Ugo Jardonnet's avatar
Ugo Jardonnet committed
48
# include <mln/algebra/quat.hh>
49
50
51
# include <mln/make/h_mat.hh>

# include <mln/norm/l2.hh>
52

53

54
55
56
57
58
59
60
61
62
namespace mln
{

  namespace fun
  {

    namespace x2x
    {

Ugo Jardonnet's avatar
Ugo Jardonnet committed
63
64
      namespace internal
      {
65
66
67
68
69
	// (Axis, angle)-based rotation: general case (not implemented).
	template < unsigned n, typename C >
	algebra::h_mat<n, C>
	get_rot_h_mat(const C alpha_, const algebra::vec<n,C>& axis_)
	{
70
71
72
	  (void) alpha_;
	  (void) axis_;

73
74
75
76
77
78
	  std::cerr
	    << __FILE__ << ":" << __LINE__ << ": error:"
	    << " generic mln::fun::x2x::internal::get_rot_h_mat<n, C>"
	    << " not implemented."
	    << std::endl;
	  std::abort();
79
80
	}

81
	// (Angle)-based rotation: 2D case.
82
83
	template <typename C >
	algebra::h_mat<2, C>
84
	get_rot_h_mat(const C alpha, const algebra::vec<2,C>& /* unused */)
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
	{
	  const C cos_a = cos(alpha);
	  const C sin_a = sin(alpha);

	  algebra::h_mat<2, C> m;

	  m(0,0) = cos_a; m(0,1) = -sin_a; m(0,2) = 0;
	  m(1,0) = sin_a; m(1,1) = cos_a;  m(1,2) = 0;
	  m(2,0) = 0;     m(2,1) = 0;      m(2,2) = 1;

	  return m;
	}

	// (Axis, angle)-based rotation: 3D case.
	template <typename C >
	algebra::h_mat<3, C>
	get_rot_h_mat(const C alpha, const algebra::vec<3,C>& axis)
	{
	  // Ensure axis is valid.
Guillaume Lazzara's avatar
Guillaume Lazzara committed
104
	  typedef algebra::vec<3,C> vec_t;
105
	  // FIXME: This check is not precise enough when the vector
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
	  // holds floating point values.
	  mln_precondition(axis != vec_t(literal::zero));

	  algebra::vec<3,C> normed_axis = axis;
	  normed_axis.normalize();

	  const C cos_a = cos(alpha);
	  const C sin_a = sin(alpha);
	  const C u = normed_axis[0];
	  const C v = normed_axis[1];
	  const C w = normed_axis[2];
	  const C u2 = u * u;
	  const C v2 = v * v;
	  const C w2 = w * w;

	  algebra::h_mat<3, C> m;

	  m(0,0) = u2 + (1 - u2) * cos_a;
	  m(0,1) = u * v * (1 - cos_a) - w * sin_a;
	  m(0,2) = u * w * (1 - cos_a) + v * sin_a;
	  m(0,3) = 0;

	  m(1,0) = u * v * (1 - cos_a) + w * sin_a;
	  m(1,1) = v2 + (1 - v2) * cos_a;
	  m(1,2) = v * w * (1 - cos_a) - u * sin_a;
	  m(1,3) = 0;

	  m(2,0) = u * w * (1 - cos_a) - v * sin_a;
	  m(2,1) = v * w * (1 - cos_a) + u * sin_a;
	  m(2,2) = w2 + (1 - w2) * cos_a;
	  m(2,3) = 0;

	  m(3,0) = 0;
	  m(3,1) = 0;
	  m(3,2) = 0;
	  m(3,3) = 1;

	  return m;
	}
Ugo Jardonnet's avatar
Ugo Jardonnet committed
145
146

      } // end of namespace internal
Ugo Jardonnet's avatar
Ugo Jardonnet committed
147
148


149
      /// Represent a rotation function.
150
      template <unsigned n, typename C>
Simon Nivault's avatar
Simon Nivault committed
151
      struct rotation
152
153
	: fun::internal::x2x_linear_impl_< algebra::vec<n,C>, C, rotation<n,C> >,
	  public Function_v2v< rotation<n,C> >
154
      {
155
156
157
	/// Type of the underlying data stored in vectors and matrices.
	typedef C data_t;

158
	/// Type of the inverse function.
Simon Nivault's avatar
Simon Nivault committed
159
	typedef rotation<n,C> invert;
160
	/// Return the inverse function.
Simon Nivault's avatar
Simon Nivault committed
161
	invert inv() const;
162

163
	/// Constructor without argument.
164
	rotation();
165
	/// Constructor with radian alpha and an optional direction
166
	/// (rotation axis).
167
	rotation(const C& alpha, const algebra::vec<n,C>& axis = literal::zero);
168
169
	/// Constructor with quaternion
	rotation(const algebra::quat& q);
170
	/// Constructor with h_mat.
171
	rotation(const algebra::h_mat<n,C>& m);
172

173
	/// Perform the rotation of the given vector.
174
	algebra::vec<n,C> operator()(const algebra::vec<n,C>& v) const;
175

Simon Nivault's avatar
Simon Nivault committed
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
184

      };


# ifndef MLN_INCLUDE_ONLY

185

186
      template <unsigned n, typename C>
187
      inline
188
189
190
191
192
      rotation<n,C>::rotation()
      {
      }

      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
      }

Ugo Jardonnet's avatar
Ugo Jardonnet committed
200
201
202
203
      template <unsigned n, typename C>
      inline
      rotation<n,C>::rotation(const algebra::quat& q)
      {
204
205
206
	// FIXME: Should also work for 2D.
	mlc_bool(n == 3)::check();
	mln_precondition(q.is_unit());
Guillaume Lazzara's avatar
Guillaume Lazzara committed
207

208
	this->m_ = mln::make::h_mat(C(), q);
209
210
211
212
213
214
215
216
217
	mln_assertion(check_rotation(q));
      }


      template <unsigned n, typename C>
      inline
      rotation<n,C>::rotation(const algebra::h_mat<n,C>& m)
      {
	this->m_ = m;
Ugo Jardonnet's avatar
Ugo Jardonnet committed
218
219
      }

220

221
      template <unsigned n, typename C>
222
      inline
223
224
      algebra::vec<n,C>
      rotation<n,C>::operator()(const algebra::vec<n,C>& v) const
225
      {
226
227
228
	algebra::mat<n+1,1,C> hmg;
	algebra::mat<n+1,1,C> tmp;
	algebra::vec<n,C> res;
229
230
231
	for (unsigned i = 0; i < n; ++i)
	  hmg(i,0) = v[i];
	hmg(n,0) = 1;
Simon Nivault's avatar
Simon Nivault committed
232
	tmp = this->m_ * hmg;
233
234
235
236
237
238
239
	mln_assertion(tmp(n,0) == 1);
	for (unsigned i = 0; i < n; ++i)
	  res[i] = tmp(i,0);
	return res;
      }

      template <unsigned n, typename C>
240
      inline
241
242
243
      rotation<n,C>
      rotation<n,C>::inv() const
      {
244
	typename rotation::invert res(this->m_._1());
245
246
247
	return res;
      }

Ugo Jardonnet's avatar
Ugo Jardonnet committed
248
      // Homogenous matrix for a rotation of a point (x,y,z)
249
      // about the vector (u,v,w) by the angle alpha.
Simon Nivault's avatar
Simon Nivault committed
250
      template <unsigned n, typename C>
251
      inline
Simon Nivault's avatar
Simon Nivault committed
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
259
260
261
262
263
264
265
266
267
268
269
270
      template <unsigned n, typename C>
      inline
      bool
      rotation<n,C>::check_rotation(const algebra::quat& q)
      {
	srand(time(0));
	assert(q.is_unit());

	algebra::vec<n,C>
	  tmp = make::vec(rand(), rand(), rand()),
	      p = tmp / norm::l2(tmp),
	      p_rot_1 = q.rotate(p),
	      p_rot_2 = (*this)(p);
Guillaume Lazzara's avatar
Guillaume Lazzara committed
271
	return norm::l2(p_rot_1 - p_rot_2) < mln_epsilon(C);
272
273
      }

274
275
# endif // ! MLN_INCLUDE_ONLY

276

277
278
279
280
281
282
283
284
    } // end of namespace mln::fun::x2x

  } // end of namespace mln::fun

} // end of namespace mln


#endif // ! MLN_FUN_X2X_ROTATION_HH