robodktypes.h

#ifndef ROBODKTYPES_H
#define ROBODKTYPES_H
 
 
#include <QString>
#include <QtGui/QMatrix4x4>
#include <QDebug>
 
#include "matrix4x4.h"
#include "legacymatrix2d.h"
#include "joints.h"
 
 
#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;
};
 
 
 
typedef robodk::legacy::Matrix2D tMatrix2D;
 
inline tMatrix2D* Matrix2D_Create()
{
    return robodk::legacy::Matrix2D_Create();
}
 
inline void Matrix2D_Delete(tMatrix2D** matrix)
{
    robodk::legacy::Matrix2D_Delete(matrix);
}
 
inline void Matrix2D_Set_Size(tMatrix2D* matrix, int rows, int columns)
{
    robodk::legacy::Matrix2D_SetDimensions(matrix, rows, columns);
}
 
inline int Matrix2D_Size(const tMatrix2D* matrix, int dimension)
{
    return robodk::legacy::Matrix2D_GetDimension(matrix, dimension);
}
 
inline int Matrix2D_Get_ncols(const tMatrix2D* matrix)
{
    return robodk::legacy::Matrix2D_ColumnCount(matrix);
}
 
inline int Matrix2D_Get_nrows(const tMatrix2D* matrix)
{
    return robodk::legacy::Matrix2D_RowCount(matrix);
}
 
inline double Matrix2D_Get_ij(const tMatrix2D* matrix, int row, int column)
{
    return robodk::legacy::Matrix2D_Get(matrix, row, column);
}
 
inline void Matrix2D_Set_ij(const tMatrix2D* matrix, int row, int column, double value)
{
    robodk::legacy::Matrix2D_Set(matrix, row, column, value);
}
 
inline double* Matrix2D_Get_col(const tMatrix2D* matrix, int column)
{
    return robodk::legacy::Matrix2D_GetColumn(matrix, column);
}
 
inline bool Matrix2D_Copy(const tMatrix2D* source, tMatrix2D* destination)
{
    return robodk::legacy::Matrix2D_Copy(source, destination);
}
 
inline void Debug_Array(const double* array, int size)
{
    robodk::legacy::Matrix2D_DebugArray(array, size);
}
 
inline void Debug_Matrix2D(const tMatrix2D* matrix)
{
    robodk::legacy::Matrix2D_Debug(matrix);
}
 
inline void Matrix2D_Save(QTextStream* stream, tMatrix2D* matrix, bool csv = false)
{
    robodk::legacy::Matrix2D_Save(stream, matrix, csv);
}
 
inline void Matrix2D_Save(QDataStream* stream, tMatrix2D* matrix)
{
    robodk::legacy::Matrix2D_Save(stream, matrix);
}
 
inline void Matrix2D_Load(QDataStream* stream, tMatrix2D** matrix)
{
    robodk::legacy::Matrix2D_Load(stream, matrix);
}
 
 
typedef robodk::Joints tJoints;
typedef robodk::Matrix4x4 Mat;
 
inline Mat transl(double x, double y, double z)
{
    return Mat::transl(x,y,z);
}
 
inline Mat rotx(double rx)
{
    return Mat::rotx(rx);
}
 
inline Mat roty(double ry)
{
    return Mat::roty(ry);
}
 
inline Mat rotz(double rz)
{
    return Mat::rotz(rz);
}
 
 
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