8 #ifndef IMPALGEBRA_ROTATION_3D_H
9 #define IMPALGEBRA_ROTATION_3D_H
11 #include <IMP/algebra/algebra_config.h>
16 #include <cereal/access.hpp>
17 #include <Eigen/Dense>
24 IMPALGEBRA_BEGIN_NAMESPACE
27 typedef std::pair<Vector3D, Rotation3DAdjoint> RotatedVector3DAdjoint;
28 typedef std::pair<Rotation3DAdjoint, Rotation3DAdjoint>
29 ComposeRotation3DAdjoint;
31 #if !defined(IMP_DOXYGEN) && !defined(SWIG)
33 Rotation3D
compose(
const Rotation3D &a,
const Rotation3D &b);
54 mutable bool has_cache_;
57 friend class cereal::access;
59 template<
class Archive>
60 void save(Archive &ar)
const {
64 template<
class Archive>
65 void load(Archive &ar) {
72 void fill_cache()
const {
74 "Attempting to apply uninitialized rotation");
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];
87 Vector3D(v0s + v1s - v2s - v3s, 2 * (v12 - v03), 2 * (v13 + v02));
89 Vector3D(2 * (v12 + v03), v0s - v1s + v2s - v3s, 2 * (v23 - v01));
91 Vector3D(2 * (v13 - v02), 2 * (v23 + v01), v0s - v1s - v2s + v3s);
96 has_cache_ = rot.has_cache_;
98 matrix_[0] = rot.matrix_[0];
99 matrix_[1] = rot.matrix_[1];
100 matrix_[2] = rot.matrix_[2];
120 bool assume_normalized=
false)
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());
147 "Attempting to access uninitialized rotation");
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]) *
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]) *
163 double get_rotated_one_coordinate_no_cache(
const Vector3D &o,
164 unsigned int coord)
const {
166 "Attempting to apply uninitialized rotation");
169 return (v_[0] * v_[0] + v_[1] * v_[1] - v_[2] * v_[2] - v_[3] * v_[3]) *
171 2 * (v_[1] * v_[2] - v_[0] * v_[3]) * o[1] +
172 2 * (v_[1] * v_[3] + v_[0] * v_[2]) * o[2];
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]) *
178 2 * (v_[2] * v_[3] - v_[0] * v_[1]) * o[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]) *
188 IMP_THROW(
"Out of range coordinate " << coord, IndexException);
194 if (!has_cache_) fill_cache();
195 return Vector3D(o * matrix_[0], o * matrix_[1], o * matrix_[2]);
205 Vector3D *Dv, Rotation3DAdjoint *Dr)
const;
213 RotatedVector3DAdjoint
218 unsigned int coord)
const {
219 if (!has_cache_) fill_cache();
220 return o * matrix_[coord];
225 Vector3D get_rotation_matrix_row(
int i)
const {
227 if (!has_cache_) fill_cache();
231 { out << v_[0] <<
" " << v_[1] <<
" " << v_[2] <<
" " << v_[3]; });
236 "Attempting to invert uninitialized rotation");
237 Rotation3D ret(v_[0], -v_[1], -v_[2], -v_[3]);
247 "Attempting to access uninitialized rotation");
254 "Attempting to compose uninitialized rotation");
261 "Attempting to compose uninitialized rotation");
286 bool wrt_unnorm =
false)
const;
288 IMPALGEBRA_DEPRECATED_METHOD_DECL(2.12)
290 bool wrt_unnorm = true) const;
304 Eigen::MatrixXd get_jacobian_of_rotated(
305 const Eigen::Vector3d &v,
bool wrt_unnorm = false) const;
307 IMPALGEBRA_DEPRECATED_METHOD_DECL(2.12)
308 Eigen::MatrixXd get_gradient(
309 const Eigen::Vector3d &v,
bool wrt_unnorm = true) const;
315 bool get_is_valid()
const {
316 return v_.get_squared_magnitude() > 0;
340 IMPALGEBRA_DEPRECATED_FUNCTION_DECL(2.12)
341 IMPALGEBRAEXPORT Eigen::MatrixXd
342 get_gradient_of_composed_with_respect_to_first(
363 IMPALGEBRA_DEPRECATED_FUNCTION_DECL(2.12)
364 IMPALGEBRAEXPORT Eigen::MatrixXd
365 get_gradient_of_composed_with_respect_to_second(
390 double ans = std::min(dot, odot);
392 static const double s2 = std::sqrt(2.0);
393 double ret = ans / s2;
394 return std::max(std::min(ret, 1.0), 0.0);
413 "expected normalized vector as axis of rotation");
414 double s = std::sin(angle / 2);
416 a = std::cos(angle / 2);
417 b = axis_norm[0] * s;
418 c = axis_norm[1] * s;
419 d = axis_norm[2] * s;
439 Vector3D axis_norm = axis.get_unit_vector();
446 IMPALGEBRAEXPORT Rotation3D
453 IMPALGEBRAEXPORT Rotation3D
455 double m11,
double m12,
double m20,
double m21,
480 IMPALGEBRAEXPORT Rotation3D
505 return Rotation3D(uv[0], uv[1], uv[2], uv[3]);
511 return Rotation3D(a.v_[0] * b.v_[0] - a.v_[1] * b.v_[1] - a.v_[2] * b.v_[2] -
513 a.v_[0] * b.v_[1] + a.v_[1] * b.v_[0] + a.v_[2] * b.v_[3] -
515 a.v_[0] * b.v_[2] - a.v_[1] * b.v_[3] + a.v_[2] * b.v_[0] +
517 a.v_[0] * b.v_[3] + a.v_[1] * b.v_[2] - a.v_[2] * b.v_[1] +
526 IMPALGEBRAEXPORT
void
528 Rotation3DAdjoint *DA, Rotation3DAdjoint *DB);
535 IMPALGEBRAEXPORT ComposeRotation3DAdjoint
536 compose_adjoint(
const Rotation3D &A,
const Rotation3D &B,
const Rotation3DAdjoint &DC);
562 IMPALGEBRAEXPORT Rotation3D
576 IMPALGEBRAEXPORT Rotation3D
588 IMPALGEBRAEXPORT Rotation3D
595 friend class cereal::access;
597 template<
class Archive>
void serialize(Archive &ar) {
598 ar(v_[0], v_[1], v_[2]);
603 FixedXYZ(
double x,
double y,
double z) {
608 double get_x()
const {
return v_[0]; }
609 double get_y()
const {
return v_[1]; }
610 double get_z()
const {
return v_[2]; }
612 { out << v_[0] <<
" " << v_[1] <<
" " << v_[2]; });
633 if (bq * aq < 0) bq = -bq;
641 IMPALGEBRAEXPORT Rotation3D
655 const Rotation3D &rot);
657 typedef std::pair<Vector3D, double> AxisAnglePair;
662 IMPALGEBRA_END_NAMESPACE
Vector3D get_rotated(const Vector3D &o) const
Rotate a vector around the origin.
Rotation3D get_rotation_about_normalized_axis(const Vector3D &axis_norm, double angle)
Rotation3D(const Rotation3D &rot)
Rotation3D copy constructor.
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.
#define IMP_USAGE_CHECK_FLOAT_EQUAL(expra, exprb, message)
Rotation2D compose(const Rotation2D &a, const Rotation2D &b)
Compose two rotations a and b.
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 ¢er, 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.
A simple class for returning XYZ Euler angles.
Rotation3D(const VectorD< 4 > &v, bool assume_normalized=false)
Vector3D operator*(const Vector3D &v) const
Rotate a vector around the origin.
Rotation3D compose(const Rotation3D &a, const Rotation3D &b)
double get_rotated_one_coordinate(const Vector3D &o, unsigned int coord) const
Get only the requested rotation coordinate of the vector.
VT get_unit_vector(VT vt)
Returns a unit vector pointing at the same direction as this vector.
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.
const Vector4D & get_quaternion() const
Return the quaternion so that it can be stored.
Rotation3D get_rotation_about_axis(const Vector3D &axis, double angle)
Generate a Rotation3D object from a rotation around an axis.
#define IMP_VALUES(Name, PluralName)
Define the type for storing sets of values.
Base class for geometric types.
Functions to deal with very common math operations.
A Cartesian vector in D-dimensions.
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.
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.
Rotation3D get_interpolated(const Rotation3D &a, const Rotation3D &b, double f)
Interpolate between two rotations.
#define IMP_NO_SWIG(x)
Hide the line when SWIG is compiled or parses it.
Rotation3D get_rotation_from_vector4d(const VectorD< 4 > &v)
Compute a rotation from an unnormalized quaternion.
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.
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.
Rotation3D(double a, double b, double c, double d)
Create a rotation from a quaternion.
Rotation3D get_identity_rotation_3d()
Return a rotation that does not do anything.
ComposeRotation3DAdjoint compose_adjoint(const Rotation3D &A, const Rotation3D &B, const Rotation3DAdjoint &DC)
Get adjoint of inputs to compose from adjoint of output.
#define IMP_USAGE_CHECK(expr, message)
A runtime test for incorrect usage of a class or method.
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