vecmath.h

Go to the documentation of this file.

#ifndef __VECMATH

#define __VECMATH

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>

#include <math.h>
#include <vector>
#include <string>
#include <string.h>
#include "config.h"
//#include "newmat/newmatap.h"

#define PRECISION double // todo: use template instead
#define PI360 6.283185307179586
#define PI90 1.570796326794897 
#define RAD2ANGLE(X) ((X)*57.295779513082322)
#define ANGLE2RAD(X) ((X)*1.745329251994e-02)
#define RAD2ANGLEMAP(X) (X > M_PI ? RAD2ANGLE(PI360 - X) : RAD2ANGLE(X))

namespace robotLibPbD {

class CDh;

class CVec 
{       
        public:
        PRECISION x, y, z, w;
        
        CVec() { x = y = z = 0.0; w = 1.0;}
        CVec(PRECISION x, PRECISION y, PRECISION z)
        {
            set(x, y, z);
        };
        
        void set(PRECISION x, PRECISION y, PRECISION z);
        
        std::string toString();
        void print() const; 
        CVec operator * ( PRECISION s) const
        {
            return CVec( x*s, y*s, z*s );
        }; 
        CVec operator / ( PRECISION s) const
        {
            return CVec( x/s, y/s, z/s );
        }; 
        CVec& operator = ( const CVec& v)
        {
            x = v.x; y = v.y; z = v.z; w = 1.0f;
            return *this;
        };
        PRECISION operator [] (unsigned int i) const
        {
            return (&x)[i];
        };
        PRECISION& operator [] (unsigned int i)
        {
            return (&x)[i];
        };
        CVec operator + ( const CVec& v) const
        {
            return CVec( x + v.x, y + v.y, z + v.z );
        };
        
        CVec& operator += ( const CVec& v)
        {
            x += v.x;
            y += v.y;
            z += v.z;
    
            return *this;
        };
        CVec& operator -= ( const CVec& v)
        {
            x -= v.x;
            y -= v.y;
            z -= v.z;
    
            return *this;
        };
        CVec& operator *= ( PRECISION s)
        {
            x *= s;
            y *= s;
            z *= s;
    
            return *this;
        };
        CVec& operator /= ( PRECISION s)
        {
            x /= s;
            y /= s;
            z /= s;
    
            return *this;
        };
        CVec operator - ( const CVec& v) const
        {
            return CVec( x - v.x, y - v.y, z - v.z );
        };
     
        CVec operator - () const
        {
            return CVec( -x, -y, -z );
        };
        PRECISION operator | (const CVec& v) const
        {
            return x*v.x + y*v.y + z*v.z;
        };
        CVec operator ^ (const CVec& v) const
        {
            return CVec( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x );
        }; 
        
        PRECISION length() const
        {
            return sqrt(x*x+y*y+z*z);
        };
        void normalize()
        {
            PRECISION len = sqrt(x*x+y*y+z*z);
     
            if (len > 0.001)
            {
                x /= len;   
                y /= len;
                z /= len;
            }
        };
};

class CMatrix
{
        public:
        PRECISION a[16];
        CMatrix();
        CMatrix(
                                                        PRECISION a0, PRECISION a4, PRECISION a8,  PRECISION a12,
                                                        PRECISION a1, PRECISION a5, PRECISION a9,  PRECISION a13,
                                                        PRECISION a2, PRECISION a6, PRECISION a10, PRECISION a14,
                                                        PRECISION a3 = 0.0f, PRECISION a7 = 0.0f, PRECISION a11 = 0.0f, PRECISION a15 = 1.0f);
        void set(
                                                        PRECISION a0, PRECISION a4, PRECISION a8,  PRECISION a12,
                                                        PRECISION a1, PRECISION a5, PRECISION a9,  PRECISION a13,
                                                        PRECISION a2, PRECISION a6, PRECISION a10, PRECISION a14,
                                                        PRECISION a3 = 0.0f, PRECISION a7 = 0.0f, PRECISION a11 = 0.0f, PRECISION a15 = 1.0f);
        PRECISION trace();
        
        void transpose();
       
        void printText();
        
        void invert();  
        
    void mul(const CMatrix &first, const CMatrix &second);

    void mulNoAlloc(const CMatrix &first, const CMatrix &second);
     
    std::string toString(bool round = false);
    
        void print(bool round = false) const;
        
        void setDh(const robotLibPbD::CDh &dh);
        void arrayToMatrix(std::vector<double> &values);
        void matrixToArray(std::vector<double> &values);

        void randomOrientation(double max = 2.0 * M_PI);
        CMatrix operator * ( PRECISION f) const ;
        CMatrix operator + ( const CMatrix &b) const ; 
        
        CMatrix operator - ( const CMatrix &b) const ; 
        CMatrix operator * ( const CMatrix &m) const ; 
        CMatrix& operator += ( const CVec &v);
        CVec operator * ( const CVec &v) const ;
        
        const CVec& operator [] ( int i ) const;

        CVec& operator [] ( int i );
        
        CMatrix& operator = ( const CMatrix& v);
        
        PRECISION length() const; 

        void unity();
};


class CMathLib
{
    public:        
    static double calcDotProduct(double firstVector[], double secondVector[], int size);
    static void calcMatrixResult(double matrix[], double vector[], int rows, int columns, double resultVector[]);
    static void getRotationFromMatrix(const CMatrix &mat, CVec &axis, double &angle);
    static void getQuaternionFromRotation(CVec &quater, CVec &axis, double angle);
    static void transposeMatrix(double *matrix, double *resultMatrix, int size);
    static void multiplyMatrices(double *firstMatrix, double *secondMatrix, int firstRows, int firstColumns, int secondColumns, double *resultMatrix);
    static void quaternionFromMatrix(const CMatrix &mat, CVec &quaternion);
    static void matrixFromQuaternion(CVec &quaternion, CMatrix &matrix);
    static void getMatrixFromRotation(CMatrix &mat, const CVec &axis, double angle);
    static void getEulerZXZ(CMatrix &mat, CVec &first);
    static void getOrientation(CMatrix &mat, CVec &first, CVec &second, bool old = false);
    static void getRotation(CMatrix &mat, CVec &vec, bool old = false);
    static void getRotation(CMatrix &mat, double x, double y, double z, bool old = false);
    static void calcAngles(double leg1, double leg2, double x, double y, double &first, double &second);
};


double sign(double value);

double vectorlength(std::vector<double> &v);
unsigned long fac(unsigned int value);
int roundToInt(double value);

void getMeanFromVectors(std::vector<std::vector<double> > &values, std::vector<double> &mean);

double getSigmoid(double value, double offset, double factor);
double getGaussian(double value, double mean, double std);
double getGaussianWithoutNormalization(double value, double mean, double std);
 double getUniform();
 int getUniform(int min, int max);
 void setUniformSeed(unsigned int seed);
double getGaussianProb(double mean, double std);
double getLogisticProb(double alpha, double beta);
void calculateTransformationFromPlane(CVec &plane, CMatrix &transformation);
void convertMatrix(CMatrix &from, double (*R)[3], double *T);
void convertMatrix(double (*R)[3], double *T, CMatrix &to);

void orientation2scaledAxis(CMatrix &matrix, CVec &axis);
void orientation2scaledAxis(CMatrix &matrix, std::vector<double> &axis);
void scaledAxis2Orientation(CVec &axis, CMatrix &matrix);
void scaledAxis2Orientation(std::vector<double> &axis, CMatrix &matrix);

void rosenbrock(int n, double *x, double *bl, double *bu,
                double bigbnd, int maxiter, double eps, int verbose,
                void obj(int,double *,double *,void *), void *extraparams);
void simulatedannealing(int n, double *x, double *bl, double *bu,
                double bigbnd, int maxiter, double eps, int verbose,
                void obj(int,double *,double *,void *), void *extraparams);

int rnd(PRECISION value);


   
// calculates matrix product: this = first * second
//not threadsafe if global variable is used     
inline void CMatrix::mul(const CMatrix &first, const CMatrix &second)
{                 
  // optimize it!!
  PRECISION tmpCMatrixMul[16];
  tmpCMatrixMul[0]  = first.a[0] * second.a[0] + first.a[4] * second.a[1] + first.a[8] * second.a[2];
  tmpCMatrixMul[1]  = first.a[1] * second.a[0] + first.a[5] * second.a[1] + first.a[9] * second.a[2];
  tmpCMatrixMul[2]  = first.a[2] * second.a[0] + first.a[6] * second.a[1] + first.a[10] * second.a[2];
  tmpCMatrixMul[3]  = 0.0;
  tmpCMatrixMul[4]  = first.a[0] * second.a[4] + first.a[4] * second.a[5] + first.a[8] * second.a[6];
  tmpCMatrixMul[5]  = first.a[1] * second.a[4] + first.a[5] * second.a[5] + first.a[9] * second.a[6];
  tmpCMatrixMul[6]  = first.a[2] * second.a[4] + first.a[6] * second.a[5] + first.a[10] * second.a[6];
  tmpCMatrixMul[7]  = 0.0;
  tmpCMatrixMul[8]  = first.a[0] * second.a[8] + first.a[4] * second.a[9] + first.a[8] * second.a[10];
  tmpCMatrixMul[9]  = first.a[1] * second.a[8] + first.a[5] * second.a[9] + first.a[9] * second.a[10];
  tmpCMatrixMul[10] = first.a[2] * second.a[8] + first.a[6] * second.a[9] + first.a[10] * second.a[10];
  tmpCMatrixMul[11] = 0.0;
  tmpCMatrixMul[12]  = first.a[0] * second.a[12] + first.a[4] * second.a[13] + first.a[8] * second.a[14] + first.a[12] * second.a[15];
  tmpCMatrixMul[13]  = first.a[1] * second.a[12] + first.a[5] * second.a[13] + first.a[9] * second.a[14] + first.a[13] * second.a[15];
  tmpCMatrixMul[14]  = first.a[2] * second.a[12] + first.a[6] * second.a[13] + first.a[10] * second.a[14] + first.a[14] * second.a[15];
  tmpCMatrixMul[15]  = first.a[15] * second.a[15];
  memcpy(a, tmpCMatrixMul, 16*sizeof(PRECISION)); 
};


// calculates matrix product: this = first * second 
inline void CMatrix::mulNoAlloc(const CMatrix &first, const CMatrix &second)
{                  
  a[0]  = first.a[0] * second.a[0] + first.a[4] * second.a[1] + first.a[8] * second.a[2];
  a[1]  = first.a[1] * second.a[0] + first.a[5] * second.a[1] + first.a[9] * second.a[2];
  a[2]  = first.a[2] * second.a[0] + first.a[6] * second.a[1] + first.a[10] * second.a[2];
  a[3]  = 0.0;
  a[4]  = first.a[0] * second.a[4] + first.a[4] * second.a[5] + first.a[8] * second.a[6];
  a[5]  = first.a[1] * second.a[4] + first.a[5] * second.a[5] + first.a[9] * second.a[6];
  a[6]  = first.a[2] * second.a[4] + first.a[6] * second.a[5] + first.a[10] * second.a[6];
  a[7]  = 0.0;
  a[8]  = first.a[0] * second.a[8] + first.a[4] * second.a[9] + first.a[8] * second.a[10];
  a[9]  = first.a[1] * second.a[8] + first.a[5] * second.a[9] + first.a[9] * second.a[10];
  a[10] = first.a[2] * second.a[8] + first.a[6] * second.a[9] + first.a[10] * second.a[10];
  a[11] = 0.0;
  a[12]  = first.a[0] * second.a[12] + first.a[4] * second.a[13] + first.a[8] * second.a[14] + first.a[12] * second.a[15];
  a[13]  = first.a[1] * second.a[12] + first.a[5] * second.a[13] + first.a[9] * second.a[14] + first.a[13] * second.a[15];
  a[14]  = first.a[2] * second.a[12] + first.a[6] * second.a[13] + first.a[10] * second.a[14] + first.a[14] * second.a[15]; 
  a[15]  = first.a[15] * second.a[15];
};

extern boost::mt19937 pRngMt19937;
extern boost::uniform_01<double> pRngDist;
extern boost::variate_generator<boost::mt19937&, boost::uniform_01<double> > pRngUniform01;

inline double getUniform()
{
  return pRngUniform01();
  //return ((double) (rand() % (RAND_MAX-1))) / (double) RAND_MAX;
}

inline int getUniform(int min, int max)
{
  return min + (int) (((double)(max - min)) * pRngUniform01() + 0.5);
}

inline double getGaussianProb(double mean, double std)
{
    double u1, u2, v, x;
    
    do 
    {
      u1 = pRngUniform01();
      u2 = pRngUniform01();
        
        v = (2.0*u1 - 1.0)*(2.0*u1 - 1.0) + (2.0*u2 - 1.0)*(2.0*u2 - 1.0);
    } while (v >= 1.0);
    
    x = (2.0*u1 -1.0)*sqrt(-2.0*log(v) / v);
    
    return std*x + mean;
}


#define SWAPELEMENTS(x,y) tmp = a[x];a[x]=a[y];a[y] = tmp;

// invert homogenous transformation matrix    
inline void CMatrix::invert()
{ 
     /*CVec  vec, res;
     vec.set(-a[12], -a[13], -a[14]);
     SWAPELEMENTS(1,4);
     SWAPELEMENTS(2,8);
     SWAPELEMENTS(6,9);
     a[12] = a[13] = a[14] = 0.0;
     res = (*this) * vec;
     a[12] = res.x;
     a[13] = res.y;
     a[14] = res.z;  
     */

  PRECISION tmp;
  PRECISION x = -a[12];
  PRECISION y = -a[13];
  PRECISION z = -a[14];
  
  SWAPELEMENTS(1,4);
  SWAPELEMENTS(2,8);
  SWAPELEMENTS(6,9); 
  a[12] = a[0] * x + a[4] * y + a[8]  * z; 
  a[13] = a[1] * x + a[5] * y + a[9]  * z; 
  a[14] = a[2] * x + a[6] * y + a[10] * z; 
}
};

#endif