8 #ifndef IMPALGEBRA_ROTATION_3D_H
9 #define IMPALGEBRA_ROTATION_3D_H
11 #include <IMP/algebra/algebra_config.h>
16 #include <Eigen/Dense>
23 IMPALGEBRA_BEGIN_NAMESPACE
26 typedef std::pair<Vector3D, Rotation3DAdjoint> RotatedVector3DAdjoint;
27 typedef std::pair<Rotation3DAdjoint, Rotation3DAdjoint>
28 ComposeRotation3DAdjoint;
30 #if !defined(IMP_DOXYGEN) && !defined(SWIG)
32 Rotation3D
compose(
const Rotation3D &a,
const Rotation3D &b);
53 mutable bool has_cache_;
57 void fill_cache()
const {
59 "Attempting to apply uninitialized rotation");
61 double v0s = get_squared(v_[0]);
62 double v1s = get_squared(v_[1]);
63 double v2s = get_squared(v_[2]);
64 double v3s = get_squared(v_[3]);
65 double v12 = v_[1] * v_[2];
66 double v01 = v_[0] * v_[1];
67 double v02 = v_[0] * v_[2];
68 double v23 = v_[2] * v_[3];
69 double v03 = v_[0] * v_[3];
70 double v13 = v_[1] * v_[3];
72 Vector3D(v0s + v1s - v2s - v3s, 2 * (v12 - v03), 2 * (v13 + v02));
74 Vector3D(2 * (v12 + v03), v0s - v1s + v2s - v3s, 2 * (v23 - v01));
76 Vector3D(2 * (v13 - v02), 2 * (v23 + v01), v0s - v1s - v2s + v3s);
81 has_cache_ = rot.has_cache_;
83 matrix_[0] = rot.matrix_[0];
84 matrix_[1] = rot.matrix_[1];
85 matrix_[2] = rot.matrix_[2];
105 bool assume_normalized=
false)
115 : v_(a, b, c, d), has_cache_(false) {
117 v_.get_squared_magnitude(), 1.0,
118 "Attempting to construct a rotation from a "
119 <<
" non-quaternion value. The coefficient vector"
120 <<
" must have a length of 1. Got: " << a <<
" " << b <<
" " << c
121 <<
" " << d <<
" gives " << v_.get_squared_magnitude());
132 "Attempting to access uninitialized rotation");
134 (v_[0] * v_[0] + v_[1] * v_[1] - v_[2] * v_[2] - v_[3] * v_[3]) * o[0] +
135 2 * (v_[1] * v_[2] - v_[0] * v_[3]) * o[1] +
136 2 * (v_[1] * v_[3] + v_[0] * v_[2]) * o[2],
137 2 * (v_[1] * v_[2] + v_[0] * v_[3]) * o[0] +
138 (v_[0] * v_[0] - v_[1] * v_[1] + v_[2] * v_[2] - v_[3] * v_[3]) *
140 2 * (v_[2] * v_[3] - v_[0] * v_[1]) * o[2],
141 2 * (v_[1] * v_[3] - v_[0] * v_[2]) * o[0] +
142 2 * (v_[2] * v_[3] + v_[0] * v_[1]) * o[1] +
143 (v_[0] * v_[0] - v_[1] * v_[1] - v_[2] * v_[2] + v_[3] * v_[3]) *
148 double get_rotated_one_coordinate_no_cache(
const Vector3D &o,
149 unsigned int coord)
const {
151 "Attempting to apply uninitialized rotation");
154 return (v_[0] * v_[0] + v_[1] * v_[1] - v_[2] * v_[2] - v_[3] * v_[3]) *
156 2 * (v_[1] * v_[2] - v_[0] * v_[3]) * o[1] +
157 2 * (v_[1] * v_[3] + v_[0] * v_[2]) * o[2];
160 return 2 * (v_[1] * v_[2] + v_[0] * v_[3]) * o[0] +
161 (v_[0] * v_[0] - v_[1] * v_[1] + v_[2] * v_[2] - v_[3] * v_[3]) *
163 2 * (v_[2] * v_[3] - v_[0] * v_[1]) * o[2];
167 return 2 * (v_[1] * v_[3] - v_[0] * v_[2]) * o[0] +
168 2 * (v_[2] * v_[3] + v_[0] * v_[1]) * o[1] +
169 (v_[0] * v_[0] - v_[1] * v_[1] - v_[2] * v_[2] + v_[3] * v_[3]) *
173 IMP_THROW(
"Out of range coordinate " << coord, IndexException);
179 if (!has_cache_) fill_cache();
180 return Vector3D(o * matrix_[0], o * matrix_[1], o * matrix_[2]);
190 Vector3D *Dv, Rotation3DAdjoint *Dr)
const;
198 RotatedVector3DAdjoint
203 unsigned int coord)
const {
204 if (!has_cache_) fill_cache();
205 return o * matrix_[coord];
210 Vector3D get_rotation_matrix_row(
int i)
const {
212 if (!has_cache_) fill_cache();
216 { out << v_[0] <<
" " << v_[1] <<
" " << v_[2] <<
" " << v_[3]; });
221 "Attempting to invert uninitialized rotation");
222 Rotation3D ret(v_[0], -v_[1], -v_[2], -v_[3]);
232 "Attempting to access uninitialized rotation");
239 "Attempting to compose uninitialized rotation");
246 "Attempting to compose uninitialized rotation");
271 bool wrt_unnorm =
false)
const;
273 IMPALGEBRA_DEPRECATED_METHOD_DECL(2.12)
275 bool wrt_unnorm = true) const;
289 Eigen::MatrixXd get_jacobian_of_rotated(
290 const Eigen::Vector3d &v,
bool wrt_unnorm = false) const;
292 IMPALGEBRA_DEPRECATED_METHOD_DECL(2.12)
293 Eigen::MatrixXd get_gradient(
294 const Eigen::Vector3d &v,
bool wrt_unnorm = true) const;
300 bool get_is_valid()
const {
301 return v_.get_squared_magnitude() > 0;
325 IMPALGEBRA_DEPRECATED_FUNCTION_DECL(2.12)
326 IMPALGEBRAEXPORT Eigen::MatrixXd
327 get_gradient_of_composed_with_respect_to_first(
348 IMPALGEBRA_DEPRECATED_FUNCTION_DECL(2.12)
349 IMPALGEBRAEXPORT Eigen::MatrixXd
350 get_gradient_of_composed_with_respect_to_second(
375 double ans = std::min(dot, odot);
377 static const double s2 = std::sqrt(2.0);
378 double ret = ans / s2;
379 return std::max(std::min(ret, 1.0), 0.0);
398 "expected normalized vector as axis of rotation");
399 double s = std::sin(angle / 2);
401 a = std::cos(angle / 2);
402 b = axis_norm[0] * s;
403 c = axis_norm[1] * s;
404 d = axis_norm[2] * s;
424 Vector3D axis_norm = axis.get_unit_vector();
431 IMPALGEBRAEXPORT Rotation3D
438 IMPALGEBRAEXPORT Rotation3D
440 double m11,
double m12,
double m20,
double m21,
465 IMPALGEBRAEXPORT Rotation3D
490 return Rotation3D(uv[0], uv[1], uv[2], uv[3]);
496 return Rotation3D(a.v_[0] * b.v_[0] - a.v_[1] * b.v_[1] - a.v_[2] * b.v_[2] -
498 a.v_[0] * b.v_[1] + a.v_[1] * b.v_[0] + a.v_[2] * b.v_[3] -
500 a.v_[0] * b.v_[2] - a.v_[1] * b.v_[3] + a.v_[2] * b.v_[0] +
502 a.v_[0] * b.v_[3] + a.v_[1] * b.v_[2] - a.v_[2] * b.v_[1] +
511 IMPALGEBRAEXPORT
void
513 Rotation3DAdjoint *DA, Rotation3DAdjoint *DB);
520 IMPALGEBRAEXPORT ComposeRotation3DAdjoint
521 compose_adjoint(
const Rotation3D &A,
const Rotation3D &B,
const Rotation3DAdjoint &DC);
547 IMPALGEBRAEXPORT Rotation3D
561 IMPALGEBRAEXPORT Rotation3D
573 IMPALGEBRAEXPORT Rotation3D
582 FixedXYZ(
double x,
double y,
double z) {
587 double get_x()
const {
return v_[0]; }
588 double get_y()
const {
return v_[1]; }
589 double get_z()
const {
return v_[2]; }
591 { out << v_[0] <<
" " << v_[1] <<
" " << v_[2]; });
612 if (bq * aq < 0) bq = -bq;
620 IMPALGEBRAEXPORT Rotation3D
634 const Rotation3D &rot);
636 typedef std::pair<Vector3D, double> AxisAnglePair;
641 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