Rotation3D.cpp

Go to the documentation of this file.

// ------------------------------------------------------------------------
// $RCSfile: Rotation3D.cpp,v $
// ------------------------------------------------------------------------
// $Revision: 1.2 $
// ------------------------------------------------------------------------
// Copyright: see Copyright.readme
// ------------------------------------------------------------------------
//
// Class: Rotation3D
//   Represents a rotation in 3-dimensional space.
//
//   There are two possible external representations:
//
//   - Matrix3D R.
//     The matrix R is an element of SO(3). Its column vectors define the
//     unit vectors of the rotated coordinate system, expressed in the
//     original system. This representation is used internally and can only
//     be read from a Rotation3D object. To build such an object, use the
//     other representation.
//
//   - Vector3D V.
//     The direction of V defines the axis of rotation, and its length the
//     rotation angle. A zero vector represents the identity.
//
// ------------------------------------------------------------------------
// Class category: BeamlineGeometry
// ------------------------------------------------------------------------
//
// $Date: 2001/08/24 19:32:00 $
// $Author: jsberg $
//
// ------------------------------------------------------------------------

#include "BeamlineGeometry/Rotation3D.h"
#include <algorithm>
#include <cmath>


// Class Rotation3D
// ------------------------------------------------------------------------

Rotation3D::Rotation3D(double vx, double vy, double vz):
    R() {
    double phi = sqrt(vx * vx + vy * vy + vz * vz);

    if(phi != 0.0) {
        double factor = sin(phi / 2.0) / phi;
        double S0 = cos(phi / 2.0);
        double S1 = factor * vx;
        double S2 = factor * vy;
        double S3 = factor * vz;

        R(0, 0) = 2.0 * (S1 * S1 + S0 * S0) - 1.0;
        R(0, 1) = 2.0 * (S1 * S2 - S0 * S3);
        R(0, 2) = 2.0 * (S1 * S3 + S0 * S2);

        R(1, 0) = 2.0 * (S2 * S1 + S0 * S3);
        R(1, 1) = 2.0 * (S2 * S2 + S0 * S0) - 1.0;
        R(1, 2) = 2.0 * (S2 * S3 - S0 * S1);

        R(2, 0) = 2.0 * (S3 * S1 - S0 * S2);
        R(2, 1) = 2.0 * (S3 * S2 + S0 * S1);
        R(2, 2) = 2.0 * (S3 * S3 + S0 * S0) - 1.0;
    }
}


void Rotation3D::getAxis(double &vx, double &vy, double &vz) const {
    vx = (R(2, 1) - R(1, 2)) / 2.0;
    vy = (R(0, 2) - R(2, 0)) / 2.0;
    vz = (R(1, 0) - R(0, 1)) / 2.0;
    double c = (R(0, 0) + R(1, 1) + R(2, 2) - 1.0) / 2.0;
    double s = sqrt(vx * vx + vy * vy + vz * vz);
    double phi = atan2(s, c);

    if(c < 0.0) {
        // NOTE: We must avoid negative arguments to sqrt(),
        // which may occur due to rounding errors.
        vx = (vx > 0.0 ? phi : (-phi)) * sqrt(std::max(R(0, 0) - c, 0.0) / (1.0 - c));
        vy = (vy > 0.0 ? phi : (-phi)) * sqrt(std::max(R(1, 1) - c, 0.0) / (1.0 - c));
        vz = (vz > 0.0 ? phi : (-phi)) * sqrt(std::max(R(2, 2) - c, 0.0) / (1.0 - c));
    } else if(std::abs(s) > 1.0e-10) {
        double factor = phi / s;
        vx *= factor;
        vy *= factor;
        vz *= factor;
    }
}


Vector3D Rotation3D::getAxis() const {
    double vx, vy, vz;
    getAxis(vx, vy, vz);
    return Vector3D(vx, vy, vz);
}


Rotation3D Rotation3D::inverse() const {
    Rotation3D inv;

    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            inv.R(i, j) = R(j, i);
        }
    }

    return inv;
}


bool Rotation3D::isPureXRotation() const {
    return (R(0, 1) == 0.0) && (R(1, 0) == 0.0) && (R(0, 2) == 0.0) && (R(2, 0) == 0.0);
}


bool Rotation3D::isPureYRotation() const {
    return (R(1, 2) == 0.0) && (R(2, 1) == 0.0) && (R(1, 0) == 0.0) && (R(0, 1) == 0.0);
}


bool Rotation3D::isPureZRotation() const {
    return (R(2, 0) == 0.0) && (R(0, 2) == 0.0) && (R(2, 1) == 0.0) && (R(1, 2) == 0.0);
}


Rotation3D Rotation3D::Identity() {
    return Rotation3D();
}


Rotation3D Rotation3D::XRotation(double angle) {
    Rotation3D result;
    result.R(1, 1) =    result.R(2, 2) = cos(angle);
    result.R(1, 2) = - (result.R(2, 1) = sin(angle));
    return result;
}


Rotation3D Rotation3D::YRotation(double angle) {
    Rotation3D result;
    result.R(2, 2) =    result.R(0, 0) = cos(angle);
    result.R(2, 0) = - (result.R(0, 2) = sin(angle));
    return result;
}


Rotation3D Rotation3D::ZRotation(double angle) {
    Rotation3D result;
    result.R(0, 0) =    result.R(1, 1) = cos(angle);
    result.R(0, 1) = - (result.R(1, 0) = sin(angle));
    return result;
}