Rotation.cpp

Go to the documentation of this file.

#include <standard.hpp>
#include <GL/gl.h>
#include "Rotation.hpp"

Rotation::Rotation() : scaledAxis(3)
{
        
}


void Rotation::setScaledAxis(const GslVector &axis)
{
        if (axis.getNumRows() != 3)
                throw Error("Rotation::setScaledAxis(): axis param must have 3 elements.");

        scaledAxis = axis;
}

void Rotation::setAxisAndAngleRadians(const GslVector &axis, double angle)
{
        if (axis.getNumRows() != 3)
                throw Error("Rotation::setAxisAndAngleRadians(): axis param must have 3 elements.");
        scaledAxis = axis;
        scaledAxis *= angle;
}

void Rotation::getAxisAndAngleRadians(GslVector &axis, double &angle) const
{
        angle = scaledAxis.getLength();

        axis = scaledAxis;
        axis /= angle; // normalize the axis to return
}

void axisAngleToRotation (const GslVector &axis, double angle, GslMatrix &r)
{
        r.resize(3,3);
        r.setToIdentity();

        GslMatrix skew(3,3);
        skew.setToZero();
        skew(0,0) =  0;
        skew(0,1) = -axis(2);
        skew(0,2) = +axis(1);

        skew(1,0) = +axis(2);
        skew(1,1) =  0;
        skew(1,2) = -axis(0);

        skew(2,0) = -axis(1);
        skew(2,1) = +axis(0);
        skew(2,2) =  0;

        r += skew * sin(angle);
        r += (skew * skew) * (1-cos(angle));
}


void Rotation::getMatrix(GslMatrix & rotationMatrix)
{
        GslVector axis;
        double angle;

        getAxisAndAngleRadians(axis, angle);
        axisAngleToRotation(axis, angle, rotationMatrix);
}

void orthogonalizeMatrix (GslMatrix &m)
{
        assert (m.getNumRows() == 3 && m.getNumCols() == 3);

        GslMatrix u(3,3), v(3,3);
        GslVector s(3);

        m.SingularValueDecomposition(u,s,v);
        m = u * v.getTranspose();
}

void rotationMatrixToQuaternion (const GslMatrix &m, double &x, double &y, double &z, double &w)
{
        double trace = m.getTrace();
        if (trace > 0)
        {
                w = sqrt(trace + 1.) / 2.;
                double divisor = 1. / (4. * w);
                x = (m(2,1)-m(1,2)) * divisor;
                y = (m(0,2)-m(2,0)) * divisor;
                z = (m(1,0)-m(0,1)) * divisor;
        }
        else if (m(0,0) > m(1,1) && m(0,0) > m(2,2))
        {
                x = sqrt(m(0,0) - m(1,1) - m(2,2) + 1.) * .5;
                double divisor = 1. / (4. * x);
                y = (m(0,1) + m(1,0)) * divisor;
                z = (m(0,2) + m(2,0)) * divisor;
                w = (m(2,1) - m(1,2)) * divisor;
        }
        else if (m(1,1) > m(2,2))
        {
                y = sqrt(m(1,1) - m(0,0) - m(2,2) + 1.) * .5;
                double divisor = 1. / (4. * y);
                x = (m(0,1) + m(1,0)) * divisor;
                w = (m(0,2) - m(2,0)) * divisor;
                z = (m(1,2) + m(2,1)) * divisor;
        }
        else
        {
                z = sqrt(m(2,2) - m(0,0) - m(1,1) + 1.) * .5;
                double divisor = 1. / (4. * z);
                w = (m(1,0) - m(0,1)) * divisor;
                x = (m(0,2) + m(2,0)) * divisor;
                y = (m(1,2) + m(2,1)) * divisor;
        }
}

void quaternionToAxisAngle (double x, double y, double z, double w, GslVector &axis, double &angle)
{
        if (fabs(w) < 1)
        {
                angle = 2.*acos(w);
                double divisor = 1./sqrt(1.-w*w);
                axis(0) = x*divisor;
                axis(1) = y*divisor;
                axis(2) = z*divisor;
        }
        else
        {
                axis.setToZero();
                axis(0) = 1;
                angle = 0;
        }
}


void Rotation::setMatrix(GslMatrix rotationMatrix)
{
        if (rotationMatrix.getNumRows() != 3 ||
                rotationMatrix.getNumCols() != 3)
        {
                throw Error("Rotation::setMatrix(): rotationMatrix param must be 3x3.");
        }
/*
 * The rotation matrix may not be orthogonal, so let's straighten it out.
 */
        orthogonalizeMatrix (rotationMatrix);

/*
 * Convert the rotation matrix into an axis whose length denotes the
 * angle in radians.
 */
        double angle;
        double x,y,z,w;
        rotationMatrixToQuaternion(rotationMatrix, x, y, z, w);
        quaternionToAxisAngle(x,y,z,w,scaledAxis,angle);
        scaledAxis *= angle;
}

---