IMP  2.0.0
The Integrative Modeling Platform
Rotation3D.h
Go to the documentation of this file.
1 /**
2  * \file IMP/algebra/Rotation3D.h \brief Simple 3D rotation class.
3  *
4  * Copyright 2007-2013 IMP Inventors. All rights reserved.
5  *
6  */
7 
8 #ifndef IMPALGEBRA_ROTATION_3D_H
9 #define IMPALGEBRA_ROTATION_3D_H
10 
11 #include <IMP/algebra/algebra_config.h>
12 #include "Vector3D.h"
13 #include "utility.h"
14 #include "constants.h"
15 #include "GeometricPrimitiveD.h"
16 
17 #include <IMP/base/log.h>
18 #include <cmath>
19 #include <iostream>
20 #include <algorithm>
21 
22 IMPALGEBRA_BEGIN_NAMESPACE
23 
24 #if !defined(IMP_DOXYGEN) && !defined(SWIG)
25 class Rotation3D;
26 Rotation3D compose(const Rotation3D &a, const Rotation3D &b) ;
27 #endif
28 
29 
30 //! 3D rotation class.
31 /** Rotations are currently represented using quaternions and a cached
32  copy of the rotation matrix. The quaternion allows for fast and
33  stable composition and the cached rotation matrix means that
34  rotations are performed quickly. See
35  http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation for
36  a comparison of different implementations of rotations.
37 
38  Currently the rotation can be initialized from either:
39  - XYZ Euler angles
40  - Rotation Matrix
41  - Quaternion
42  - angle/axis representation
43 
44  \geometry
45 */
46 class IMPALGEBRAEXPORT Rotation3D: public GeometricPrimitiveD<3> {
47  VectorD<4> v_;
48  mutable bool has_cache_;
49  mutable Vector3D matrix_[3];
50  IMP_NO_SWIG(friend Rotation3D compose(const Rotation3D &a,
51  const Rotation3D &b));
52  void fill_cache() const {
53  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
54  "Attempting to apply uninitialized rotation");
55  has_cache_=true;
56  double v0s=get_squared(v_[0]);
57  double v1s=get_squared(v_[1]);
58  double v2s=get_squared(v_[2]);
59  double v3s=get_squared(v_[3]);
60  double v12= v_[1]*v_[2];
61  double v01=v_[0]*v_[1];
62  double v02=v_[0]*v_[2];
63  double v23=v_[2]*v_[3];
64  double v03=v_[0]*v_[3];
65  double v13=v_[1]*v_[3];
66  matrix_[0]= Vector3D(v0s+v1s-v2s-v3s,
67  2*(v12-v03),
68  2*(v13+v02));
69  matrix_[1]= Vector3D(2*(v12+v03),
70  v0s-v1s+v2s-v3s,
71  2*(v23-v01));
72  matrix_[2]= Vector3D(2*(v13-v02),
73  2*(v23+v01),
74  v0s-v1s-v2s+v3s);
75  }
76  public:
77  //! Create a rotation from an unnormalized vector 4
79  v_(v.get_unit_vector()),
80  has_cache_(false){}
81 
82  //! Create an invalid rotation
83  Rotation3D():v_(0,0,0,0), has_cache_(false) {}
84  //! Create a rotation from a quaternion
85  /** \throw base::ValueException if the rotation is not a unit vector.
86  */
87  Rotation3D(double a, double b, double c, double d): v_(a,b,c,d),
88  has_cache_(false) {
89  IMP_USAGE_CHECK_FLOAT_EQUAL(v_.get_squared_magnitude(), 1.0,
90  "Attempting to construct a rotation from a "
91  << " non-quaternion value. The coefficient vector"
92  << " must have a length of 1. Got: "
93  << a << " " << b << " " << c << " " << d
94  << " gives " << v_.get_squared_magnitude());
95  if (a<0) {
96  // make them canonical
97  v_=-v_;
98  }
99 
100  }
101  ~Rotation3D();
102 
103 
104 #ifndef IMP_DOXYGEN
105  Vector3D get_rotated_no_cache(const Vector3D &o) const {
106  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
107  "Attempting to access uninitialized rotation");
108  return Vector3D((v_[0]*v_[0]+v_[1]*v_[1]-v_[2]*v_[2]-v_[3]*v_[3])*o[0]
109  + 2*(v_[1]*v_[2]-v_[0]*v_[3])*o[1]
110  + 2*(v_[1]*v_[3]+v_[0]*v_[2])*o[2],
111  2*(v_[1]*v_[2]+v_[0]*v_[3])*o[0]
112  + (v_[0]*v_[0]-v_[1]*v_[1]+v_[2]*v_[2]-v_[3]*v_[3])*o[1]
113  + 2*(v_[2]*v_[3]-v_[0]*v_[1])*o[2],
114  2*(v_[1]*v_[3]-v_[0]*v_[2])*o[0]
115  + 2*(v_[2]*v_[3]+v_[0]*v_[1])*o[1]
116  + (v_[0]*v_[0]-v_[1]*v_[1]-v_[2]*v_[2]+v_[3]*v_[3])*o[2]);
117  }
118 
119  //! Gets only the requested rotation coordinate of the vector
120  double get_rotated_one_coordinate_no_cache(const Vector3D &o,
121  unsigned int coord) const {
122  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
123  "Attempting to apply uninitialized rotation");
124  switch(coord) {
125  case 0:
126  return (v_[0]*v_[0]+v_[1]*v_[1]-v_[2]*v_[2]-v_[3]*v_[3])*o[0]
127  + 2*(v_[1]*v_[2]-v_[0]*v_[3])*o[1]
128  + 2*(v_[1]*v_[3]+v_[0]*v_[2])*o[2];
129  break;
130  case 1:
131  return 2*(v_[1]*v_[2]+v_[0]*v_[3])*o[0]
132  + (v_[0]*v_[0]-v_[1]*v_[1]+v_[2]*v_[2]-v_[3]*v_[3])*o[1]
133  + 2*(v_[2]*v_[3]-v_[0]*v_[1])*o[2];
134 
135  break;
136  case 2:
137  return 2*(v_[1]*v_[3]-v_[0]*v_[2])*o[0]
138  + 2*(v_[2]*v_[3]+v_[0]*v_[1])*o[1]
139  + (v_[0]*v_[0]-v_[1]*v_[1]-v_[2]*v_[2]+v_[3]*v_[3])*o[2];
140  break;
141  default:
142  IMP_THROW("Out of range coordinate " << coord,
144  }
145  }
146 #endif
147  //! Rotate a vector around the origin
148  Vector3D get_rotated(const Vector3D &o) const {
149  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
150  "Attempting to apply uninitialized rotation");
151  if (!has_cache_) fill_cache();
152  return Vector3D(o*matrix_[0],
153  o*matrix_[1],
154  o*matrix_[2]);
155  }
156 
157  //! Gets only the requested rotation coordinate of the vector
158  double get_rotated_one_coordinate(const Vector3D &o,
159  unsigned int coord) const {
160  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
161  "Attempting to apply uninitialized rotation");
162  if (!has_cache_) fill_cache();
163  return o*matrix_[coord];
164  }
165 
166  //! Rotate a vector around the origin
167  Vector3D operator*(const Vector3D &v) const {
168  return get_rotated(v);
169  }
170  Vector3D get_rotation_matrix_row(int i) const {
171  IMP_USAGE_CHECK((i>=0)&&(i<=2),"row index out of range");
172  if (!has_cache_) fill_cache();
173  return matrix_[i];
174  }
175  IMP_SHOWABLE_INLINE(Rotation3D, {out << v_[0] << " " << v_[1]<< " " <<v_[2]
176  << " " <<v_[3];});
177 
178  //! Return the rotation which undoes this rotation.
179  inline Rotation3D get_inverse() const {
180  IMP_USAGE_CHECK(v_.get_squared_magnitude() > 0,
181  "Attempting to invert uninitialized rotation");
182  Rotation3D ret(v_[0], -v_[1], -v_[2], -v_[3]);
183  return ret;
184  }
185 
186 
187  //! Return the quaternion so that it can be stored
188  /** Note that there is no guarantee on which of the two
189  equivalent quaternions is returned.
190  */
191  const Vector4D& get_quaternion() const {
192  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
193  "Attempting to access uninitialized rotation");
194  return v_;
195  }
196  //! multiply two rotations
197  Rotation3D operator*(const Rotation3D& q) const {
198  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
199  "Attempting to compose uninitialized rotation");
200  return compose(*this, q);
201  }
202 
203  //! Compute the rotation which when composed with r gives this
204  Rotation3D operator/(const Rotation3D &r) const {
205  IMP_USAGE_CHECK(v_.get_squared_magnitude() >0,
206  "Attempting to compose uninitialized rotation");
207  return compose(*this, r.get_inverse());
208  }
209 
210  const Rotation3D &operator/=(const Rotation3D &r) {
211  *this= *this/r;
212  return *this;
213  }
214 
215  /** \brief Return the derivative of the position x with respect to
216  internal variable i. */
217  const Vector3D get_derivative(const Vector3D &o, unsigned int i) const;
218 };
219 
221 
222 
223 //! Return a rotation that does not do anything
224 /** \relatesalso Rotation3D */
226  return Rotation3D(1,0,0,0);
227 }
228 
229 //! Return a distance between the two rotations
230 /** The distance runs between 0 and 1. More precisely,
231  the distance returned is distance between the two
232  quaternion vectors properly normalized, divided
233  by sqrt(2).
234 
235  A vector with distance d from the unit vector
236  represents a rotation of
237 
238  \f$ \theta= \cos^{-1}\left(1-\sqrt{2}d\right)\f$
239  \relatesalso Rotation3D
240 */
241 inline double get_distance(const Rotation3D &r0,
242  const Rotation3D &r1) {
243  double dot= (r0.get_quaternion()-r1.get_quaternion()).get_squared_magnitude();
244  double odot= (r0.get_quaternion()
245  +r1.get_quaternion()).get_squared_magnitude();
246  double ans= std::min(dot, odot);
247  const double s2=std::sqrt(2.0);
248  double ret= ans/s2;
249  return std::max(std::min(ret, 1.0), 0.0);
250 }
251 
252 //! Generate a Rotation3D object from a rotation around an axis
253 /**
254  \param[in] axis the rotation axis passes through (0,0,0)
255  \param[in] angle the rotation angle in radians in the
256  clockwise direction
257  \note http://en.wikipedia.org/wiki/Rotation_matrix
258  \note www.euclideanspace.com/maths/geometry/rotations/conversions/
259  angleToQuaternion/index.htm
260  \relatesalso Rotation3D
261 */
262 IMPALGEBRAEXPORT
263 Rotation3D get_rotation_about_axis(const Vector3D& axis,
264  double angle);
265 
266 //! Create a rotation from the first vector to the second one.
267 /** \relatesalso Rotation3D
268  */
269 IMPALGEBRAEXPORT
270 Rotation3D get_rotation_taking_first_to_second(const Vector3D &v1,
271  const Vector3D &v2);
272 
273 //! Generate a Rotation3D object from a rotation matrix
274 /**
275  \relatesalso Rotation3D
276 */
277 IMPALGEBRAEXPORT Rotation3D
278 get_rotation_from_matrix(double m00,double m01,double m02,
279  double m10,double m11,double m12,
280  double m20,double m21,double m22);
281 
282 
283 
284 
285 //! Pick a rotation at random from all possible rotations
286 /** \relatesalso Rotation3D */
287 IMPALGEBRAEXPORT Rotation3D get_random_rotation_3d();
288 
289 
290 //! Pick a rotation at random near the provided one
291 /** This method generates a rotation that is within the provided
292  distance of center.
293  \param[in] center The center of the rotational volume
294  \param[in] distance See
295  get_distance(const Rotation3D&,const Rotation3D&)
296  for a full definition.
297 
298  \note The cost of this operation increases as distance goes to 0.
299 
300  \relatesalso Rotation3D
301 */
302 IMPALGEBRAEXPORT Rotation3D get_random_rotation_3d(const Rotation3D &center,
303  double distance);
304 
305 
306 //! Cover the space of rotations evenly
307 /** If you care about the distance between samples instead of the number
308  of samples, the "surface area" of the set of rotations is pi^2. If
309  you allocate each sample a volume of 4/3 pi d^3 (to space them d apart),
310  Then you want 3/4 pi/d^3 points.
311 
312  Creates at least num_points rotations.
313 */
314 IMPALGEBRAEXPORT Rotation3Ds
315 get_uniform_cover_rotations_3d(unsigned int num_points);
316 
317 //! Generates a nondegenerate set of Euler angles with a delta resolution
318 /**
319 \param[in] delta sample every delta angles in radians.
320  */
321 IMPALGEBRAEXPORT
323 
324 //! Compute a rotatation from an unnormalized quaternion
325 /** \relatesalso Rotation3D */
327  VectorD<4> uv= v.get_unit_vector();
328  return Rotation3D(uv[0], uv[1], uv[2], uv[3]);
329 }
330 
331 
332 /** \relatesalso Rotation3D
333  */
334 inline Rotation3D compose(const Rotation3D &a, const Rotation3D &b) {
335  return Rotation3D(a.v_[0]*b.v_[0] - a.v_[1]*b.v_[1]
336  - a.v_[2]*b.v_[2] - a.v_[3]*b.v_[3],
337  a.v_[0]*b.v_[1] + a.v_[1]*b.v_[0]
338  + a.v_[2]*b.v_[3] - a.v_[3]*b.v_[2],
339  a.v_[0]*b.v_[2] - a.v_[1]*b.v_[3]
340  + a.v_[2]*b.v_[0] + a.v_[3]*b.v_[1],
341  a.v_[0]*b.v_[3] + a.v_[1]*b.v_[2]
342  - a.v_[2]*b.v_[1] + a.v_[3]*b.v_[0]);
343 }
344 
345 /** \name Euler Angles
346  There are many conventions for how to define Euler angles, based on choices
347  of which of the x,y,z axis to use in what order and whether the rotation
348  axis is in the body frame (and hence affected by previous rotations) or in
349  in a fixed frame. See
350  http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
351  for a general description.
352 
353  - All Euler angles are specified in radians.
354  - The names are all \c rotation_from_{fixed/body}_abc() where abc is the
355  ordering of x,y,z.
356  @{
357 */
358 
359 //! Initialize a rotation in x-y-z order from three angles
360 /** \param[in] xr Rotation around the X axis in radians
361  \param[in] yr Rotation around the Y axis in radians
362  \param[in] zr Rotation around the Z axis in radians
363  \note The three rotations are represented in the original (fixed)
364  coordinate frame.
365  \relatesalso Rotation3D
366  \relatesalso FixedXYZ
367 */
368 IMPALGEBRAEXPORT Rotation3D get_rotation_from_fixed_xyz(double xr,
369  double yr,
370  double zr);
371 
372 //! Initialize a rotation from euler angles
373 /**
374  \param[in] phi Rotation around the Z axis in radians
375  \param[in] theta Rotation around the X axis in radians
376  \param[in] psi Rotation around the Z axis in radians
377  \note The first rotation is by an angle phi about the z-axis.
378  The second rotation is by an angle theta in [0,pi] about the
379  former x-axis , and the third rotation is by an angle psi
380  about the former z-axis.
381  \relatesalso Rotation3D
382 */
383 IMPALGEBRAEXPORT Rotation3D get_rotation_from_fixed_zxz(double phi,
384  double theta,
385  double psi);
386 
387 //! Generate a rotation object from Euler Angles
388 /** \note The first rotation is by an angle about the z-axis.
389  The second rotation is by an angle about the new y-axis.
390  The third rotation is by an angle about the new z-axis.
391  \param[in] Rot First Euler angle (radians) defining the rotation (Z axis)
392  \param[in] Tilt Second Euler angle (radians) defining the rotation (Y axis)
393  \param[in] Psi Third Euler angle (radians) defining the rotation (Z axis)
394  \relatesalso Rotation3D
395  \relatesalso FixedZYZ
396 */
397 IMPALGEBRAEXPORT Rotation3D get_rotation_from_fixed_zyz(double Rot,
398  double Tilt,
399  double Psi);
400 
401 
402 
403 //! A simple class for returning ZYZ Euler angles
404 /**
405 */
406 class FixedZYZ: public GeometricPrimitiveD<3> {
407  double v_[3];
408 public:
409  FixedZYZ(){}
410  FixedZYZ(double rot, double tilt, double psi)
411  {v_[0]=rot; v_[1]= tilt; v_[2]=psi;}
412  double get_rot() const {
413  return v_[0];
414  }
415  double get_tilt() const {
416  return v_[1];
417  }
418  double get_psi() const {
419  return v_[2];
420  }
422  {out << v_[0] << " " << v_[1]
423  << " " << v_[2];});
424 };
425 
427 
428 
429 
430 
431 //! A simple class for returning ZXZ Euler angles
432 class FixedZXZ: public GeometricPrimitiveD<3> {
433  double v_[3];
434 public:
435  FixedZXZ(){}
436  FixedZXZ(double psi, double theta, double phi)
437  {v_[0]=psi; v_[1]= theta; v_[2]=phi;}
438  double get_psi() const {
439  return v_[0];
440  }
441  double get_theta() const {
442  return v_[1];
443  }
444  double get_phi() const {
445  return v_[2];
446  }
448  {out << v_[0] << " " << v_[1]
449  << " " << v_[2];});
450 };
451 
453 
454 
455 //! A simple class for returning XYZ Euler angles
456 class FixedXYZ: public GeometricPrimitiveD<3> {
457  double v_[3];
458 public:
459  FixedXYZ(){}
460  FixedXYZ(double x, double y, double z)
461  {v_[0]=x; v_[1]= y; v_[2]=z;}
462  double get_x() const {
463  return v_[0];
464  }
465  double get_y() const {
466  return v_[1];
467  }
468  double get_z() const {
469  return v_[2];
470  }
472  out << v_[0] << " " << v_[1] << " " << v_[2];
473  });
474 };
475 
477 
478 //! The inverse of rotation_from_fixed_zyz()
479 /**
480  \see rotation_from_fixed_zyz()
481  \relatesalso Rotation3D
482  \relatesalso FixedZYZ
483 */
484 IMPALGEBRAEXPORT FixedZYZ get_fixed_zyz_from_rotation(const Rotation3D &r);
485 
486 
487 //! The inverse of rotation_from_fixed_zyz()
488 /**
489  \see rotation_from_fixed_zxz()
490  \relatesalso Rotation3D
491  \relatesalso FixedZXZ
492 */
493 IMPALGEBRAEXPORT FixedZXZ get_fixed_zxz_from_rotation(const Rotation3D &r);
494 
495 //! The inverse of rotation_from_fixed_xyz()
496 /**
497  \see rotation_from_fixed_xyz()
498  \relatesalso Rotation3D
499  \relatesalso FixesXYZ
500 */
501 IMPALGEBRAEXPORT FixedXYZ get_fixed_xyz_from_rotation(const Rotation3D &r);
502 
503 /** @}*/
504 
505 
506 //! Interpolate between two rotations
507 /** It f ==0, return b, if f==1 return a.
508  \relatesalso Rotation3D*/
510  const Rotation3D &b,
511  double f) {
512  VectorD<4> bq= b.get_quaternion(), aq= a.get_quaternion();
513  if (bq*aq < 0) bq=-bq;
514  return f*aq+(1-f)*bq;
515 }
516 
517 /** Return the rotation which takes the native x and y axes to the
518  given x and y axes.
519  The two axis must be perpendicular unit vectors.
520 */
521 IMPALGEBRAEXPORT
522 Rotation3D get_rotation_from_x_y_axes(const Vector3D &x,
523  const Vector3D &y);
524 
525 //! Decompose a Rotation3D object into a rotation around an axis
526 /**
527  \note http://en.wikipedia.org/wiki/Rotation_matrix
528  \note www.euclideanspace.com/maths/geometry/rotations/conversions/
529  angleToQuaternion/index.htm
530  \relatesalso Rotation3D
531 
532  @return axis direction and rotation about the axis in Radians
533 */
534 IMPALGEBRAEXPORT
535 std::pair<Vector3D,double> get_axis_and_angle(const Rotation3D &rot);
536 
537 
538 
539 typedef std::pair<Vector3D,double> AxisAnglePair;
540 #ifndef IMP_DOXYGEN
541 typedef base::Vector<AxisAnglePair> AxisAnglePairs;
542 #endif
543 
544 IMPALGEBRA_END_NAMESPACE
545 #endif /* IMPALGEBRA_ROTATION_3D_H */