QuaternionAngVelDynamics.h

Go to the documentation of this file.

/* 
*/

#ifndef __OSESSAME_ATTDYN_QUATANGVEL_H__
#define __OSESSAME_ATTDYN_QUATANGVEL_H__
#include "matrix/Matrix.h"
#include "Integrator.h"
#include "Time.h"
#include "Orbit.h"
#include "OrbitState.h"
#include "Attitude.h"
#include <vector.h>
using namespace std;
using namespace O_SESSAME;

static void QuaternionAngVelConvFunc(const Matrix &_meshPoint, AttitudeState &_convertedAttitudeState)
{
    static Vector tempQ(4); 
    static Vector tempAngVel(3); 
    
    tempQ(_) = ~_meshPoint(_,_(2, 5));
    tempAngVel(_) = ~_meshPoint(1, _(6, 8));
    _convertedAttitudeState.SetState(Rotation(Quaternion(tempQ)), tempAngVel);
    return;
}

static Vector AttituteDynamics_QuaternionAngVel(const ssfTime &_time, const Vector& _integratingState, Orbit *_Orbit, Attitude *_Attitude, const Matrix &_parameters, const Functor &_torqueFuncPtr)
{
    static Vector stateDot(7);
    static Matrix bMOI(3,3); 
    static Matrix invMOI(3,3);
    static Matrix qtemp(4,3); 
    static Vector Tmoment(3);
    static Vector qIn(4); 
    static Vector wIn(3);
    qIn = _integratingState(_(VectorIndexBase,VectorIndexBase + 3));
    wIn = _integratingState(_(VectorIndexBase + 4,VectorIndexBase + 6));
    qIn /= norm2(qIn);

//    _attitudeState->SetState(Rotation(Quaternion(qIn)), wIn);  // Need to speed up in some way
    qtemp(_(VectorIndexBase+0,VectorIndexBase+2),_(VectorIndexBase+0,VectorIndexBase+2)) = skew(qIn(_(VectorIndexBase+0,VectorIndexBase+2))) + qIn(VectorIndexBase+3) * eye(3);
    qtemp(VectorIndexBase+3, _(VectorIndexBase+0,VectorIndexBase+2)) = -(~qIn(_(VectorIndexBase+0,VectorIndexBase+2)));

    bMOI = _parameters(_(MatrixIndexBase+0,MatrixIndexBase+2),_(MatrixIndexBase+0,MatrixIndexBase+2));
    invMOI = _parameters(_(MatrixIndexBase+3,MatrixIndexBase+5),_(MatrixIndexBase+0,MatrixIndexBase+2));
    Tmoment = _torqueFuncPtr.Call(_time, _Orbit->GetStateObject(), _Attitude->GetStateObject());
    
    stateDot(_(VectorIndexBase,VectorIndexBase + 3)) = 0.5 * qtemp * wIn;
    stateDot(_(VectorIndexBase + 4,VectorIndexBase + 6)) = (invMOI * (Tmoment - skew(wIn) * (bMOI * wIn)));
    return stateDot;
}

#endif
// Do not change the comments below - they will be added automatically by CVS
/*****************************************************************************
*       $Log: QuaternionAngVelDynamics.h,v $
*       Revision 1.2  2003/06/12 20:51:14  nilspace
*       Requires a precomputer MOI inverse.
*       
*       Revision 1.1  2003/06/12 19:17:42  nilspace
*       Initial Submission.
*       
*
******************************************************************************/

---