Rotation.cpp

Go to the documentation of this file.

/*  \warning Testing not complete
*/

#include "Rotation.h"

namespace O_SESSAME {
Rotation::Rotation() : m_RotationSense(RIGHT_HAND) { }

Rotation::~Rotation() { }

Rotation::Rotation(const Matrix& _inMatrix) { Set(_inMatrix); }

Rotation::Rotation(const DirectionCosineMatrix& _Matrix) { Set(_Matrix); }

Rotation::Rotation(const Vector& _Angles, const int& _Sequence) { Set(_Angles, _Sequence); }

Rotation::Rotation(const Angle& _Angle1, const Angle& _Angle2, const Angle& _Angle3, const int& _Sequence) { Set(_Angle1, _Angle2, _Angle3, _Sequence); }

Rotation::Rotation(const Vector& _Axis, const Angle& _Angle) { Set(_Axis, _Angle); }

Rotation::Rotation(const ModifiedRodriguezParameters& _MRP) { Set(_MRP); }

Rotation::Rotation(const Quaternion& _quat) { Set(_quat); }

void Rotation::Set(const Matrix& _inMatrix)
{
    if((_inMatrix[MatrixRowsIndex].getIndexCount() == 3) && (_inMatrix[MatrixColsIndex].getIndexCount() == 3))
    {
        m_quaternion.Set(DirectionCosineMatrix(_inMatrix));
    }
    if((_inMatrix[MatrixRowsIndex].getIndexCount() == QUATERNION_SIZE) && (_inMatrix[MatrixColsIndex].getIndexCount() == 1))
    {
//        m_quaternion.Set(_inMatrix(_,MatrixIndexBase));
    }
    return;
}

void Rotation::Set(const DirectionCosineMatrix& _DCM)
{
    m_quaternion.Set(_DCM);
    return;
}

void Rotation::Set(const Vector& _Angles, const int& _Sequence)
{
    m_quaternion.Set(_Angles, _Sequence);
    return;
}

void Rotation::Set(const Angle& _Angle1, const Angle& _Angle2, const Angle& _Angle3, const int& _Sequence)
{
    m_quaternion.Set(_Angle1, _Angle2, _Angle3, _Sequence);
    return;
}

void Rotation::Set(const Vector& _Axis, const Angle& _Angle)
{
    m_quaternion.Set(_Axis, _Angle);
    return;
}

void Rotation::Set(const ModifiedRodriguezParameters& _MRP)
{
    m_quaternion.Set(_MRP);
    return;
}

void Rotation::Set(const Quaternion& _quat)
{
    m_quaternion(1) = _quat(1);
    m_quaternion(2) = _quat(2);
    m_quaternion(3) = _quat(3);
    m_quaternion(4) = _quat(4);
    return;
}

// Inspectors
DirectionCosineMatrix Rotation::GetDCM() const
{
    return DirectionCosineMatrix(m_quaternion);
}

Vector Rotation::GetEulerAngles(const int& _Sequence) const
{
    return m_quaternion.GetEulerAngles(_Sequence);
}

Vector Rotation::GetEulerAxisAngle() const
{
    return m_quaternion.GetEulerAxisAngle();
}

Vector Rotation::GetEulerAxisAngle(Vector& _EulerAxis, Angle& _EulerAngle) const
{
    return m_quaternion.GetEulerAxisAngle(_EulerAxis, _EulerAngle);
}

ModifiedRodriguezParameters Rotation::GetMRP() const
{
    return ModifiedRodriguezParameters(m_quaternion);
}

Quaternion Rotation::GetQuaternion() const
{
    return m_quaternion;
}

Vector Rotation::GetRotation(const RotationType& _type, const int& _Sequence) const
{
    if(_type == DCM_Type)
    {       
        Matrix tempMatrix = GetDCM();
        Vector tempVector(DCM_SIZE * DCM_SIZE);
        // convert the 3x3 matrix into a 9x1 vector, by rows.
        for (int ii = VectorIndexBase; ii < DCM_SIZE + VectorIndexBase; ++ii)
        {
            tempVector(_(VectorIndexBase + DCM_SIZE*(ii-VectorIndexBase),VectorIndexBase + DCM_SIZE*ii-1)) = ~tempMatrix(ii, _);
        }
        return tempVector;
    }
    else if(_type == EulerAngle_Type)
    {
        return GetEulerAngles(_Sequence);
    }
    else if(_type == EulerAxisAngle_Type)
    {
        return GetEulerAxisAngle();
    }
    else if(_type == MRP_Type)
    {
        return (Vector)GetMRP();
    }
    else if(_type == Quaternion_Type)
    {
        return (Vector)GetQuaternion();
    }
    
    // When in doubt, return the quaternion.
    return (Vector)GetQuaternion();
}

Rotation Rotation::operator* (const Rotation& _rot2) const
{
    return Rotation((*this).GetDCM() * _rot2.GetDCM());
}
Vector Rotation::operator* (const Vector& _vec2) const
{
    return ((*this).GetDCM() * _vec2);
}
    
Rotation Rotation::operator~ () const
{
    Rotation rotOut(~((*this).GetDCM()));
    return rotOut;
}
Rotation Rotation::operator+ (const Rotation& _rot2) const
{
    Rotation rotOut(this->operator*(_rot2));
    return rotOut;
}
Rotation Rotation::operator- (const Rotation& _rot2) const
{
    Rotation rotOut(this->operator*(~_rot2));
    return rotOut;
}
// DirectionCosineMatrix Class
DirectionCosineMatrix::DirectionCosineMatrix():Matrix(DCM_SIZE, DCM_SIZE)
{
    (*this) = eye(DCM_SIZE);
}
DirectionCosineMatrix::DirectionCosineMatrix(const DirectionCosineMatrix& _DCM):Matrix(DCM_SIZE, DCM_SIZE)
{
    Set(_DCM);
}
DirectionCosineMatrix::DirectionCosineMatrix(const Matrix& _DCM):Matrix(DCM_SIZE, DCM_SIZE)
{
    Set(_DCM);
}

DirectionCosineMatrix::DirectionCosineMatrix(const Vector& _EulerAngles, const int& _Sequence):Matrix(DCM_SIZE, DCM_SIZE)
{       
    Set(_EulerAngles, _Sequence);
}

DirectionCosineMatrix::DirectionCosineMatrix(const Angle& _Angle1, const Angle& _Angle2, const Angle& _Angle3, const int& _Sequence):Matrix(DCM_SIZE, DCM_SIZE)
{       
    Set(_Angle1, _Angle2, _Angle3, _Sequence);
}

DirectionCosineMatrix::DirectionCosineMatrix(const Vector& _EulerAxis, const Angle& _EulerAngle):Matrix(DCM_SIZE, DCM_SIZE)
{       
    Set(_EulerAxis, _EulerAngle);
}

DirectionCosineMatrix::DirectionCosineMatrix(const ModifiedRodriguezParameters& _MRP):Matrix(DCM_SIZE, DCM_SIZE)
{       
    Set(_MRP);
}
DirectionCosineMatrix::DirectionCosineMatrix(const Quaternion& _quat):Matrix(DCM_SIZE, DCM_SIZE)
{       
    Set(_quat);
}
DirectionCosineMatrix::~DirectionCosineMatrix() 
{
}

void DirectionCosineMatrix::Set(const DirectionCosineMatrix& _DCM)
{
    for (int ii = MatrixIndexBase; ii <= _DCM[MatrixRowsIndex].getIndexBound(); ++ii)
        for (int jj = MatrixIndexBase; jj <= _DCM[MatrixColsIndex].getIndexBound(); ++jj)
            (*this)(ii,jj) = _DCM(ii,jj);
    Normalize();
    return;
}

void DirectionCosineMatrix::Set(const Matrix& _DCM)
{
    for (int ii = MatrixIndexBase; ii <= _DCM[MatrixRowsIndex].getIndexBound(); ++ii)
        for (int jj = MatrixIndexBase; jj <= _DCM[MatrixColsIndex].getIndexBound(); ++jj)
            (*this)(ii,jj) = _DCM(ii,jj);
    Normalize();
    return;
}

void DirectionCosineMatrix::Set(const Vector& _EulerAngles, const int& _Sequence)
{
    Set(_EulerAngles(VectorIndexBase+0), _EulerAngles(VectorIndexBase+1), _EulerAngles(VectorIndexBase+2), _Sequence);
    return;
}

void DirectionCosineMatrix::Set(const Angle& _Angle1, const Angle& _Angle2, const Angle& _Angle3, const int& _Sequence)
{
    switch(_Sequence)
    {
        case 121: (*this) = R1(_Angle3) * R2(_Angle2) * R1(_Angle1); break;
        case 131: (*this) = R1(_Angle3) * R3(_Angle2) * R1(_Angle1); break;
        case 132: (*this) = R2(_Angle3) * R3(_Angle2) * R1(_Angle1); break;
        case 123: (*this) = R3(_Angle3) * R2(_Angle2) * R1(_Angle1); break;
        case 212: (*this) = R2(_Angle3) * R1(_Angle2) * R2(_Angle1); break;
        case 321: (*this) = R1(_Angle3) * R2(_Angle2) * R3(_Angle1); break;
        case 232: (*this) = R2(_Angle3) * R3(_Angle2) * R2(_Angle1); break;
        case 231: (*this) = R1(_Angle3) * R3(_Angle2) * R2(_Angle1); break;
        case 213: (*this) = R3(_Angle3) * R1(_Angle2) * R2(_Angle1); break;
        case 313: (*this) = R3(_Angle3) * R1(_Angle2) * R3(_Angle1); break;
        case 323: (*this) = R3(_Angle3) * R2(_Angle2) * R3(_Angle1); break;
        case 312: (*this) = R2(_Angle3) * R1(_Angle2) * R3(_Angle1); break;
        default: (*this) = eye(DCM_SIZE);
    }
    Normalize();
    return;
}

void DirectionCosineMatrix::Set(const Vector& _EulerAxis, const Angle& _EulerAngle)
{
    // Calculations done for each element to increase speed
    double Phi = 1 - cos(_EulerAngle);
    double e1 = _EulerAxis(VectorIndexBase+0);
    double e2 = _EulerAxis(VectorIndexBase+1);
    double e3 = _EulerAxis(VectorIndexBase+2);
    double cosPhi = cos(_EulerAngle);
    double sinPhi = sin(_EulerAngle);
    
    // Column 1
    (*this)(MatrixIndexBase+0,MatrixIndexBase+0) = e1 * e1 * Phi + cosPhi;
    (*this)(MatrixIndexBase+1,MatrixIndexBase+0) = e2 * e1 * Phi - e3 * sinPhi;
    (*this)(MatrixIndexBase+2,MatrixIndexBase+0) = e3 * e1 * Phi + e2 * sinPhi;
        
    // Column 2
    (*this)(MatrixIndexBase+0,MatrixIndexBase+1) = e1 * e2 * Phi + e3 * sinPhi;
    (*this)(MatrixIndexBase+1,MatrixIndexBase+1) = e2 * e2 * Phi + cosPhi; 
    (*this)(MatrixIndexBase+2,MatrixIndexBase+1) = e3 * e2 * Phi - e1 * sinPhi;
    
    // Column 3
    (*this)(MatrixIndexBase+0,MatrixIndexBase+2) = e1 * e3 * Phi - e2 * sinPhi;
    (*this)(MatrixIndexBase+1,MatrixIndexBase+2) = e2 * e3 * Phi + e1 * sinPhi;
    (*this)(MatrixIndexBase+2,MatrixIndexBase+2) = e3 * e3 * Phi + cosPhi;
    Normalize();
    return;
}

void DirectionCosineMatrix::Set(const ModifiedRodriguezParameters& _MRP)
{
    // Calculations done for each element to increase speed
    Matrix tempMatrix = (~(_MRP) * _MRP);
    double Sigma = 1 - tempMatrix(MatrixIndexBase,MatrixIndexBase);
    double sigma1 = _MRP(VectorIndexBase+0);
    double sigma2 = _MRP(VectorIndexBase+1);
    double sigma3 = _MRP(VectorIndexBase+2);
    
    // Column 1
    (*this)(MatrixIndexBase+0,MatrixIndexBase+0) = 4*(sigma1*sigma1 - sigma2*sigma2 - sigma3*sigma3) + Sigma*Sigma;
    (*this)(MatrixIndexBase+1,MatrixIndexBase+0) = 8*sigma2*sigma1 - 4*sigma3*Sigma;
    (*this)(MatrixIndexBase+2,MatrixIndexBase+0) = 8*sigma3*sigma1 + 4*sigma2*Sigma;
    
    // Column 2
    (*this)(MatrixIndexBase+0,MatrixIndexBase+1) = 8*sigma1*sigma2 + 4*sigma3*Sigma;
    (*this)(MatrixIndexBase+1,MatrixIndexBase+1) = 4*(-sigma1*sigma1 + sigma2*sigma2 - sigma3*sigma3) + Sigma*Sigma;
    (*this)(MatrixIndexBase+2,MatrixIndexBase+1) = 8*sigma3*sigma2 - 4*sigma1*Sigma;
    
    // Column 3
    (*this)(MatrixIndexBase+0,MatrixIndexBase+2) = 8*sigma1*sigma3 - 4*sigma2 * Sigma;
    (*this)(MatrixIndexBase+1,MatrixIndexBase+2) = 8*sigma2*sigma3 + 4*sigma1*Sigma;
    (*this)(MatrixIndexBase+2,MatrixIndexBase+2) = 4*(-sigma1*sigma1 - sigma2*sigma2 + sigma3*sigma3) + Sigma*Sigma;

    // Multiplying factor
    (*this) /= pow(1 + tempMatrix(MatrixIndexBase,MatrixIndexBase), 2);
    Normalize();
    return;
}

void DirectionCosineMatrix::Set(const Quaternion& _qIn)
{
    // Calculations done for each element to increase speed
    double q1 = _qIn(VectorIndexBase+0);
    double q2 = _qIn(VectorIndexBase+1);
    double q3 = _qIn(VectorIndexBase+2);
    double q4 = _qIn(VectorIndexBase+3);

    // Column 1
    (*this)(MatrixIndexBase+0,MatrixIndexBase+0) = q4*q4 + q1*q1 - q2*q2 - q3*q3;
//    (*this)(MatrixIndexBase+0,MatrixIndexBase+0) = 1 - 2 * (q2*q2 + q3*q3);
    (*this)(MatrixIndexBase+1,MatrixIndexBase+0) = 2 * (q1*q2 - q4*q3);
    (*this)(MatrixIndexBase+2,MatrixIndexBase+0) = 2 * (q1*q3 + q4*q2);
    
    // Column 2
    (*this)(MatrixIndexBase+0,MatrixIndexBase+1) = 2 * (q1*q2 + q4*q3);
    (*this)(MatrixIndexBase+1,MatrixIndexBase+1) = q4*q4 - q1*q1 + q2*q2 -q3*q3;
//    (*this)(MatrixIndexBase+0,MatrixIndexBase+0) = 1 - 2 * (q1*q1 + q3*q3);
    (*this)(MatrixIndexBase+2,MatrixIndexBase+1) = 2 * (q2*q3 - q4*q1);
    
    // Column 3
    (*this)(MatrixIndexBase+0,MatrixIndexBase+2) = 2 * (q1*q3 - q4*q2);
    (*this)(MatrixIndexBase+1,MatrixIndexBase+2) = 2 * (q2*q3 + q4*q1);
//    (*this)(MatrixIndexBase+0,MatrixIndexBase+0) = 1 - 2 * (q1*q1 + q2*q2);
    (*this)(MatrixIndexBase+2,MatrixIndexBase+2) = q4*q4 - q1*q1 - q2*q2 + q3*q3; 
    
    Normalize();
    return;
}

Vector DirectionCosineMatrix::GetEulerAngles(const int& _Sequence) const
{
    return Vector(EULERANGLES_SIZE);
}

void DirectionCosineMatrix::GetEulerAxisAngle(Vector& _EulerAxis, Angle& _EulerAngle) const
{
    _EulerAngle = acos(0.5 * (trace(*this) - 1));
    if(sin(_EulerAngle) != 0)
    {
        _EulerAxis(VectorIndexBase + 0) = ((*this)(MatrixIndexBase+1,MatrixIndexBase+2)
                                           - (*this)(MatrixIndexBase+1,MatrixIndexBase+2))
        / (2 * sin(_EulerAngle));
        _EulerAxis(VectorIndexBase + 1) = ((*this)(MatrixIndexBase+2,MatrixIndexBase+0)
                                           - (*this)(MatrixIndexBase+0,MatrixIndexBase+2))
            / (2 * sin(_EulerAngle));
        _EulerAxis(VectorIndexBase + 2) = ((*this)(MatrixIndexBase+0,MatrixIndexBase+1)
                                           - (*this)(MatrixIndexBase+0,MatrixIndexBase+1))
            / (2 * sin(_EulerAngle));
    }
    else
    { // no rotation, or rotation of 180 degs, therefore arbitrary axis.
        _EulerAxis.setToValue(0.0);
        _EulerAxis(VectorIndexBase + 0) = 1;
    }
    return;
}

ModifiedRodriguezParameters DirectionCosineMatrix::GetMRP() const
{
    return ModifiedRodriguezParameters(*this);
}

Quaternion DirectionCosineMatrix::GetQuaternion() const
{
    return Quaternion(*this);
}

void DirectionCosineMatrix::Normalize()
{
    double total = 0;
    
    for (int ii = MatrixIndexBase; ii < MatrixIndexBase + DCM_SIZE; ++ii)
    {
        total = sqrt(pow((*this)(ii, MatrixIndexBase),2) 
                        + pow((*this)(ii, MatrixIndexBase+1),2) 
                        + pow((*this)(ii, MatrixIndexBase+2),2));
        for (int jj = MatrixIndexBase; jj < MatrixIndexBase + DCM_SIZE; ++jj)
        {
            (*this)(ii, jj) /= total;
        }
    }    
    return;
}

DirectionCosineMatrix DirectionCosineMatrix::operator+ (const DirectionCosineMatrix& _DCM2) const
{
    return static_cast<DirectionCosineMatrix>(Matrix::operator+(static_cast<Matrix>(_DCM2)));
}

DirectionCosineMatrix DirectionCosineMatrix::operator- (const DirectionCosineMatrix& _DCM2) const
{
    return static_cast<DirectionCosineMatrix>(Matrix::operator-(static_cast<Matrix>(_DCM2)));
}

DirectionCosineMatrix DirectionCosineMatrix::operator* (DirectionCosineMatrix _DCM2) const 
{
    return static_cast<DirectionCosineMatrix>(Matrix::operator*(static_cast<Matrix>(_DCM2)));
}

inline Vector DirectionCosineMatrix::operator* (const Vector& _vec) const 
{
    return Matrix::operator*(_vec);
}

DirectionCosineMatrix DirectionCosineMatrix::operator~ () 
{
    return static_cast<DirectionCosineMatrix>(Matrix::operator~());
}

DirectionCosineMatrix R1(const Angle& _Angle)
{
    DirectionCosineMatrix R1_Out;
    R1_Out(MatrixIndexBase+1, MatrixIndexBase+1) = cos(_Angle);
    R1_Out(MatrixIndexBase+1, MatrixIndexBase+2) = sin(_Angle);
    R1_Out(MatrixIndexBase+2, MatrixIndexBase+1) = -sin(_Angle);
    R1_Out(MatrixIndexBase+2, MatrixIndexBase+2) = cos(_Angle);
    return R1_Out;
}

DirectionCosineMatrix R2(const Angle& _Angle)
{
    DirectionCosineMatrix R2_Out;
    R2_Out(MatrixIndexBase+0, MatrixIndexBase+0) = cos(_Angle);
    R2_Out(MatrixIndexBase+0, MatrixIndexBase+2) = -sin(_Angle);
    R2_Out(MatrixIndexBase+2, MatrixIndexBase+0) = sin(_Angle);
    R2_Out(MatrixIndexBase+2, MatrixIndexBase+2) = cos(_Angle);
    return R2_Out;
}

DirectionCosineMatrix R3(const Angle& _Angle)
{
    DirectionCosineMatrix R3_Out;
    R3_Out(MatrixIndexBase+0, MatrixIndexBase+0) = cos(_Angle);
    R3_Out(MatrixIndexBase+0, MatrixIndexBase+1) = sin(_Angle);
    R3_Out(MatrixIndexBase+1, MatrixIndexBase+0) = -sin(_Angle);
    R3_Out(MatrixIndexBase+1, MatrixIndexBase+1) = cos(_Angle);
    return R3_Out;
}

// ModifiedRodriguezParameters Class
ModifiedRodriguezParameters::ModifiedRodriguezParameters():Vector(MRP_SIZE)
{
    AutoSwitch();
}

ModifiedRodriguezParameters::ModifiedRodriguezParameters(const ModifiedRodriguezParameters& _MRP):Vector(MRP_SIZE)
{
    Set(_MRP);
    AutoSwitch();
}

ModifiedRodriguezParameters::ModifiedRodriguezParameters(const double& _s1, const double& _s2, const double& _s3):Vector(MRP_SIZE)
{
    Set(_s1,_s2,_s3);
    AutoSwitch();
}
    
ModifiedRodriguezParameters::ModifiedRodriguezParameters(const Vector& _sVector):Vector(MRP_SIZE)
{
    Set(_sVector);
    AutoSwitch();
}

ModifiedRodriguezParameters::ModifiedRodriguezParameters(const DirectionCosineMatrix& _DCM):Vector(MRP_SIZE)
{
    Set(_DCM);
    AutoSwitch();
}

ModifiedRodriguezParameters::ModifiedRodriguezParameters(const Vector& _Angles, const int& _Sequence):Vector(MRP_SIZE)
{
    Set(_Angles, _Sequence);
    AutoSwitch();
}

ModifiedRodriguezParameters::ModifiedRodriguezParameters(const Vector& _EulerAxis, const Angle& _EulerAngle):Vector(MRP_SIZE)
{
    Set(_EulerAxis, _EulerAngle);
    AutoSwitch();
}
    
ModifiedRodriguezParameters::ModifiedRodriguezParameters(const Quaternion& _qIn):Vector(MRP_SIZE)
{
    Set(_qIn);
    AutoSwitch();
}

void ModifiedRodriguezParameters::Set(const ModifiedRodriguezParameters& _MRP)
{
    for(int ii = VectorIndexBase;ii < VectorIndexBase + MRP_SIZE; ++ii)
        (*this)(ii) = _MRP(ii);
    return;
}
    
void ModifiedRodriguezParameters::Set(const double& _s1, const double& _s2, const double& _s3)
{
    (*this)(VectorIndexBase+0) = _s1;
    (*this)(VectorIndexBase+1) = _s2;
    (*this)(VectorIndexBase+2) = _s3;
    return;
}

void ModifiedRodriguezParameters::Set(const Vector& _sVector)
{
    for(int ii = VectorIndexBase;ii < VectorIndexBase + MRP_SIZE; ++ii)
        (*this)(ii) = _sVector(ii);
    return;
}
    
void ModifiedRodriguezParameters::Set(const DirectionCosineMatrix& _DCM)
{
    Set(_DCM.GetQuaternion());
    return;
}

void ModifiedRodriguezParameters::Set(const Vector& _EulerAngles, const int& _Sequence)
{
    Set(DirectionCosineMatrix(_EulerAngles, _Sequence));
    return;
}

void ModifiedRodriguezParameters::Set(const Angle& _Angle1, const Angle& _Angle2, const Angle& _Angle3, const int& _Sequence)
{
    Set(DirectionCosineMatrix(_Angle1, _Angle2, _Angle3, _Sequence));
    return;
}


void ModifiedRodriguezParameters::Set(const Vector& _EulerAxis, const Angle& _EulerAngle)
{
    (*this) = (ModifiedRodriguezParameters) (tan(0.25 * _EulerAngle) * _EulerAxis);
    return;
}

    
void ModifiedRodriguezParameters::Set(const Quaternion& _qIN)
{
    double denom = (1+_qIN(VectorIndexBase+3));
    if(denom == 0)
    {
        denom = 2;
        (*this)(1) = (-_qIN(1) / denom);
        (*this)(2) = (-_qIN(2) / denom);
        (*this)(3) = (-_qIN(3) / denom);
    }
    else
    {
        (*this)(1) = (_qIN(1) / denom);
        (*this)(2) = (_qIN(2) / denom);
        (*this)(3) = (_qIN(3) / denom);
    }
    return;
}

DirectionCosineMatrix ModifiedRodriguezParameters::GetDCM() const
{
    return DirectionCosineMatrix(*this);
}

Vector ModifiedRodriguezParameters::GetEulerAngles(int _Sequence) const
{
    return Vector(MRP_SIZE);
}

void ModifiedRodriguezParameters::GetEulerAxisAngle(Vector& _EulerAxis, Angle& _EulerAngle) const
{
    Quaternion qTemp(*this);
    qTemp.GetEulerAxisAngle(_EulerAxis, _EulerAngle);
    return;
}
    
Quaternion ModifiedRodriguezParameters::GetQuaternion() const
{
    return Quaternion(*this);
}
    
void ModifiedRodriguezParameters::Switch(int _SwitchThreshold)
{
    if (norm2((Vector)*this) > _SwitchThreshold)
        (*this) = ShadowSet();
    return;
}

void ModifiedRodriguezParameters::AutoSwitch(bool _SwitchBoolean)
{
    m_AutoSwitch = _SwitchBoolean;
    return;
}


ModifiedRodriguezParameters ModifiedRodriguezParameters::ShadowSet() const
{
    return (ModifiedRodriguezParameters)(-(*this)(_) / pow(norm2(*this),2));
}


ModifiedRodriguezParameters ModifiedRodriguezParameters::operator+ (const ModifiedRodriguezParameters& _MRP2) const
{
    ModifiedRodriguezParameters MRPsum;
    MRPsum(_) = (1-pow(norm2(*this),2)) * _MRP2
        + (1-pow(norm2(_MRP2),2)) * (*this)
              - 2*crossP(_MRP2,(*this));
    MRPsum(_) = MRPsum / (1 + pow(norm2(*this),2) * pow(norm2(_MRP2),2)
        - 2 * (*this).dot( _MRP2));
    return MRPsum;
}


ModifiedRodriguezParameters ModifiedRodriguezParameters::operator- (const ModifiedRodriguezParameters& _MRP2) const
{
    ModifiedRodriguezParameters MRPsum;
    MRPsum(_) = (1-pow(norm2(_MRP2),2)) * (*this)
        + (1-pow(norm2(*this),2)) * _MRP2
        - 2*crossP((*this), _MRP2);
    MRPsum(_) = MRPsum / ((1 + pow(norm2(_MRP2),2)) * pow(norm2(*this),2)
                        - 2 * _MRP2.dot(*this));
    return MRPsum;
}

// Quaternion Class
Quaternion::Quaternion():Vector(QUATERNION_SIZE)
{
//    (*this).setToValue(0.0);
    (*this)(VectorIndexBase+3) = 1.0;   
}

Quaternion::Quaternion(double _q1, double _q2, double _q3, double _q4):Vector(QUATERNION_SIZE)
{
    Set(_q1, _q2, _q3, _q4);
}

/*
Quaternion::Quaternion(const Matrix& _qMatrix):Vector(QUATERNION_SIZE)
{
    Set(_qMatrix);
}
*/

Quaternion::Quaternion(const Vector& _qVector):Vector(QUATERNION_SIZE)
{
    Set(_qVector);
}

Quaternion::Quaternion(const DirectionCosineMatrix& _DCM):Vector(QUATERNION_SIZE)
{
    Set(_DCM);
}

Quaternion::Quaternion(const Vector& _EulerAngles, const int& _Sequence):Vector(QUATERNION_SIZE)
{
    Set(_EulerAngles, _Sequence);
    Normalize();
}

Quaternion::Quaternion(const Vector& _EulerAxis, const Angle& _EulerAngle):Vector(QUATERNION_SIZE)
{
    Set(_EulerAxis, _EulerAngle);
}

Quaternion::Quaternion(const ModifiedRodriguezParameters& _MRP):Vector(QUATERNION_SIZE)
{
    Set(static_cast<ModifiedRodriguezParameters>(_MRP));
}

void Quaternion::Set(const Quaternion& _qIn)
{
    (*this)(VectorIndexBase+0) = _qIn(MatrixIndexBase+0);       
    (*this)(VectorIndexBase+1) = _qIn(MatrixIndexBase+1);       
    (*this)(VectorIndexBase+2) = _qIn(MatrixIndexBase+2);       
    (*this)(VectorIndexBase+3) = _qIn(MatrixIndexBase+3);
    Normalize();
    return;
}

void Quaternion::Set(double _q1, double _q2, double _q3, double _q4)
{
    (*this)(VectorIndexBase+0) = _q1;   
    (*this)(VectorIndexBase+1) = _q2;   
    (*this)(VectorIndexBase+2) = _q3;   
    (*this)(VectorIndexBase+3) = _q4;   
    Normalize();
    return;
}

/*
void Quaternion::Set(const Matrix& _qMatrix)
{
    if((_qMatrix[MatrixRowsIndex].getIndexBound() == QUATERNION_SIZE) && (_qMatrix[MatrixColsIndex].getIndexBound() == 1)) 
    {
        (*this)(VectorIndexBase+0) = _qMatrix(MatrixIndexBase+0,MatrixIndexBase);       
        (*this)(VectorIndexBase+1) = _qMatrix(MatrixIndexBase+1,MatrixIndexBase);       
        (*this)(VectorIndexBase+2) = _qMatrix(MatrixIndexBase+2,MatrixIndexBase);       
        (*this)(VectorIndexBase+3) = _qMatrix(MatrixIndexBase+3,MatrixIndexBase);
    }
    else
    {
        (*this) = Quaternion();
    }
    return;
}*/
    
void Quaternion::Set(const Vector& _qVector)
{
    (*this)(VectorIndexBase+0) = _qVector(MatrixIndexBase+0);   
    (*this)(VectorIndexBase+1) = _qVector(MatrixIndexBase+1);   
    (*this)(VectorIndexBase+2) = _qVector(MatrixIndexBase+2);   
    (*this)(VectorIndexBase+3) = _qVector(MatrixIndexBase+3);
    Normalize();
    return;
}


void Quaternion::Set(const DirectionCosineMatrix& _DCM)
{
    (*this)(VectorIndexBase+3) = 0.5 * sqrt(1 + trace(_DCM));
    (*this)(VectorIndexBase+0) = _DCM(MatrixIndexBase+1,MatrixIndexBase+2)
        - _DCM(MatrixIndexBase+2,MatrixIndexBase+1);
    (*this)(VectorIndexBase+1) = _DCM(MatrixIndexBase+2,MatrixIndexBase+0)
        - _DCM(MatrixIndexBase+0,MatrixIndexBase+2);
    (*this)(VectorIndexBase+2) = _DCM(MatrixIndexBase+0,MatrixIndexBase+1)
        - _DCM(MatrixIndexBase+1,MatrixIndexBase+0);
    (*this)(_(VectorIndexBase,VectorIndexBase+2)) /= (4 * (*this)(VectorIndexBase+3));
    Normalize();
    return;
}

void Quaternion::Set(const Vector& _EulerAngles, const int& _Sequence)
{
    Set(DirectionCosineMatrix(_EulerAngles, _Sequence));
    Normalize();
    return;
}


void Quaternion::Set(const Vector& _EulerAxis, const Angle& _EulerAngle)
{
    (*this)(_(VectorIndexBase+0,VectorIndexBase+2)) = _EulerAxis * sin(0.5 * _EulerAngle);              
    (*this)(VectorIndexBase+3) = cos(0.5 * _EulerAngle);
    Normalize();
    return;
}


void Quaternion::Set(const ModifiedRodriguezParameters& _MRP)
{
    Matrix tempMatrix = (~_MRP) * (_MRP);
    double sigmaSq = tempMatrix(MatrixIndexBase,MatrixIndexBase);
    double denom = 1 + sigmaSq;
    (*this)(1) = 2 * _MRP(1) / denom;           
    (*this)(2) = 2 * _MRP(2) / denom;           
    (*this)(3) = 2 * _MRP(3) / denom;           
    (*this)(VectorIndexBase+3) = (1 - sigmaSq) /denom;
    Normalize();
    return;
}

DirectionCosineMatrix Quaternion::GetDCM() const
{
    return DirectionCosineMatrix(*this);
}

Vector Quaternion::GetEulerAngles(const int& _Sequence) const
{
    return Vector(EULERANGLES_SIZE);
}


Vector Quaternion::GetEulerAxisAngle(Vector& _EulerAxis, Angle& _EulerAngle) const
{
    Vector returnVector(EULERAXIS_SIZE + 1);
    _EulerAngle = 2.0 * acos((*this)(VectorIndexBase+3));
    _EulerAxis = (*this)(_(VectorIndexBase+0,VectorIndexBase+2)) * sin(0.5 * _EulerAngle);
    returnVector(_(VectorIndexBase,VectorIndexBase+EULERAXIS_SIZE-1)) = _EulerAxis;
    returnVector(VectorIndexBase + EULERAXIS_SIZE) = _EulerAngle;
    return returnVector;
}


Vector Quaternion::GetEulerAxisAngle() const
{
    Vector EulerAxis(EULERAXIS_SIZE);
    double EulerAngle;
    return GetEulerAxisAngle(EulerAxis, EulerAngle);
}

    
ModifiedRodriguezParameters Quaternion::GetMRP() const
{
    return ModifiedRodriguezParameters(*this);
}


void Quaternion::Normalize()
{
    normalize(*this);
    return;
}

Quaternion Quaternion::operator+ (const Quaternion& _quat2) const
{
    Matrix qSkewed(4,4);
    // Column 1
    qSkewed(_,MatrixIndexBase+0) = (*this);
    // Column 2
    qSkewed(MatrixIndexBase+0,MatrixIndexBase+1) =  (*this)(VectorIndexBase+0); 
    qSkewed(MatrixIndexBase+1,MatrixIndexBase+1) =  (*this)(VectorIndexBase+2);
    qSkewed(MatrixIndexBase+2,MatrixIndexBase+1) = -(*this)(VectorIndexBase+1);
    qSkewed(MatrixIndexBase+3,MatrixIndexBase+1) = -(*this)(VectorIndexBase+3);
    // Column 3
    qSkewed(MatrixIndexBase+0,MatrixIndexBase+2) = -(*this)(VectorIndexBase+1); 
    qSkewed(MatrixIndexBase+1,MatrixIndexBase+2) =  (*this)(VectorIndexBase+2);
    qSkewed(MatrixIndexBase+2,MatrixIndexBase+2) =  (*this)(VectorIndexBase+3);
    qSkewed(MatrixIndexBase+3,MatrixIndexBase+2) = -(*this)(VectorIndexBase+0);
    // Column 4
    qSkewed(MatrixIndexBase+0,MatrixIndexBase+3) =  (*this)(VectorIndexBase+1); 
    qSkewed(MatrixIndexBase+1,MatrixIndexBase+3) = -(*this)(VectorIndexBase+0);
    qSkewed(MatrixIndexBase+2,MatrixIndexBase+3) =  (*this)(VectorIndexBase+3);
    qSkewed(MatrixIndexBase+3,MatrixIndexBase+3) = -(*this)(VectorIndexBase+2);

    return Quaternion(qSkewed * _quat2);
}

Quaternion Quaternion::operator- (const Quaternion& _quat2) const
{
    Matrix qSkewed(4,4);
    // Column 1
    qSkewed(MatrixIndexBase+0,MatrixIndexBase+0) = -_quat2(VectorIndexBase+0);
    qSkewed(MatrixIndexBase+1,MatrixIndexBase+0) = -_quat2(VectorIndexBase+1);
    qSkewed(MatrixIndexBase+2,MatrixIndexBase+0) = -_quat2(VectorIndexBase+2);
    qSkewed(MatrixIndexBase+3,MatrixIndexBase+0) =  _quat2(VectorIndexBase+3);
    // Column 2
    qSkewed(MatrixIndexBase+0,MatrixIndexBase+1) =  _quat2(VectorIndexBase+3);  
    qSkewed(MatrixIndexBase+1,MatrixIndexBase+1) = -_quat2(VectorIndexBase+2);
    qSkewed(MatrixIndexBase+2,MatrixIndexBase+1) =  _quat2(VectorIndexBase+1);
    qSkewed(MatrixIndexBase+3,MatrixIndexBase+1) =  _quat2(VectorIndexBase+0);
    // Column 3
    qSkewed(MatrixIndexBase+0,MatrixIndexBase+2) =  _quat2(VectorIndexBase+2);  
    qSkewed(MatrixIndexBase+1,MatrixIndexBase+2) =  _quat2(VectorIndexBase+3);
    qSkewed(MatrixIndexBase+2,MatrixIndexBase+2) = -_quat2(VectorIndexBase+0);
    qSkewed(MatrixIndexBase+3,MatrixIndexBase+2) =  _quat2(VectorIndexBase+1);
    // Column 4
    qSkewed(MatrixIndexBase+0,MatrixIndexBase+3) = -_quat2(VectorIndexBase+1);  
    qSkewed(MatrixIndexBase+1,MatrixIndexBase+3) =  _quat2(VectorIndexBase+0);
    qSkewed(MatrixIndexBase+2,MatrixIndexBase+3) =  _quat2(VectorIndexBase+3);
    qSkewed(MatrixIndexBase+3,MatrixIndexBase+3) =  _quat2(VectorIndexBase+2);

    return Quaternion((qSkewed) * (*this));
}
} // end of namespace O_SESSAME
/*!***************************************************************************
*       $Log: Rotation.cpp,v $
*       Revision 1.23  2003/06/10 14:23:42  nilspace
*       Changed MRP::Set(Quaternion) to not overwrite the const.
*       
*       Revision 1.22  2003/06/10 02:25:32  nilspace
*       Fixed MRP::Set(Quaternion) since there apparently is no Vector::operator-() that returns the negative of a Vector.
*       
*       Revision 1.21  2003/05/27 20:01:38  nilspace
*       Fixed numerous bugs with MRP conversion functions.
*       
*       Revision 1.20  2003/05/27 14:39:12  nilspace
*       Fixed the MRP->Quaternion conversion.
*       
*       Revision 1.19  2003/05/22 03:03:38  nilspace
*       Update documentation, and changed all angles to use Angle type.
*       
*       Revision 1.18  2003/05/21 22:18:05  nilspace
*       Moved the documentation to the implementation file.
*       
*       Revision 1.17  2003/05/20 17:53:51  nilspace
*       Updated comments.
*       
*       Revision 1.16  2003/05/13 19:45:10  nilspace
*       Updated comments.
*       
*       Revision 1.15  2003/05/02 19:44:24  nilspace
*       Modified the DCM::Set(Quaternion) function for a more efficient calculation on the diagonals.
*       
*       Revision 1.14  2003/04/28 14:16:26  nilspace
*       Moved all function definitions out of header file into implementation file.
*       
*       Revision 1.13  2003/04/27 20:41:09  nilspace
*       Encapsulated the rotation library into the namespace O_SESSAME.
*       
*       Revision 1.12  2003/04/22 21:59:58  nilspace
*       Fixed Quaternion::Set(DCM) so it evaluates correctly.
*       Implemented Rotation::operator-()
*       
*       Revision 1.11  2003/04/22 20:52:39  nilspace
*       Added Normalize() call to all of the DCM::Set functions.
*       
*       Revision 1.10  2003/04/22 19:45:16  nilspace
*       Fixed DCM::Set to correctly calculate the MRP tempMatrix (sigma^2)
*       
*       Revision 1.9  2003/04/22 19:36:02  nilspace
*       Various bug fixes to DCM & quaternion conversions. Added DirectionCosineMatrix Normalize().
*       
*       Revision 1.8  2003/04/22 16:03:11  nilspace
*       Updated MRP::Set(Quaternion) to correctly set.
*       
*       Revision 1.7  2003/04/10 17:25:51  nilspace
*       changelog
*       
*       Revision 1.6  2003/04/08 23:02:27  nilspace
*       Added Rotation Sense, or "Handedness". Defaults to RIGHT_HAND
*       
*       Revision 1.5  2003/03/27 20:22:26  nilspace
*       Added GetRotation() function.
*       Fixed Quaternion.Set(Quaternion) function.
*       Added RotationType enum.
*       Made sure to normalize the Quaternions for all the Set() functions.
*       
*       Revision 1.4  2003/03/25 02:37:36  nilspace
*       Added quaternion matrix set and constructor.
*       Added Rotation generalized matrix set to check for quaternion.
*       
*       Revision 1.3  2003/03/04 17:36:19  nilspace
*       Added CVS tags for documenting uploads. Also chmod'd to not be executable.
*       
*       Revision 1.1  2003/02/27 18:37:26  nilspace
*       Initial submission of Rotation class implementation.
*       
*
******************************************************************************/

---