QuaternionKinematics.h Source File

00001 
00002 
00008 
00010 
00011 
00012 #ifndef QUATERNIONKINEMATICS_H
00013 #define QUATERNIONKINEMATICS_H
00014 #include "../rotation/Rotation.h"
00015 
00016 static Vector QuaternionKinematics(const double &_time, const Vector &_qIn, const Matrix &_wIn)
00017 {
00018     Matrix qtemp(4,3); 
00019     qtemp(_(VectorIndexBase+0,VectorIndexBase+2),_(VectorIndexBase+0,VectorIndexBase+2)) = skew(_qIn(_(VectorIndexBase+0,VectorIndexBase+2))) + _qIn(VectorIndexBase+3) * eye(3);
00020     qtemp(VectorIndexBase+3, _(VectorIndexBase+0,VectorIndexBase+2)) = -(~_qIn(_(VectorIndexBase+0,VectorIndexBase+2)));
00021 
00022     qtemp.initialize(0.5 * qtemp * _wIn);
00023     Vector qDot(QUATERNION_SIZE);qDot(_) = qtemp(_,MatrixIndexBase);
00024     return (Vector)qDot;
00025 }
00026 
00027 #endif
00028 // Do not change the comments below - they will be added automatically by CVS
00029 /*****************************************************************************
00030 *       $Log: QuaternionKinematics.h,v $
00031 *       Revision 1.2  2003/04/23 16:30:58  nilspace
00032 *       Various bugfixes & uploading of all changed code for new programmers.
00033 *       
00034 *       Revision 1.1  2003/03/25 02:28:20  nilspace
00035 *       initial submission. Needs to be verified.
00036 *
00037 ******************************************************************************/