SO3Util.h

//
//  SO3Util.h
//
 
#ifndef __So3Util__
#define __So3Util__
 
#include "Vector3d.h"
#include "Quaternion.h"
#include "Matrix3x3d.h"
#include <math.h>
 
// SO3 = matrice de rotation
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
 
#define EARTH_GRAVITY (9.81f)
 
static const double DEG_TO_RAD = M_PI / 180.0;
static const double RAD_TO_DEG = 180.0 / M_PI;
 
class SO3Util
{
  public:
    static void sO3FromTwoVec(Vector3d a, Vector3d b, Matrix3x3d &result);
    static void rotationPiAboutAxis(Vector3d v, Matrix3x3d &result);
    static void sO3FromMu(Vector3d w, Matrix3x3d &result);
    static void muFromSO3(Matrix3x3d so3, Vector3d &result);
    static void rodriguesSo3Exp(Vector3d w, double kA, double kB, Matrix3x3d &result);
    static void generatorField(int i, Matrix3x3d pos, Matrix3x3d &result);
};
 
#endif