Rotation.h

Go to the documentation of this file.

#ifndef ROBOTICS_ROTATION_H
#define ROBOTICS_ROTATION_H

#include <KrisLibrary/math/matrix.h>
#include <KrisLibrary/math3d/primitives.h>
#include <KrisLibrary/math3d/rotation.h>

using namespace Math3D;


struct DirectionCosines : public Vector
{
  DirectionCosines();
  DirectionCosines(const DirectionCosines& c);
  DirectionCosines(const Vector& x);
  DirectionCosines(const Matrix3& m);

  void setMatrix(const Matrix3&);
  void getMatrix(Matrix3&) const;
  void getMoment(MomentRotation& mr) const;
  void getMomentJacobian(Matrix& J) const;
  void getQuaternion(QuaternionRotation& q) const;
  void getQuaternionJacobian(Matrix& J) const;

  static void momentJacobian(const MomentRotation& mr,Matrix& J);
  static void quaternionJacobian(const Quaternion& q,Matrix& J);

  inline Real& a(int i) { return operator()(i); }
  inline Real& b(int i) { return operator()(i+3); }
  inline Real& c(int i) { return operator()(i+6); }

  inline const Real& a(int i) const { return operator()(i); }
  inline const Real& b(int i) const { return operator()(i+3); }
  inline const Real& c(int i) const { return operator()(i+6); }
};

bool GetRotationCenter(const RigidTransform& T,Vector3& p);

void GetMinimalRotation(const Vector3& x,const Vector3& y,Matrix3& R);

void GetMinimalRotationToPlane(const Vector3& x,const Vector3& y,Matrix3& R);

void MatrixDerivative(const Matrix3& R,const Vector3& z,Matrix3& dR);

void DirectionCosinesDerivative(const Vector& R,const Vector3& z,Vector& dR);

void MomentDerivative(const Matrix3& R,const Vector3& z,Vector3& dm);
void MomentDerivative(const MomentRotation& m,const Vector3& z,Vector3& dm);
void MomentDerivative(const Vector3& m,const Matrix3& R,const Vector3& z,Vector3& dm);

void QuaternionDerivative(const Matrix3& R,const Vector3& z,Quaternion& dq);

bool EulerAngleDerivative(const Vector3& theta,const Vector3& z,int u,int v,int w,Vector3& dtheta);

void AngularVelocityEulerAngle(const Vector3& theta,const Vector3& dtheta,int u,int v,int w,Vector3& z);

void EulerAngleMoments(const Vector3& theta,int u,int v,int w,Matrix3& A);

Real MatrixAbsoluteAngle(const Matrix3& R);

Real MatrixAngleAboutAxis(const Matrix3& R,const Vector3& a);

Real MatrixAngleDerivative(const Matrix3& R,const Vector3& z);

void MatrixDerivative2(const Matrix3& R,const Vector3& w,const Vector3& a,Matrix3& ddR);

void ForwardDifferenceAngularVelocity(const Matrix3& R0,const Matrix3& R1,Real h,Vector3& z);

void CenteredDifferenceAngularVelocity(const Matrix3& R_1,const Matrix3& R0,const Matrix3& R1,Real h,Vector3& z);

void CenteredDifferenceAngularAccel(const Matrix3& R_1,const Matrix3& R0,const Matrix3& R1,Real h,Vector3& a);

Real AxisRotationMagnitude(const Matrix3& R,const Vector3& a);

void NormalizeRotation(Matrix3& R);


#endif