robodktypes.h

#ifndef ROBODKTYPES_H
#define ROBODKTYPES_H
 
 
#include <QString>
#include <QtGui/QMatrix4x4>
#include <QDebug>
 
 
#ifndef M_PI
#define M_PI 3.14159265358979323846264338327950288
#endif
 
 
class IItem;
class IRoboDK;
typedef IItem* Item;
typedef IRoboDK RoboDK;
 
 
 
 
#define RDK_SIZE_JOINTS_MAX 12
// IMPORTANT!! Do not change this value
 
#define RDK_SIZE_MAX_CONFIG 4
// IMPORTANT!! Do not change this value
 
 
typedef double tXYZWPR[6];
 
typedef double tXYZ[3];
 
 
typedef double tConfig[RDK_SIZE_MAX_CONFIG];
 
 
 
#define DOT(v,q)   ((v)[0]*(q)[0] + (v)[1]*(q)[1] + (v)[2]*(q)[2])
 
#define NORM(v)   (sqrt((v)[0]*(v)[0] + (v)[1]*(v)[1] + (v)[2]*(v)[2]))
 
#define NORMALIZE(inout){\
    double norm;\
    norm = sqrt((inout)[0]*(inout)[0] + (inout)[1]*(inout)[1] + (inout)[2]*(inout)[2]);\
    (inout)[0] = (inout)[0]/norm;\
    (inout)[1] = (inout)[1]/norm;\
    (inout)[2] = (inout)[2]/norm;}
 
#define CROSS(out,a,b) \
    (out)[0] = (a)[1]*(b)[2] - (b)[1]*(a)[2]; \
    (out)[1] = (a)[2]*(b)[0] - (b)[2]*(a)[0]; \
    (out)[2] = (a)[0]*(b)[1] - (b)[0]*(a)[1];
 
#define COPY3(out,in)\
    (out)[0]=(in)[0];\
    (out)[1]=(in)[1];\
    (out)[2]=(in)[2];
 
#define MULT_MAT(out,inA,inB)\
    (out)[0] = (inA)[0]*(inB)[0] + (inA)[4]*(inB)[1] + (inA)[8]*(inB)[2];\
    (out)[1] = (inA)[1]*(inB)[0] + (inA)[5]*(inB)[1] + (inA)[9]*(inB)[2];\
    (out)[2] = (inA)[2]*(inB)[0] + (inA)[6]*(inB)[1] + (inA)[10]*(inB)[2];\
    (out)[3] = 0;\
    (out)[4] = (inA)[0]*(inB)[4] + (inA)[4]*(inB)[5] + (inA)[8]*(inB)[6];\
    (out)[5] = (inA)[1]*(inB)[4] + (inA)[5]*(inB)[5] + (inA)[9]*(inB)[6];\
    (out)[6] = (inA)[2]*(inB)[4] + (inA)[6]*(inB)[5] + (inA)[10]*(inB)[6];\
    (out)[7] = 0;\
    (out)[8] = (inA)[0]*(inB)[8] + (inA)[4]*(inB)[9] + (inA)[8]*(inB)[10];\
    (out)[9] = (inA)[1]*(inB)[8] + (inA)[5]*(inB)[9] + (inA)[9]*(inB)[10];\
    (out)[10] = (inA)[2]*(inB)[8] + (inA)[6]*(inB)[9] + (inA)[10]*(inB)[10];\
    (out)[11] = 0;\
    (out)[12] = (inA)[0]*(inB)[12] + (inA)[4]*(inB)[13] + (inA)[8]*(inB)[14] + (inA)[12];\
    (out)[13] = (inA)[1]*(inB)[12] + (inA)[5]*(inB)[13] + (inA)[9]*(inB)[14] + (inA)[13];\
    (out)[14] = (inA)[2]*(inB)[12] + (inA)[6]*(inB)[13] + (inA)[10]*(inB)[14] + (inA)[14];\
    (out)[15] = 1;
 
#define MULT_MAT_VECTOR(out,H,p)\
    (out)[0] = (H)[0]*(p)[0] + (H)[4]*(p)[1] + (H)[8]*(p)[2];\
    (out)[1] = (H)[1]*(p)[0] + (H)[5]*(p)[1] + (H)[9]*(p)[2];\
    (out)[2] = (H)[2]*(p)[0] + (H)[6]*(p)[1] + (H)[10]*(p)[2];
 
#define MULT_MAT_POINT(out,H,p)\
    (out)[0] = (H)[0]*(p)[0] + (H)[4]*(p)[1] + (H)[8]*(p)[2] + (H)[12];\
    (out)[1] = (H)[1]*(p)[0] + (H)[5]*(p)[1] + (H)[9]*(p)[2] + (H)[13];\
    (out)[2] = (H)[2]*(p)[0] + (H)[6]*(p)[1] + (H)[10]*(p)[2] + (H)[14];
 
    
 
struct tColor{
    float r;
 
    float g;
 
    float b;
 
    float a;
};
 
 
 
 
 
 
//------------------------------------------------------------------------------------------------------------
 
 
 
struct tMatrix2D {
    double *data;
 
    int *size;
 
    int allocatedSize;
 
    int numDimensions;
 
    bool canFreeData;
};
 
 
 
 
// -------------------------------------------
 
 
tMatrix2D* Matrix2D_Create();
 
void Matrix2D_Delete(tMatrix2D **mat);
 
void Matrix2D_Set_Size(tMatrix2D *mat, int rows, int cols);
 
int Matrix2D_Size(const tMatrix2D *mat, int dim);
 
int Matrix2D_Get_ncols(const tMatrix2D *var);
 
int Matrix2D_Get_nrows(const tMatrix2D *var);
 
double Matrix2D_Get_ij(const tMatrix2D *var, int i, int j);
 
void Matrix2D_Set_ij(const tMatrix2D *var, int i, int j, double value);
 
double* Matrix2D_Get_col(const tMatrix2D *var, int col);
 
bool Matrix2D_Copy(const tMatrix2D *in, tMatrix2D *out);
 
void Debug_Array(const double *array, int arraysize);
 
void Debug_Matrix2D(const tMatrix2D *mat);
 
void Matrix2D_Save(QDataStream *st, tMatrix2D *emx);
 
void Matrix2D_Save(QTextStream *st, tMatrix2D *emx, bool csv=false);
 
void Matrix2D_Load(QDataStream *st, tMatrix2D **emx);
 
 
//--------------------- Joints class -----------------------
 
class tJoints {
 
public:
    tJoints(int ndofs = 0);
 
    tJoints(const double *joints, int ndofs = 0);
 
    tJoints(const float *joints, int ndofs = 0);
 
    tJoints(const tJoints &jnts);
 
    tJoints(const tMatrix2D *mat2d, int column=0, int ndofs=-1);
 
    tJoints(const QString &str);
    
    operator QString() const { return ToString(); }
 
    const double *ValuesD() const;
 
    const float *ValuesF() const;
 
#ifdef ROBODK_API_FLOATS
    const float *Values() const;
#else
    const double *Values() const;
#endif
 
 
 
    double Compare(const tJoints &other) const;
 
    double *Data();
 
    int Length() const;
    
    void setLength(int new_length);
 
    bool Valid();
 
    int GetValues(double *joints);
 
    void SetValues(const double *joints, int ndofs = -1);
 
    void SetValues(const float *joints, int ndofs = -1);
 
    QString ToString(const QString &separator=", ", int precision = 3) const ;
 
    bool FromString(const QString &str);
 
 
public:
    int _nDOFs;
 
    double _Values[RDK_SIZE_JOINTS_MAX];
 
    float _ValuesF[RDK_SIZE_JOINTS_MAX];
};
 
 
 
 
class Mat : public QMatrix4x4 {
 
public:
 
    Mat();
 
    Mat(bool valid);
 
    Mat(const QMatrix4x4 &matrix);
 
    Mat(const Mat &matrix);
 
    Mat(double nx, double ox, double ax, double tx, double ny, double oy, double ay, double ty, double nz, double oz, double az, double tz);
 
    Mat(const double values[16]);
 
    Mat(const float values[16]);
 
    Mat(double x, double y, double z);
 
    ~Mat();
    
    operator QString() const { return ToString(); }
 
    void setVX(double x, double y, double z);
 
    void setVY(double x, double y, double z);
 
    void setVZ(double x, double y, double z);
 
    void setPos(double x, double y, double z);
 
    void setVX(double xyz[3]);
 
    void setVY(double xyz[3]);
 
    void setVZ(double xyz[3]);
 
    void setPos(double xyz[3]);
 
    void setValues(double pose[16]);
 
    void VX(tXYZ xyz) const;
 
    void VY(tXYZ xyz) const;
 
    void VZ(tXYZ xyz) const;
 
    void Pos(tXYZ xyz) const;    
    
    void Set(int r, int c, double value);
 
    double Get(int r, int c) const;
 
    Mat inv() const;
    
    bool isHomogeneous() const;
 
    bool MakeHomogeneous();
 
    //------ Pose to xyzrpw and xyzrpw to pose------------
 
    void ToXYZRPW(tXYZWPR xyzwpr) const;
 
    QString ToString(const QString &separator=", ", int precision = 3, bool xyzrpw_only = false) const;
 
    bool FromString(const QString &str);
 
    void FromXYZRPW(tXYZWPR xyzwpr);
 
    static Mat XYZRPW_2_Mat(double x, double y, double z, double r, double p, double w);
    static Mat XYZRPW_2_Mat(tXYZWPR xyzwpr);
 
    const double* ValuesD() const;
 
    const float* ValuesF() const;
 
#ifdef ROBODK_API_FLOATS
    const float* Values() const;
#else
    const double* Values() const;
#endif
 
    void Values(double values[16]) const;
 
    void Values(float values[16]) const;
 
    bool Valid() const;
 
    static Mat transl(double x, double y, double z);
 
    static Mat rotx(double rx);
 
    static Mat roty(double ry);
 
    static Mat rotz(double rz);
 
private:
    double _valid;
 
// this is a dummy variable to easily obtain a pointer to a 16-double-array for matrix multiplications
private:
    double _ddata16[16];
 
};
 
Mat transl(double x, double y, double z);
 
Mat rotx(double rx);
 
Mat roty(double ry);
 
Mat rotz(double rz);
 
//QDataStream &operator<<(QDataStream &data, const QMatrix4x4 &);
inline QDebug operator<<(QDebug dbg, const Mat &m){ return dbg.noquote() << m.ToString(); }
inline QDebug operator<<(QDebug dbg, const tJoints &jnts){ return dbg.noquote() << jnts.ToString(); }
 
inline QDebug operator<<(QDebug dbg, const Mat *m){ return dbg.noquote() << (m == nullptr ? "Mat(null)" : m->ToString()); }
inline QDebug operator<<(QDebug dbg, const tJoints *jnts){ return dbg.noquote() << (jnts == nullptr ? "tJoints(null)" : jnts->ToString()); }
 
 
 
#endif // ROBODKTYPES_H