#ifndef __JROTATION3D__ #define __JROTATION3D__ #include #include "JGeometry3D/JMatrix3D.hh" #include "JGeometry3D/JAngle3D.hh" #include "JGeometry3D/JVersor3D.hh" #include "JGeometry3D/JVersor3Z.hh" #include "JGeometry3D/JQuaternion3D.hh" #include "JGeometry2D/JRotation2D.hh" #include "JMath/JMath.hh" /** * \author mdejong */ namespace JGEOMETRY3D {} namespace JPP { using namespace JGEOMETRY3D; } namespace JGEOMETRY3D { using JGEOMETRY2D::JRotation2D; using JMATH::JMath; /** * Rotation around X-axis. */ class JRotation3X : public JRotation2D { public: /** * Default constructor (= identity matrix). */ JRotation3X() : JRotation2D() {} /** * Constructor. * * \param phi rotation angle (anti-clock wise) [rad] */ JRotation3X(const double phi) : JRotation2D(phi) {} }; /** * Rotation around Y-axis. */ class JRotation3Y : public JRotation2D { public: /** * Default constructor (= identity matrix). */ JRotation3Y() : JRotation2D() {} /** * Constructor. * * Note that in the x-z plane the corresponding rotation is clock wise. * * \param phi rotation angle (anti-clock wise) [rad] */ JRotation3Y(const double phi) : JRotation2D(-phi) {} }; /** * Rotation around Z-axis. */ class JRotation3Z : public JRotation2D { public: /** * Default constructor (= identity matrix). */ JRotation3Z() : JRotation2D() {} /** * Constructor. * * \param phi rotation angle (anti-clock wise) [rad] */ JRotation3Z(const double phi) : JRotation2D(phi) {} }; /** * Rotation matrix */ class JRotation3D : public JMatrix3D, public JMath { public: using JMath::mul; /** * Default constructor (= identity matrix). */ JRotation3D() : JMatrix3D() { setIdentity(); } /** * Constructor. * * The matrix is defined such that the rotation of a vector in the given direction ends up along the z-axis * and the back rotation of a vector parallel to the z-axis ends up in the given direction. * * \param dir direction */ JRotation3D(const JAngle3D& dir) : JMatrix3D() { const double ct = cos(dir.getTheta()); const double st = sin(dir.getTheta()); const double cp = cos(dir.getPhi()); const double sp = sin(dir.getPhi()); a00 = ct*cp; a01 = ct*sp; a02 = -st; a10 = -sp; a11 = cp; a12 = 0.0; a20 = st*cp; a21 = st*sp; a22 = +ct; } /** * Constructor. * * The matrix is defined such that the rotation of a vector in the given direction ends up along the z-axis * and the back rotation of a vector parallel to the z-axis ends up in the given direction. * * \param dir direction */ JRotation3D(const JVersor3D& dir) : JMatrix3D() { const double ct = dir.getDZ(); const double st = sqrt((1.0 + ct)*(1.0 - ct)); const double phi = atan2(dir.getDY(), dir.getDX()); const double cp = cos(phi); const double sp = sin(phi); a00 = ct*cp; a01 = ct*sp; a02 = -st; a10 = -sp; a11 = cp; a12 = 0.0; a20 = st*cp; a21 = st*sp; a22 = +ct; } /** * Constructor. * * The matrix is defined such that the rotation of a vector in the given direction ends up along the z-axis * and the back rotation of a vector parallel to the z-axis ends up in the given direction. * * \param dir direction */ JRotation3D(const JVersor3Z& dir) : JMatrix3D() { const double ct = dir.getDZ(); const double st = sqrt((1.0 + ct)*(1.0 - ct)); const double phi = atan2(dir.getDY(), dir.getDX()); const double cp = cos(phi); const double sp = sin(phi); a00 = ct*cp; a01 = ct*sp; a02 = -st; a10 = -sp; a11 = cp; a12 = 0.0; a20 = st*cp; a21 = st*sp; a22 = +ct; } /** * Constructor. * * \param R 2D rotation matrix around X-axis */ JRotation3D(const JRotation3X& R) : JMatrix3D() { a00 = 1.0; a01 = 0.0; a02 = 0.0; a10 = 0.0; a11 = R.a00; a12 = R.a01; a20 = 0.0; a21 = R.a10; a22 = R.a11; } /** * Constructor. * * \param R 2D rotation matrix around Y-axis */ JRotation3D(const JRotation3Y& R) : JMatrix3D() { a00 = R.a00; a01 = 0.0; a02 = R.a01; a10 = 0.0; a11 = 1.0; a12 = 0.0; a20 = R.a10; a21 = 0.0; a22 = R.a11; } /** * Constructor. * * \param R 2D rotation matrix around Z-axis */ JRotation3D(const JRotation3Z& R) : JMatrix3D() { a00 = R.a00; a01 = R.a01; a02 = 0.0; a10 = R.a10; a11 = R.a11; a12 = 0.0; a20 = 0.0; a21 = 0.0; a22 = 1.0; } /** * Constructor. * * \param Q quaternion */ JRotation3D(const JQuaternion3D& Q) : JMatrix3D() { const double a2 = Q.getA()*Q.getA(); const double b2 = Q.getB()*Q.getB(); const double c2 = Q.getC()*Q.getC(); const double d2 = Q.getD()*Q.getD(); const double ab = Q.getA()*Q.getB(); const double ac = Q.getA()*Q.getC(); const double ad = Q.getA()*Q.getD(); const double bc = Q.getB()*Q.getC(); const double bd = Q.getB()*Q.getD(); const double cd = Q.getC()*Q.getD(); a00 = a2 + b2 - c2 - d2; a01 = 2.0*bc - 2.0*ad; a02 = 2.0*bd + 2.0*ac; a10 = 2.0*bc + 2.0*ad; a11 = a2 - b2 + c2 - d2; a12 = 2.0*cd - 2.0*ab; a20 = 2.0*bd - 2.0*ac; a21 = 2.0*cd + 2.0*ab; a22 = a2 - b2 - c2 + d2; } /** * Get rotation. * * \return rotation */ const JRotation3D& getRotation() const { return static_cast(*this); } /** * Type conversion operator. * * \return quaternion */ operator JQuaternion3D() const { const double q2 = 0.25 * (1.0 + a00 + a11 + a22); if (q2 > 0.0) { const double a = sqrt(q2); const double w = 0.25 / a; const double b = (a21 - a12) * w; const double c = (a02 - a20) * w; const double d = (a10 - a01) * w; return JQuaternion3D(a,b,c,d).normalise(); } return JQuaternion3D(1.0, 0.0, 0.0, 0.0); } /** * Transpose. */ JRotation3D& transpose() { static_cast(*this).transpose(); return *this; } /** * Matrix multiplication. * * \param A matrix * \param B matrix * \return this matrix */ JRotation3D& mul(const JRotation3D& A, const JRotation3D& B) { static_cast(*this).mul(A, B); return *this; } /** * Rotate. * * \param __x x value * \param __y y value * \param __z z value */ void rotate(double& __x, double& __y, double& __z) const { const double x = a00 * __x + a01 * __y + a02 * __z; const double y = a10 * __x + a11 * __y + a12 * __z; const double z = a20 * __x + a21 * __y + a22 * __z; __x = x; __y = y; __z = z; } /** * Rotate back. * * \param __x x value * \param __y y value * \param __z z value */ void rotate_back(double& __x, double& __y, double& __z) const { const double x = a00 * __x + a10 * __y + a20 * __z; const double y = a01 * __x + a11 * __y + a21 * __z; const double z = a02 * __x + a12 * __y + a22 * __z; __x = x; __y = y; __z = z; } }; } #endif