#ifndef CALCULATIONS_H
#define CALCULATIONS_H

#include "GL_Matrix.h"
#include <GL/glut.h>

//    ATTENTION : There is one rather bad thing abount RPY 
// representation of a frame - each robot system(manufacture,
// country, etc) uses it's OWN TYPE !!!  Here I approach the EASIEST
// way : first three values represent X,Y,Z coordinates of
// a frame obtained by shifting identity( the same as global) frame
// along axes oX, oY, oZ. Other three values A, B and C represent
// frame obtained by rotation of resulting frame about  
// axes xx', then yy' and at last zz', which all are parallel to
// _GLOBAL_ axes and go through point (X,Y,Z), by angles A, B
// and C. If you need your own representation (Japanese or
// whichever, #:0)), PLEASE, DO NOT CONTACT ME IN THAT CASE WITH
// OBJECTIONS. If you need it - use joints or build up new
// function for RPY calculations. IT WON'T HURT YOU.  
//
//    NOTICE : another way of rotations to achieve the same results 
// is to rotate that shifted frame first about it's z-axis by C,
// then about y-axis of NEW frame by B and at last about x-axis
// of NEW frame by A value. Many people - many ways...

 

bool RPY_To_Joints( double *rpy, double *joints);
GL_Matrix Get_Robot_Frame( double* joints);
GL_Matrix RPY_To_Frame( double *rpy);
bool Frame_To_RPY( GL_Matrix& frame, double *rpy);
bool Frame_To_Joints( GL_Matrix frame, double *joints);

#endif
