IMP logo
IMP Reference Guide  2.20.1
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-2022 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 #include <cereal/access.hpp>
17 #include <Eigen/Dense>
18 
19 #include <IMP/log.h>
20 #include <cmath>
21 #include <iostream>
22 #include <algorithm>
23 
24 IMPALGEBRA_BEGIN_NAMESPACE
25 
26 typedef Vector4D Rotation3DAdjoint;
27 typedef std::pair<Vector3D, Rotation3DAdjoint> RotatedVector3DAdjoint;
28 typedef std::pair<Rotation3DAdjoint, Rotation3DAdjoint>
29  ComposeRotation3DAdjoint;
30 
31 #if !defined(IMP_DOXYGEN) && !defined(SWIG)
32 class Rotation3D;
33 Rotation3D compose(const Rotation3D &a, const Rotation3D &b);
34 #endif
35 
36 //! 3D rotation class.
37 /** Rotations are currently represented using quaternions and a cached
38  copy of the rotation matrix. The quaternion allows for fast and
39  stable composition and the cached rotation matrix means that
40  rotations are performed quickly. See
41  https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation for
42  a comparison of different implementations of rotations.
43 
44  Currently the rotation can be initialized from either:
45  - XYZ Euler angles
46  - Rotation Matrix
47  - Quaternion
48  - angle/axis representation
49 
50  \geometry
51 */
52 class IMPALGEBRAEXPORT Rotation3D : public GeometricPrimitiveD<3> {
53  VectorD<4> v_;
54  mutable bool has_cache_;
55  mutable Vector3D matrix_[3];
56 
57  friend class cereal::access;
58 
59  template<class Archive>
60  void save(Archive &ar) const {
61  ar(v_);
62  }
63 
64  template<class Archive>
65  void load(Archive &ar) {
66  ar(v_);
67  has_cache_ = false;
68  }
69 
70  IMP_NO_SWIG(friend Rotation3D compose(const Rotation3D &a,
71  const Rotation3D &b));
72  void fill_cache() const {
73  IMP_USAGE_CHECK(get_is_valid(),
74  "Attempting to apply uninitialized rotation");
75  has_cache_ = true;
76  double v0s = get_squared(v_[0]);
77  double v1s = get_squared(v_[1]);
78  double v2s = get_squared(v_[2]);
79  double v3s = get_squared(v_[3]);
80  double v12 = v_[1] * v_[2];
81  double v01 = v_[0] * v_[1];
82  double v02 = v_[0] * v_[2];
83  double v23 = v_[2] * v_[3];
84  double v03 = v_[0] * v_[3];
85  double v13 = v_[1] * v_[3];
86  matrix_[0] =
87  Vector3D(v0s + v1s - v2s - v3s, 2 * (v12 - v03), 2 * (v13 + v02));
88  matrix_[1] =
89  Vector3D(2 * (v12 + v03), v0s - v1s + v2s - v3s, 2 * (v23 - v01));
90  matrix_[2] =
91  Vector3D(2 * (v13 - v02), 2 * (v23 + v01), v0s - v1s - v2s + v3s);
92  }
93 
94  // When copying preserve the cached rotation matrix, if present
95  void copy_cache(const Rotation3D &rot) {
96  has_cache_ = rot.has_cache_;
97  if (has_cache_) {
98  matrix_[0] = rot.matrix_[0];
99  matrix_[1] = rot.matrix_[1];
100  matrix_[2] = rot.matrix_[2];
101  }
102  }
103 
104  public:
105  //! Rotation3D copy constructor
106  Rotation3D(const Rotation3D &rot) : v_(rot.v_) {
107  copy_cache(rot);
108  }
109 
110  Rotation3D &operator=(const Rotation3D &rot) {
111  v_ = rot.v_;
112  copy_cache(rot);
113  return *this;
114  }
115 
116  //! Create a rotation from a vector of 4 quaternion coefficients.
117  //! @note: use assume_normalized with care - inputting an unnormalized
118  //! vector would result in unexpected results if it is true
119  explicit Rotation3D(const VectorD<4> &v,
120  bool assume_normalized=false)
121  : v_(assume_normalized ? v : v.get_unit_vector()),
122  has_cache_(false) {}
123 
124  //! Create an invalid rotation
125  Rotation3D() : v_(0, 0, 0, 0), has_cache_(false) {}
126  //! Create a rotation from a quaternion
127  /** \throw ValueException if the rotation is not a unit vector.
128  */
129  Rotation3D(double a, double b, double c, double d)
130  : v_(a, b, c, d), has_cache_(false) {
132  v_.get_squared_magnitude(), 1.0,
133  "Attempting to construct a rotation from a "
134  << " non-quaternion value. The coefficient vector"
135  << " must have a length of 1. Got: " << a << " " << b << " " << c
136  << " " << d << " gives " << v_.get_squared_magnitude());
137  if (a < 0) {
138  // make them canonical
139  v_ = -v_;
140  }
141  }
142  ~Rotation3D(){}
143 
144 #ifndef IMP_DOXYGEN
145  Vector3D get_rotated_no_cache(const Vector3D &o) const {
146  IMP_USAGE_CHECK(get_is_valid(),
147  "Attempting to access uninitialized rotation");
148  return Vector3D(
149  (v_[0] * v_[0] + v_[1] * v_[1] - v_[2] * v_[2] - v_[3] * v_[3]) * o[0] +
150  2 * (v_[1] * v_[2] - v_[0] * v_[3]) * o[1] +
151  2 * (v_[1] * v_[3] + v_[0] * v_[2]) * o[2],
152  2 * (v_[1] * v_[2] + v_[0] * v_[3]) * o[0] +
153  (v_[0] * v_[0] - v_[1] * v_[1] + v_[2] * v_[2] - v_[3] * v_[3]) *
154  o[1] +
155  2 * (v_[2] * v_[3] - v_[0] * v_[1]) * o[2],
156  2 * (v_[1] * v_[3] - v_[0] * v_[2]) * o[0] +
157  2 * (v_[2] * v_[3] + v_[0] * v_[1]) * o[1] +
158  (v_[0] * v_[0] - v_[1] * v_[1] - v_[2] * v_[2] + v_[3] * v_[3]) *
159  o[2]);
160  }
161 
162  //! Get only the requested rotation coordinate of the vector
163  double get_rotated_one_coordinate_no_cache(const Vector3D &o,
164  unsigned int coord) const {
165  IMP_USAGE_CHECK(get_is_valid(),
166  "Attempting to apply uninitialized rotation");
167  switch (coord) {
168  case 0:
169  return (v_[0] * v_[0] + v_[1] * v_[1] - v_[2] * v_[2] - v_[3] * v_[3]) *
170  o[0] +
171  2 * (v_[1] * v_[2] - v_[0] * v_[3]) * o[1] +
172  2 * (v_[1] * v_[3] + v_[0] * v_[2]) * o[2];
173  break;
174  case 1:
175  return 2 * (v_[1] * v_[2] + v_[0] * v_[3]) * o[0] +
176  (v_[0] * v_[0] - v_[1] * v_[1] + v_[2] * v_[2] - v_[3] * v_[3]) *
177  o[1] +
178  2 * (v_[2] * v_[3] - v_[0] * v_[1]) * o[2];
179 
180  break;
181  case 2:
182  return 2 * (v_[1] * v_[3] - v_[0] * v_[2]) * o[0] +
183  2 * (v_[2] * v_[3] + v_[0] * v_[1]) * o[1] +
184  (v_[0] * v_[0] - v_[1] * v_[1] - v_[2] * v_[2] + v_[3] * v_[3]) *
185  o[2];
186  break;
187  default:
188  IMP_THROW("Out of range coordinate " << coord, IndexException);
189  }
190  }
191 #endif
192  //! Rotate a vector around the origin
193  Vector3D get_rotated(const Vector3D &o) const {
194  if (!has_cache_) fill_cache();
195  return Vector3D(o * matrix_[0], o * matrix_[1], o * matrix_[2]);
196  }
197 
198 #ifndef SWIG
199  //! Get adjoint of inputs to `get_rotated` from adjoint of output
200  /** Compute the adjoint (reverse-mode sensitivity) of input vector
201  to `get_rotated` and this rotation from the adjoint of the
202  output vector.
203  */
204  void get_rotated_adjoint(const Vector3D &v, const Vector3D &Dw,
205  Vector3D *Dv, Rotation3DAdjoint *Dr) const;
206 #endif
207 
208  //! Get adjoint of inputs to `get_rotated` from adjoint of output
209  /** Compute the adjoint (reverse-mode sensitivity) of input vector
210  to `get_rotated` and this rotation from the adjoint of the
211  output vector.
212  */
213  RotatedVector3DAdjoint
214  get_rotated_adjoint(const Vector3D &v, const Vector3D &Dw) const;
215 
216  //! Get only the requested rotation coordinate of the vector
218  unsigned int coord) const {
219  if (!has_cache_) fill_cache();
220  return o * matrix_[coord];
221  }
222 
223  //! Rotate a vector around the origin
224  Vector3D operator*(const Vector3D &v) const { return get_rotated(v); }
225  Vector3D get_rotation_matrix_row(int i) const {
226  IMP_USAGE_CHECK((i >= 0) && (i <= 2), "row index out of range");
227  if (!has_cache_) fill_cache();
228  return matrix_[i];
229  }
230  IMP_SHOWABLE_INLINE(Rotation3D,
231  { out << v_[0] << " " << v_[1] << " " << v_[2] << " " << v_[3]; });
232 
233  //! Return the rotation which undoes this rotation.
234  inline Rotation3D get_inverse() const {
235  IMP_USAGE_CHECK(get_is_valid(),
236  "Attempting to invert uninitialized rotation");
237  Rotation3D ret(v_[0], -v_[1], -v_[2], -v_[3]);
238  return ret;
239  }
240 
241  //! Return the quaternion so that it can be stored
242  /** Note that there is no guarantee on which of the two
243  equivalent quaternions is returned.
244  */
245  const Vector4D &get_quaternion() const {
246  IMP_USAGE_CHECK(get_is_valid(),
247  "Attempting to access uninitialized rotation");
248  return v_;
249  }
250 
251  //! Multiply two rotations
252  Rotation3D operator*(const Rotation3D &q) const {
253  IMP_USAGE_CHECK(get_is_valid(),
254  "Attempting to compose uninitialized rotation");
255  return compose(*this, q);
256  }
257 
258  //! Compute the rotation which when composed with r gives this
259  Rotation3D operator/(const Rotation3D &r) const {
260  IMP_USAGE_CHECK(get_is_valid(),
261  "Attempting to compose uninitialized rotation");
262  return compose(*this, r.get_inverse());
263  }
264 
265  const Rotation3D &operator/=(const Rotation3D &r) {
266  *this = *this / r;
267  return *this;
268  }
269 
270  //! Return the gradient of rotated vector wrt the ith quaternion element.
271  /** Given the rotation \f$x = R(q) v\f$, where \f$v\f$ is a vector,
272  \f$q=(q_0,q_1,q_2,q_3)\f$ is the quaternion of the rotation with
273  elements \f$q_i\f$, and \f$R(q)\f$ is the corresponding rotation matrix,
274  the function returns the gradient \f$\nabla_{q_i} x\f$.
275 
276  This function just returns a single column from `get_jacobian()`, so it is
277  more efficient to call that function if all columns are needed.
278 
279  \param[in] v vector to be rotated by rotation \f$R(q)\f$
280  \param[in] wrt_unnorm Gradient is computed wrt unnormalized quaternion.
281  Rotation includes a normalization operation, and
282  the gradient is projected to the tangent space at
283  \f$q\f$.
284  */
285  Vector3D get_gradient_of_rotated(const Vector3D &v, unsigned int i,
286  bool wrt_unnorm = false) const;
287 
288  //! Return the Jacobian of rotated vector wrt the quaternion.
289  /** Given the rotation \f$x = R(q) v\f$, where \f$v\f$ is a vector,
290  \f$q\f$ is the quaternion of the rotation, and \f$R(q)\f$ is the
291  corresponding rotation matrix, the function returns the 3x4 matrix
292  \f$J\f$ with elements \f$J_{ij}=\frac{\partial x_i}{\partial q_j}\f$.
293 
294  \param[in] v vector to be rotated by rotation \f$R(q)\f$
295  \param[in] wrt_unnorm Jacobian is computed wrt unnormalized quaternion.
296  Rotation includes a normalization operation, and
297  the columns are projected to the tangent space at
298  \f$q\f$.
299  */
300  Eigen::MatrixXd get_jacobian_of_rotated(
301  const Eigen::Vector3d &v, bool wrt_unnorm = false) const;
302 
303  /** Return true is the rotation is valid, false if
304  invalid or not initialized (e.g., only initialized by
305  the empty constructor)
306  */
307  bool get_is_valid() const {
308  return v_.get_squared_magnitude() > 0; // TODO: add that magnitude ~ 1?
309  }
310 };
311 
313 
314 
315 //! Get gradient of quaternion product with respect to first quaternion.
316 /** Given the rotation \f$R(p)\f$ followed by \f$R(q)\f$, where \f$p\f$ and
317  \f$q\f$ are quaternions, the quaternion of the composed rotation
318  \f$R(s)=R(q) R(p)\f$ can be expressed through the Hamilton product of the
319  two quaternions \f$s(q,p) = q p\f$. This function returns the matrix
320  \f$J\f$ with elements \f$J_{ij}=\frac{\partial s_i}{\partial q_j}\f$.
321 
322  \param[in] q rotation corresponding to first quaternion
323  \param[in] p rotation corresponding to second quaternion
324  \param[in] wrt_unnorm Jacobian is computed wrt unnormalized quaternion.
325  Rotation includes a normalization operation, and
326  the columns are projected to the tangent space at
327  \f$q\f$.
328  */
329 IMPALGEBRAEXPORT Eigen::MatrixXd get_jacobian_of_composed_wrt_first(
330  const Rotation3D &q, const Rotation3D &p, bool wrt_unnorm = false);
331 
332 //! Get Jacobian of quaternion product with respect to second quaternion.
333 /** Given the rotation \f$R(p)\f$ followed by \f$R(q)\f$, where \f$p\f$ and
334  \f$q\f$ are quaternions, the quaternion of the composed rotation
335  \f$R(s)=R(q) R(p)\f$ can be expressed through the Hamilton product of the
336  two quaternions \f$s(q,p) = q p\f$. This function returns the matrix
337  \f$J\f$ with elements \f$J_{ij}=\frac{\partial s_i}{\partial p_j}\f$.
338 
339  \param[in] q rotation corresponding to first quaternion
340  \param[in] p rotation corresponding to second quaternion
341  \param[in] wrt_unnorm Jacobian is computed wrt unnormalized quaternion.
342  Rotation includes a normalization operation, and
343  the columns are projected to the tangent space at
344  \f$p\f$.
345  */
346 IMPALGEBRAEXPORT Eigen::MatrixXd get_jacobian_of_composed_wrt_second(
347  const Rotation3D &q, const Rotation3D &p, bool wrt_unnorm = false);
348 
349 //! Return a rotation that does not do anything
350 /** \see Rotation3D */
351 inline Rotation3D get_identity_rotation_3d() { return Rotation3D(1, 0, 0, 0); }
352 
353 //! Return a distance between the two rotations
354 /** The distance runs between 0 and 1. More precisely,
355  the distance returned is distance between the two
356  quaternion vectors properly normalized, divided
357  by sqrt(2).
358 
359  A vector with distance d from the unit vector
360  represents a rotation of
361 
362  \f$ \theta= \cos^{-1}\left(1-\sqrt{2}d\right)\f$
363  \see Rotation3D
364 */
365 inline double get_distance(const Rotation3D &r0, const Rotation3D &r1) {
366  double dot =
367  (r0.get_quaternion() - r1.get_quaternion()).get_squared_magnitude();
368  double odot =
369  (r0.get_quaternion() + r1.get_quaternion()).get_squared_magnitude();
370  double ans = std::min(dot, odot);
371  // TODO: barak - added static for efficiency
372  static const double s2 = std::sqrt(2.0);
373  double ret = ans / s2;
374  return std::max(std::min(ret, 1.0), 0.0);
375 }
376 
377 //! Generate a Rotation3D object from a rotation around an axis
378 //! that is assumed to be normalized
379 /**
380  \param[in] axis_norm the normalized rotation axis passing through (0,0,0)
381  \param[in] angle the rotation angle in radians in the
382  clockwise direction
383  \note https://en.wikipedia.org/wiki/Rotation_matrix
384  \note www.euclideanspace.com/maths/geometry/rotations/conversions/
385  angleToQuaternion/index.htm
386  \see Rotation3D
387 */
388 inline Rotation3D
390 (const Vector3D &axis_norm, double angle)
391 {
392  IMP_USAGE_CHECK(axis_norm.get_magnitude() - 1.0 < 1e-6,
393  "expected normalized vector as axis of rotation");
394  double s = std::sin(angle / 2);
395  double a, b, c, d;
396  a = std::cos(angle / 2);
397  b = axis_norm[0] * s;
398  c = axis_norm[1] * s;
399  d = axis_norm[2] * s;
400  return Rotation3D(a, b, c, d);
401 }
402 
403 
404 //! Generate a Rotation3D object from a rotation around an axis
405 /**
406  \param[in] axis the rotation axis passes through (0,0,0)
407  \param[in] angle the rotation angle in radians in the
408  clockwise direction
409  \note https://en.wikipedia.org/wiki/Rotation_matrix
410  \note www.euclideanspace.com/maths/geometry/rotations/conversions/
411  angleToQuaternion/index.htm
412  \see Rotation3D
413 */
414 inline Rotation3D
416 (const Vector3D &axis, double angle)
417 {
418  // normalize the vector
419  Vector3D axis_norm = axis.get_unit_vector();
420  return get_rotation_about_normalized_axis(axis_norm, angle);
421 }
422 
423 //! Create a rotation from the first vector to the second one.
424 /** \see Rotation3D
425  */
426 IMPALGEBRAEXPORT Rotation3D
428 
429 //! Generate a Rotation3D object from a rotation matrix
430 /**
431  \see Rotation3D
432 */
433 IMPALGEBRAEXPORT Rotation3D
434  get_rotation_from_matrix(double m00, double m01, double m02, double m10,
435  double m11, double m12, double m20, double m21,
436  double m22);
437 
438 //! Generate a Rotation3D object from a rotation matrix
439 /**
440  \see Rotation3D
441 */
442 IMPALGEBRAEXPORT Rotation3D get_rotation_from_matrix(Eigen::Matrix3d m);
443 
444 //! Pick a rotation at random from all possible rotations
445 /** \see Rotation3D */
446 IMPALGEBRAEXPORT Rotation3D get_random_rotation_3d();
447 
448 //! Pick a rotation at random near the provided one
449 /** This method generates a rotation that is within the provided
450  distance of center.
451  \param[in] center The center of the rotational volume
452  \param[in] distance See
453  get_distance(const Rotation3D&,const Rotation3D&)
454  for a full definition.
455 
456  \note The cost of this operation increases as distance goes to 0.
457 
458  \see Rotation3D
459 */
460 IMPALGEBRAEXPORT Rotation3D
461  get_random_rotation_3d(const Rotation3D &center, double distance);
462 
463 //! Cover the space of rotations evenly
464 /** If you care about the distance between samples instead of the number
465  of samples, the "surface area" of the set of rotations is pi^2. If
466  you allocate each sample a volume of 4/3 pi d^3 (to space them d apart),
467  Then you want 3/4 pi/d^3 points.
468 
469  Creates at least num_points rotations.
470 */
471 IMPALGEBRAEXPORT Rotation3Ds
472  get_uniform_cover_rotations_3d(unsigned int num_points);
473 
474 //! Generates a nondegenerate set of Euler angles with a delta resolution
475 /**
476 \param[in] delta sample every delta angles in radians.
477  */
479  double delta);
480 
481 //! Compute a rotation from an unnormalized quaternion
482 /** \see Rotation3D */
484  VectorD<4> uv = v.get_unit_vector();
485  return Rotation3D(uv[0], uv[1], uv[2], uv[3]);
486 }
487 
488 /** \see Rotation3D
489  */
490 inline Rotation3D compose(const Rotation3D &a, const Rotation3D &b) {
491  return Rotation3D(a.v_[0] * b.v_[0] - a.v_[1] * b.v_[1] - a.v_[2] * b.v_[2] -
492  a.v_[3] * b.v_[3],
493  a.v_[0] * b.v_[1] + a.v_[1] * b.v_[0] + a.v_[2] * b.v_[3] -
494  a.v_[3] * b.v_[2],
495  a.v_[0] * b.v_[2] - a.v_[1] * b.v_[3] + a.v_[2] * b.v_[0] +
496  a.v_[3] * b.v_[1],
497  a.v_[0] * b.v_[3] + a.v_[1] * b.v_[2] - a.v_[2] * b.v_[1] +
498  a.v_[3] * b.v_[0]);
499 }
500 
501 #ifndef SWIG
502 //! Get adjoint of inputs to `compose` from adjoint of output
503 /** Compute the adjoint (reverse-mode sensitivity) of input rotations
504  to `compose` from the adjoint of the output rotation.
505  */
506 IMPALGEBRAEXPORT void
507 compose_adjoint(const Rotation3D &A, const Rotation3D &B, Vector4D DC,
508  Rotation3DAdjoint *DA, Rotation3DAdjoint *DB);
509 #endif
510 
511 //! Get adjoint of inputs to `compose` from adjoint of output
512 /** Compute the adjoint (reverse-mode sensitivity) of input rotations
513  to `compose` from the adjoint of the output rotation.
514  */
515 IMPALGEBRAEXPORT ComposeRotation3DAdjoint
516 compose_adjoint(const Rotation3D &A, const Rotation3D &B, const Rotation3DAdjoint &DC);
517 
518 /** \name Euler Angles
519  There are many conventions for how to define Euler angles, based on choices
520  of which of the x,y,z axis to use in what order and whether the rotation
521  axis is in the body frame (and hence affected by previous rotations) or in
522  in a fixed frame. See
523  https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
524  for a general description.
525 
526  - All Euler angles are specified in radians.
527  - Only x-y-z order is currently supported.
528  - To convert Euler angles in a different order, one can compose a Rotation3D
529  from three rotations using get_rotation_about_axis function.
530  @{
531 */
532 
533 //! Initialize a rotation in x-y-z order from three angles
534 /** \param[in] xr Rotation around the X axis in radians
535  \param[in] yr Rotation around the Y axis in radians
536  \param[in] zr Rotation around the Z axis in radians
537  \note The three rotations are represented in the original (fixed)
538  coordinate frame.
539  \see Rotation3D
540  \see FixedXYZ
541 */
542 IMPALGEBRAEXPORT Rotation3D
543  get_rotation_from_fixed_xyz(double xr, double yr, double zr);
544 
545 //! Initialize a rotation from Euler angles
546 /**
547  \param[in] phi Rotation around the Z axis in radians
548  \param[in] theta Rotation around the X axis in radians
549  \param[in] psi Rotation around the Z axis in radians
550  \note The first rotation is by an angle phi about the z-axis.
551  The second rotation is by an angle theta in [0,pi] about the
552  former x-axis , and the third rotation is by an angle psi
553  about the former z-axis.
554  \see Rotation3D
555 */
556 IMPALGEBRAEXPORT Rotation3D
557  get_rotation_from_fixed_zxz(double phi, double theta, double psi);
558 
559 //! Generate a rotation object from Euler Angles
560 /** \note The first rotation is by an angle about the z-axis.
561  The second rotation is by an angle about the new y-axis.
562  The third rotation is by an angle about the new z-axis.
563  \param[in] Rot First Euler angle (radians) defining the rotation (Z axis)
564  \param[in] Tilt Second Euler angle (radians) defining the rotation (Y axis)
565  \param[in] Psi Third Euler angle (radians) defining the rotation (Z axis)
566  \see Rotation3D
567 */
568 IMPALGEBRAEXPORT Rotation3D
569  get_rotation_from_fixed_zyz(double Rot, double Tilt, double Psi);
570 
571 //! A simple class for returning XYZ Euler angles
572 class FixedXYZ : public GeometricPrimitiveD<3> {
573  double v_[3];
574 
575  friend class cereal::access;
576 
577  template<class Archive> void serialize(Archive &ar) {
578  ar(v_[0], v_[1], v_[2]);
579  }
580 
581  public:
582  FixedXYZ() {}
583  FixedXYZ(double x, double y, double z) {
584  v_[0] = x;
585  v_[1] = y;
586  v_[2] = z;
587  }
588  double get_x() const { return v_[0]; }
589  double get_y() const { return v_[1]; }
590  double get_z() const { return v_[2]; }
592  { out << v_[0] << " " << v_[1] << " " << v_[2]; });
593 };
594 
596 
597 //! The inverse of rotation_from_fixed_xyz()
598 /**
599  \see rotation_from_fixed_xyz()
600  \see Rotation3D
601  \see FixesXYZ
602 */
603 IMPALGEBRAEXPORT FixedXYZ get_fixed_xyz_from_rotation(const Rotation3D &r);
604 
605 /** @}*/
606 
607 //! Interpolate between two rotations
608 /** It f ==0, return b, if f==1 return a.
609  \see Rotation3D */
611  double f) {
612  VectorD<4> bq = b.get_quaternion(), aq = a.get_quaternion();
613  if (bq * aq < 0) bq = -bq;
614  return Rotation3D(f * aq + (1 - f) * bq);
615 }
616 
617 /** Return the rotation which takes the native x and y axes to the
618  given x and y axes.
619  The two axes must be perpendicular unit vectors.
620 */
621 IMPALGEBRAEXPORT Rotation3D
622  get_rotation_from_x_y_axes(const Vector3D &x, const Vector3D &y);
623 
624 //! Decompose a Rotation3D object into a rotation around an axis
625 /** For all identity rotations, returns the axis [1,0,0] and the angle 0.0.
626 
627  \note https://en.wikipedia.org/wiki/Rotation_matrix
628  \note www.euclideanspace.com/maths/geometry/rotations/conversions/
629  angleToQuaternion/index.htm
630  \see Rotation3D
631 
632  @return axis direction and rotation about the axis in Radians
633 */
634 IMPALGEBRAEXPORT std::pair<Vector3D, double> get_axis_and_angle(
635  const Rotation3D &rot);
636 
637 typedef std::pair<Vector3D, double> AxisAnglePair;
638 #ifndef IMP_DOXYGEN
639 typedef Vector<AxisAnglePair> AxisAnglePairs;
640 #endif
641 
642 IMPALGEBRA_END_NAMESPACE
643 #endif /* IMPALGEBRA_ROTATION_3D_H */
Vector3D get_rotated(const Vector3D &o) const
Rotate a vector around the origin.
Definition: Rotation3D.h:193
Rotation3D get_rotation_about_normalized_axis(const Vector3D &axis_norm, double angle)
Definition: Rotation3D.h:390
Rotation3D(const Rotation3D &rot)
Rotation3D copy constructor.
Definition: Rotation3D.h:106
Base class for geometric types.
#define IMP_SHOWABLE_INLINE(Name, how_to_show)
Declare the methods needed by an object that can be printed.
Rotation3D get_rotation_from_x_y_axes(const Vector3D &x, const Vector3D &y)
Rotation3D get_rotation_from_fixed_xyz(double xr, double yr, double zr)
Initialize a rotation in x-y-z order from three angles.
Rotation3D()
Create an invalid rotation.
Definition: Rotation3D.h:125
#define IMP_USAGE_CHECK_FLOAT_EQUAL(expra, exprb, message)
Definition: check_macros.h:178
Rotation2D compose(const Rotation2D &a, const Rotation2D &b)
Compose two rotations a and b.
Definition: Rotation2D.h:121
algebra::Rotation3Ds get_uniformly_sampled_rotations(double delta)
Generates a nondegenerate set of Euler angles with a delta resolution.
Rotation3D get_random_rotation_3d(const Rotation3D &center, double distance)
Pick a rotation at random near the provided one.
Rotation3D operator/(const Rotation3D &r) const
Compute the rotation which when composed with r gives this.
Definition: Rotation3D.h:259
A simple class for returning XYZ Euler angles.
Definition: Rotation3D.h:572
Rotation3D(const VectorD< 4 > &v, bool assume_normalized=false)
Definition: Rotation3D.h:119
Vector3D operator*(const Vector3D &v) const
Rotate a vector around the origin.
Definition: Rotation3D.h:224
Rotation3D compose(const Rotation3D &a, const Rotation3D &b)
Definition: Rotation3D.h:490
double get_rotated_one_coordinate(const Vector3D &o, unsigned int coord) const
Get only the requested rotation coordinate of the vector.
Definition: Rotation3D.h:217
VT get_unit_vector(VT vt)
Returns a unit vector pointing at the same direction as this vector.
Definition: VectorBaseD.h:251
Eigen::MatrixXd get_jacobian_of_composed_wrt_first(const Rotation3D &q, const Rotation3D &p, bool wrt_unnorm=false)
Get gradient of quaternion product with respect to first quaternion.
bool get_is_valid() const
Definition: Rotation3D.h:307
const Vector4D & get_quaternion() const
Return the quaternion so that it can be stored.
Definition: Rotation3D.h:245
VectorD< 4 > Vector4D
Definition: VectorD.h:429
Rotation3D get_rotation_about_axis(const Vector3D &axis, double angle)
Generate a Rotation3D object from a rotation around an axis.
Definition: Rotation3D.h:416
#define IMP_VALUES(Name, PluralName)
Define the type for storing sets of values.
Definition: value_macros.h:23
Base class for geometric types.
Functions to deal with very common math operations.
A Cartesian vector in D-dimensions.
Definition: VectorD.h:56
FixedXYZ get_fixed_xyz_from_rotation(const Rotation3D &r)
The inverse of rotation_from_fixed_xyz()
Rotation3D get_inverse() const
Return the rotation which undoes this rotation.
Definition: Rotation3D.h:234
Rotation3D get_rotation_from_fixed_zyz(double Rot, double Tilt, double Psi)
Generate a rotation object from Euler Angles.
std::pair< Vector3D, double > get_axis_and_angle(const Rotation3D &rot)
Decompose a Rotation3D object into a rotation around an axis.
double get_distance(const Rotation3D &r0, const Rotation3D &r1)
Return a distance between the two rotations.
Definition: Rotation3D.h:365
Rotation3D get_interpolated(const Rotation3D &a, const Rotation3D &b, double f)
Interpolate between two rotations.
Definition: Rotation3D.h:610
#define IMP_NO_SWIG(x)
Hide the line when SWIG is compiled or parses it.
Definition: swig_macros.h:18
Rotation3D get_rotation_from_vector4d(const VectorD< 4 > &v)
Compute a rotation from an unnormalized quaternion.
Definition: Rotation3D.h:483
Rotation3D get_rotation_taking_first_to_second(const Vector3D &v1, const Vector3D &v2)
Create a rotation from the first vector to the second one.
Rotation3Ds get_uniform_cover_rotations_3d(unsigned int num_points)
Cover the space of rotations evenly.
Various useful constants.
Rotation3D operator*(const Rotation3D &q) const
Multiply two rotations.
Definition: Rotation3D.h:252
3D rotation class.
Definition: Rotation3D.h:52
Eigen::MatrixXd get_jacobian_of_composed_wrt_second(const Rotation3D &q, const Rotation3D &p, bool wrt_unnorm=false)
Get Jacobian of quaternion product with respect to second quaternion.
#define IMP_THROW(message, exception_name)
Throw an exception with a message.
Definition: check_macros.h:50
Rotation3D(double a, double b, double c, double d)
Create a rotation from a quaternion.
Definition: Rotation3D.h:129
VectorD< 3 > Vector3D
Definition: VectorD.h:425
Rotation3D get_identity_rotation_3d()
Return a rotation that does not do anything.
Definition: Rotation3D.h:351
ComposeRotation3DAdjoint compose_adjoint(const Rotation3D &A, const Rotation3D &B, const Rotation3DAdjoint &DC)
Get adjoint of inputs to compose from adjoint of output.
Simple 3D vector class.
#define IMP_USAGE_CHECK(expr, message)
A runtime test for incorrect usage of a class or method.
Definition: check_macros.h:168
Logging and error reporting support.
Rotation3D get_rotation_from_fixed_zxz(double phi, double theta, double psi)
Initialize a rotation from Euler angles.
Rotation3D get_rotation_from_matrix(Eigen::Matrix3d m)
Generate a Rotation3D object from a rotation matrix.
IMP::Vector< Rotation3D > Rotation3Ds
Definition: Rotation3D.h:312