diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index c8228ef9a3..045ebd94fb 100644 --- a/matrix/Dcm.hpp +++ b/matrix/Dcm.hpp @@ -78,32 +78,6 @@ public: * @param q quaternion to set dcm to */ Dcm(const Quaternion & q) { - set_from_quaternion(q); - } - - /** - * Constructor from euler angles - * - * This sets the transformation matrix from frame 2 to frame 1 where the rotation - * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence. - * - * - * @param euler euler angle instance - */ - Dcm(const Euler & euler) { - set_from_euler(euler); - } - - - /** - * Set from quaternion - * - * Instance is set from quaternion representing - * transformation from frame 2 to frame 1. - * - * @param q quaternion to set dcm to - */ - void set_from_quaternion(const Quaternion & q) { Dcm &dcm = *this; Type a = q(0); Type b = q(1); @@ -125,14 +99,15 @@ public: } /** - * Set from euler angles + * Constructor from euler angles * - * This provides the transformation matrix from frame 2 to frame 1 where the rotation - * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence + * This sets the transformation matrix from frame 2 to frame 1 where the rotation + * from frame 1 to frame 2 is described by a 3-2-1 intrinsic Tait-Bryan rotation sequence. * - * @param euler euler angle instannce + * + * @param euler euler angle instance */ - void set_from_euler(const Euler & euler) { + Dcm(const Euler & euler) { Dcm &dcm = *this; Type cosPhi = Type(cos(euler.phi())); Type sinPhi = Type(sin(euler.phi())); diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp index 0f3e934703..88abbc58ab 100644 --- a/matrix/Euler.hpp +++ b/matrix/Euler.hpp @@ -82,7 +82,9 @@ public: */ Euler(Type phi_, Type theta_, Type psi_) : Vector() { - set_from_euler(phi_, theta_, psi_); + phi() = phi_; + theta() = theta_; + psi() = psi_; } /** @@ -96,54 +98,6 @@ public: * @param dcm Direction cosine matrix */ Euler(const Dcm & dcm) : Vector() - { - set_from_dcm(dcm); - } - - /** - * Constructor from quaternion instance. - * - * Instance is set from a quaternion representing transformation - * from frame 2 to frame 1. - * This instance will hold the angles defining the 3-2-1 intrinsic - * Tait-Bryan rotation sequence from frame 1 to frame 2. - * - * @param q quaternion - */ - Euler(const Quaternion & q) : - Vector() - { - set_from_quaternion(q); - } - - /** - * Set from euler angles - * - * Instance is set from an 3-2-1 intrinsic Tait-Bryan rotation - * sequence representing transformation from frame 1 to frame 2. - * - * @param phi_ rotation angle about X axis - * @param theta_ rotation angle about Y axis - * @param psi_ rotation angle about Z axis - */ - void set_from_euler(Type phi_, Type theta_, Type psi_) - { - phi() = phi_; - theta() = theta_; - psi() = psi_; - } - - /** - * Set from DCM matrix - * - * Instance is set from Dcm representing transformation from - * frame 2 to frame 1. - * This instance will hold the angles defining the 3-2-1 intrinsic - * Tait-Bryan rotation sequence from frame 1 to frame 2. - * - * @param dcm Direction cosine matrix - */ - void set_from_dcm(const Dcm & dcm) { Type phi_val = Type(atan2(dcm(2,1), dcm(2,2))); Type theta_val = Type(asin(-dcm(2,0))); @@ -164,7 +118,7 @@ public: } /** - * Set from quaternion instance. + * Constructor from quaternion instance. * * Instance is set from a quaternion representing transformation * from frame 2 to frame 1. @@ -172,8 +126,9 @@ public: * Tait-Bryan rotation sequence from frame 1 to frame 2. * * @param q quaternion - */ - void set_from_quaternion(const Quaternion & q) + */ + Euler(const Quaternion & q) : + Vector() { *this = Euler(Dcm(q)); } diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 5730eab3b5..6965a37e87 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -3,8 +3,13 @@ * * All rotations and axis systems follow the right-hand rule. * - * A quaternion instance of this class describes a rotation from - * coordinate frame 1 to coordinate frame 2. The first element of the quaternion + * In order to rotate a vector v by a righthand rotation defined by the quaternion q + * one can use the following operation: + * v_rotated = q^(-1) * [0;v] * q + * where q^(-1) represents the inverse of the quaternion q. + * The product z of two quaternions z = q1 * q2 represents an intrinsic rotation + * in the order of first q1 followed by q2. + * The first element of the quaternion * represents the real part, thus, a quaternion representing a zero-rotation * is defined as (1,0,0,0). * @@ -84,7 +89,15 @@ public: Quaternion(const Dcm & dcm) : Vector() { - set_from_dcm(dcm); + Quaternion &q = *this; + q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) + + dcm(1, 1) + dcm(2, 2))); + q(1) = Type((dcm(2, 1) - dcm(1, 2)) / + (4 * q(0))); + q(2) = Type((dcm(0, 2) - dcm(2, 0)) / + (4 * q(0))); + q(3) = Type((dcm(1, 0) - dcm(0, 1)) / + (4 * q(0))); } /** @@ -98,59 +111,6 @@ public: */ Quaternion(const Euler & euler) : Vector() - { - set_from_euler(euler); - } - - /** - * Constructor from quaternion values - * - * Instance is initialized from quaternion values representing coordinate - * transformation from frame 2 to frame 1. - * A zero-rotation quaternion is represented by (1,0,0,0). - * - * @param a set quaternion value 0 - * @param b set quaternion value 1 - * @param c set quaternion value 2 - * @param d set quaternion value 3 - */ - Quaternion(Type a, Type b, Type c, Type d) : - Vector() - { - set_from_quaternion(a, b, c, d); - } - - /** - * Set from dcm - * - * Instance is set from a dcm representing coordinate transformation - * from frame 2 to frame 1. - * - * @param dcm dcm to set quaternion to - */ - void set_from_dcm(const Dcm & dcm) - { - Quaternion &q = *this; - q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) + - dcm(1, 1) + dcm(2, 2))); - q(1) = Type((dcm(2, 1) - dcm(1, 2)) / - (4 * q(0))); - q(2) = Type((dcm(0, 2) - dcm(2, 0)) / - (4 * q(0))); - q(3) = Type((dcm(1, 0) - dcm(0, 1)) / - (4 * q(0))); - } - - /** - * Set from euler angles - * - * This sets the instance to a quaternion representing coordinate transformation from - * frame 2 to frame 1 where the rotation from frame 1 to frame 2 is described - * by a 3-2-1 intrinsic Tait-Bryan rotation sequence. - * - * @param euler euler angles to set quaternion to - */ - void set_from_euler(const Euler & euler) { Quaternion &q = *this; Type cosPhi_2 = Type(cos(euler.phi() / (Type)2.0)); @@ -170,17 +130,19 @@ public: } /** - * Set from quaternion values + * Constructor from quaternion values * - * Instance is set from quaternion values representing coordinate + * Instance is initialized from quaternion values representing coordinate * transformation from frame 2 to frame 1. + * A zero-rotation quaternion is represented by (1,0,0,0). * * @param a set quaternion value 0 * @param b set quaternion value 1 * @param c set quaternion value 2 * @param d set quaternion value 3 */ - void set_from_quaternion(Type a, Type b, Type c, Type d) + Quaternion(Type a, Type b, Type c, Type d) : + Vector() { Quaternion &q = *this; q(0) = a;