vecmath.h
Go to the documentation of this file.
1
29 #ifndef __VECMATH
30
31 #define __VECMATH
32
33 #include <boost/random/mersenne_twister.hpp>
34 #include <boost/random/uniform_01.hpp>
35 #include <boost/random/variate_generator.hpp>
36
37 #include <math.h>
38 #include <vector>
39 #include <string>
40 #include <string.h>
41 #include "config.h"
42 //#include "newmat/newmatap.h"
43
44 #define PRECISION double // todo: use template instead
45 #define PI360 6.283185307179586
46 #define PI90 1.570796326794897
50
51 namespace robotLibPbD {
52
53 class CDh;
54
57 class CVec
58 {
59  public:
60  PRECISION x, y, z, w;
61
62  CVec() { x = y = z = 0.0; w = 1.0;}
64  {
65  set(x, y, z);
66  };
67
68  void set(PRECISION x, PRECISION y, PRECISION z);
69
70  std::string toString();
73  void print() const;
77  {
78  return CVec( x*s, y*s, z*s );
79  };
81  {
82  return CVec( x/s, y/s, z/s );
83  };
86  CVec& operator = ( const CVec& v)
87  {
88  x = v.x; y = v.y; z = v.z; w = 1.0f;
89  return *this;
90  };
93  PRECISION operator [] (unsigned int i) const
94  {
95  return (&x)[i];
96  };
99  PRECISION& operator [] (unsigned int i)
100  {
101  return (&x)[i];
102  };
105  CVec operator + ( const CVec& v) const
106  {
107  return CVec( x + v.x, y + v.y, z + v.z );
108  };
109
110  CVec& operator += ( const CVec& v)
111  {
112  x += v.x;
113  y += v.y;
114  z += v.z;
115
116  return *this;
117  };
118  CVec& operator -= ( const CVec& v)
119  {
120  x -= v.x;
121  y -= v.y;
122  z -= v.z;
123
124  return *this;
125  };
127  {
128  x *= s;
129  y *= s;
130  z *= s;
131
132  return *this;
133  };
135  {
136  x /= s;
137  y /= s;
138  z /= s;
139
140  return *this;
141  };
144  CVec operator - ( const CVec& v) const
145  {
146  return CVec( x - v.x, y - v.y, z - v.z );
147  };
148
152  {
153  return CVec( -x, -y, -z );
154  };
157  PRECISION operator | (const CVec& v) const
158  {
159  return x*v.x + y*v.y + z*v.z;
160  };
163  CVec operator ^ (const CVec& v) const
164  {
165  return CVec( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x );
166  };
167
169  {
170  return sqrt(x*x+y*y+z*z);
171  };
172  void normalize()
173  {
174  PRECISION len = sqrt(x*x+y*y+z*z);
175
176  if (len > 0.001)
177  {
178  x /= len;
179  y /= len;
180  z /= len;
181  }
182  };
183 };
184
187 class CMatrix
188 {
189  public:
190  PRECISION a[16];
191  CMatrix();
192  CMatrix(
193  PRECISION a0, PRECISION a4, PRECISION a8, PRECISION a12,
194  PRECISION a1, PRECISION a5, PRECISION a9, PRECISION a13,
195  PRECISION a2, PRECISION a6, PRECISION a10, PRECISION a14,
196  PRECISION a3 = 0.0f, PRECISION a7 = 0.0f, PRECISION a11 = 0.0f, PRECISION a15 = 1.0f);
197  void set(
198  PRECISION a0, PRECISION a4, PRECISION a8, PRECISION a12,
199  PRECISION a1, PRECISION a5, PRECISION a9, PRECISION a13,
200  PRECISION a2, PRECISION a6, PRECISION a10, PRECISION a14,
201  PRECISION a3 = 0.0f, PRECISION a7 = 0.0f, PRECISION a11 = 0.0f, PRECISION a15 = 1.0f);
204  PRECISION trace();
205
208  void transpose();
209
210  void printText();
211
214  void invert();
215
218  void mul(const CMatrix &first, const CMatrix &second);
219
220  void mulNoAlloc(const CMatrix &first, const CMatrix &second);
221
222  std::string toString(bool round = false);
223
226  void print(bool round = false) const;
227
230  void setDh(const robotLibPbD::CDh &dh);
231  void arrayToMatrix(std::vector<double> &values);
232  void matrixToArray(std::vector<double> &values);
233
234  void randomOrientation(double max = 2.0 * M_PI);
237  CMatrix operator * ( PRECISION f) const ;
240  CMatrix operator + ( const CMatrix &b) const ;
241
244  CMatrix operator - ( const CMatrix &b) const ;
247  CMatrix operator * ( const CMatrix &m) const ;
250  CMatrix& operator += ( const CVec &v);
253  CVec operator * ( const CVec &v) const ;
254
257  const CVec& operator [] ( int i ) const;
258
261  CVec& operator [] ( int i );
262
265  CMatrix& operator = ( const CMatrix& v);
266
267  PRECISION length() const;
268
269  void unity();
270 };
271
272
275 class CMathLib
276 {
277  public:
280  static double calcDotProduct(double firstVector[], double secondVector[], int size);
285  static void calcMatrixResult(double matrix[], double vector[], int rows, int columns, double resultVector[]);
288  static void getRotationFromMatrix(const CMatrix &mat, CVec &axis, double &angle);
289  static void getQuaternionFromRotation(CVec &quater, CVec &axis, double angle);
296  static void transposeMatrix(double *matrix, double *resultMatrix, int size);
301  static void multiplyMatrices(double *firstMatrix, double *secondMatrix, int firstRows, int firstColumns, int secondColumns, double *resultMatrix);
306  static void quaternionFromMatrix(const CMatrix &mat, CVec &quaternion);
309  static void matrixFromQuaternion(CVec &quaternion, CMatrix &matrix);
312  static void getMatrixFromRotation(CMatrix &mat, const CVec &axis, double angle);
315  static void getEulerZXZ(CMatrix &mat, CVec &first);
318  static void getOrientation(CMatrix &mat, CVec &first, CVec &second, bool old = false);
323  static void getRotation(CMatrix &mat, CVec &vec, bool old = false);
326  static void getRotation(CMatrix &mat, double x, double y, double z, bool old = false);
336  static void calcAngles(double leg1, double leg2, double x, double y, double &first, double &second);
337 };
338
339
340 double sign(double value);
341
342 double vectorlength(std::vector<double> &v);
343 unsigned long fac(unsigned int value);
344 int roundToInt(double value);
345
346 void getMeanFromVectors(std::vector<std::vector<double> > &values, std::vector<double> &mean);
347
348 double getSigmoid(double value, double offset, double factor);
349 double getGaussian(double value, double mean, double std);
350 double getGaussianWithoutNormalization(double value, double mean, double std);
351  double getUniform();
352  int getUniform(int min, int max);
353  void setUniformSeed(unsigned int seed);
354 double getGaussianProb(double mean, double std);
355 double getLogisticProb(double alpha, double beta);
356 void calculateTransformationFromPlane(CVec &plane, CMatrix &transformation);
357 void convertMatrix(CMatrix &from, double (*R)[3], double *T);
358 void convertMatrix(double (*R)[3], double *T, CMatrix &to);
359
360 void orientation2scaledAxis(CMatrix &matrix, CVec &axis);
361 void orientation2scaledAxis(CMatrix &matrix, std::vector<double> &axis);
362 void scaledAxis2Orientation(CVec &axis, CMatrix &matrix);
363 void scaledAxis2Orientation(std::vector<double> &axis, CMatrix &matrix);
364
365 void rosenbrock(int n, double *x, double *bl, double *bu,
366  double bigbnd, int maxiter, double eps, int verbose,
367  void obj(int,double *,double *,void *), void *extraparams);
368 void simulatedannealing(int n, double *x, double *bl, double *bu,
369  double bigbnd, int maxiter, double eps, int verbose,
370  void obj(int,double *,double *,void *), void *extraparams);
371
374 int rnd(PRECISION value);
375
376
377
378 // calculates matrix product: this = first * second
379 //not threadsafe if global variable is used
380 inline void CMatrix::mul(const CMatrix &first, const CMatrix &second)
381 {
382  // optimize it!!
383  PRECISION tmpCMatrixMul[16];
384  tmpCMatrixMul[0] = first.a[0] * second.a[0] + first.a[4] * second.a[1] + first.a[8] * second.a[2];
385  tmpCMatrixMul[1] = first.a[1] * second.a[0] + first.a[5] * second.a[1] + first.a[9] * second.a[2];
386  tmpCMatrixMul[2] = first.a[2] * second.a[0] + first.a[6] * second.a[1] + first.a[10] * second.a[2];
387  tmpCMatrixMul[3] = 0.0;
388  tmpCMatrixMul[4] = first.a[0] * second.a[4] + first.a[4] * second.a[5] + first.a[8] * second.a[6];
389  tmpCMatrixMul[5] = first.a[1] * second.a[4] + first.a[5] * second.a[5] + first.a[9] * second.a[6];
390  tmpCMatrixMul[6] = first.a[2] * second.a[4] + first.a[6] * second.a[5] + first.a[10] * second.a[6];
391  tmpCMatrixMul[7] = 0.0;
392  tmpCMatrixMul[8] = first.a[0] * second.a[8] + first.a[4] * second.a[9] + first.a[8] * second.a[10];
393  tmpCMatrixMul[9] = first.a[1] * second.a[8] + first.a[5] * second.a[9] + first.a[9] * second.a[10];
394  tmpCMatrixMul[10] = first.a[2] * second.a[8] + first.a[6] * second.a[9] + first.a[10] * second.a[10];
395  tmpCMatrixMul[11] = 0.0;
396  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];
397  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];
398  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];
399  tmpCMatrixMul[15] = first.a[15] * second.a[15];
400  memcpy(a, tmpCMatrixMul, 16*sizeof(PRECISION));
401 };
402
403
404 // calculates matrix product: this = first * second
405 inline void CMatrix::mulNoAlloc(const CMatrix &first, const CMatrix &second)
406 {
407  a[0] = first.a[0] * second.a[0] + first.a[4] * second.a[1] + first.a[8] * second.a[2];
408  a[1] = first.a[1] * second.a[0] + first.a[5] * second.a[1] + first.a[9] * second.a[2];
409  a[2] = first.a[2] * second.a[0] + first.a[6] * second.a[1] + first.a[10] * second.a[2];
410  a[3] = 0.0;
411  a[4] = first.a[0] * second.a[4] + first.a[4] * second.a[5] + first.a[8] * second.a[6];
412  a[5] = first.a[1] * second.a[4] + first.a[5] * second.a[5] + first.a[9] * second.a[6];
413  a[6] = first.a[2] * second.a[4] + first.a[6] * second.a[5] + first.a[10] * second.a[6];
414  a[7] = 0.0;
415  a[8] = first.a[0] * second.a[8] + first.a[4] * second.a[9] + first.a[8] * second.a[10];
416  a[9] = first.a[1] * second.a[8] + first.a[5] * second.a[9] + first.a[9] * second.a[10];
417  a[10] = first.a[2] * second.a[8] + first.a[6] * second.a[9] + first.a[10] * second.a[10];
418  a[11] = 0.0;
419  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];
420  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];
421  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];
422  a[15] = first.a[15] * second.a[15];
423 };
424
425 extern boost::mt19937 pRngMt19937;
426 extern boost::uniform_01<double> pRngDist;
427 extern boost::variate_generator<boost::mt19937&, boost::uniform_01<double> > pRngUniform01;
428
429 inline double getUniform()
430 {
431  return pRngUniform01();
432  //return ((double) (rand() % (RAND_MAX-1))) / (double) RAND_MAX;
433 }
434
435 inline int getUniform(int min, int max)
436 {
437  return min + (int) (((double)(max - min)) * pRngUniform01() + 0.5);
438 }
439
440 inline double getGaussianProb(double mean, double std)
441 {
442  double u1, u2, v, x;
443
444  do
445  {
446  u1 = pRngUniform01();
447  u2 = pRngUniform01();
448
449  v = (2.0*u1 - 1.0)*(2.0*u1 - 1.0) + (2.0*u2 - 1.0)*(2.0*u2 - 1.0);
450  } while (v >= 1.0);
451
452  x = (2.0*u1 -1.0)*sqrt(-2.0*log(v) / v);
453
454  return std*x + mean;
455 }
456
457
458 #define SWAPELEMENTS(x,y) tmp = a[x];a[x]=a[y];a[y] = tmp;
459
460 // invert homogenous transformation matrix
461 inline void CMatrix::invert()
462 {
463  /*CVec vec, res;
464  vec.set(-a[12], -a[13], -a[14]);
465  SWAPELEMENTS(1,4);
466  SWAPELEMENTS(2,8);
467  SWAPELEMENTS(6,9);
468  a[12] = a[13] = a[14] = 0.0;
469  res = (*this) * vec;
470  a[12] = res.x;
471  a[13] = res.y;
472  a[14] = res.z;
473  */
474
475  PRECISION tmp;
476  PRECISION x = -a[12];
477  PRECISION y = -a[13];
478  PRECISION z = -a[14];
479
480  SWAPELEMENTS(1,4);
481  SWAPELEMENTS(2,8);
482  SWAPELEMENTS(6,9);
483  a[12] = a[0] * x + a[4] * y + a[8] * z;
484  a[13] = a[1] * x + a[5] * y + a[9] * z;
485  a[14] = a[2] * x + a[6] * y + a[10] * z;
486 }
487 };
488
489 #endif
490
PRECISION z
Definition: vecmath.h:60
Mathematical functions.
Definition: vecmath.h:275
void calculateTransformationFromPlane(CVec &plane, CMatrix &transformation)
Definition: vecmath.cpp:1011
Definition: frame.h:241
PRECISION length() const
Definition: vecmath.h:168
boost::uniform_01< double > pRngDist
Definition: vecmath.cpp:45
CVec operator*(PRECISION s) const
Scalar multiplication.
Definition: vecmath.h:76
CVec operator+(const CVec &v) const
Definition: vecmath.h:105
double getSigmoid(double value, double offset, double factor)
Definition: vecmath.cpp:1066
std::string toString()
Definition: vecmath.cpp:65
CVec & operator/=(PRECISION s)
Definition: vecmath.h:134
void mul(const CMatrix &first, const CMatrix &second)
Assigns product of two matrices to matrix.
Definition: vecmath.h:380
void getMeanFromVectors(std::vector< std::vector< double > > &values, std::vector< double > &mean)
Definition: vecmath.cpp:1139
CVec(PRECISION x, PRECISION y, PRECISION z)
Definition: vecmath.h:63
PRECISION a[16]
Definition: vecmath.h:190
double getLogisticProb(double alpha, double beta)
Definition: vecmath.cpp:1085
double getGaussian(double value, double mean, double std)
Definition: vecmath.cpp:1071
CVec operator^(const CVec &v) const
Cross product.
Definition: vecmath.h:163
boost::mt19937 pRngMt19937
Definition: vecmath.cpp:44
void mulNoAlloc(const CMatrix &first, const CMatrix &second)
Definition: vecmath.h:405
CVec & operator+=(const CVec &v)
Definition: vecmath.h:110
PRECISION w
Definition: vecmath.h:60
#define SWAPELEMENTS(x, y)
Definition: vecmath.h:458
CVec operator/(PRECISION s) const
Definition: vecmath.h:80
double getGaussianProb(double mean, double std)
Definition: vecmath.h:440
void print() const
Prints vector as (x,y,z) to console.
Definition: vecmath.cpp:60
void setUniformSeed(unsigned int seed)
Definition: vecmath.cpp:1059
Homogenous vector.
Definition: vecmath.h:57
Homogenous matrix.
Definition: vecmath.h:187
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)
Definition: vecmath.cpp:804
double getGaussianWithoutNormalization(double value, double mean, double std)
Definition: vecmath.cpp:1078
PRECISION operator[](unsigned int i) const
Definition: vecmath.h:93
int rnd(PRECISION value)
Rounds a value.
Definition: vecmath.cpp:308
unsigned long fac(unsigned int value)
double getUniform()
Definition: vecmath.h:429
boost::variate_generator< boost::mt19937 &, boost::uniform_01< double > > pRngUniform01
CVec operator-() const
Vector negation.
Definition: vecmath.h:151
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)
Definition: vecmath.cpp:871
void normalize()
Definition: vecmath.h:172
void orientation2scaledAxis(CMatrix &matrix, CVec &axis)
Definition: vecmath.cpp:1162
void matrixToArray(const CMatrix &matrix, std::vector< double > &values)
Definition: utils.cpp:176
PRECISION operator|(const CVec &v) const
Dot product.
Definition: vecmath.h:157
CVec & operator*=(PRECISION s)
Definition: vecmath.h:126
PRECISION y
Definition: vecmath.h:60
void scaledAxis2Orientation(CVec &axis, CMatrix &matrix)
Definition: vecmath.cpp:1180
#define PRECISION
Definition: vecmath.h:44
CVec & operator-=(const CVec &v)
Definition: vecmath.h:118
CVec & operator=(const CVec &v)
Assigns values of vector v to vector.
Definition: vecmath.h:86
double vectorlength(std::vector< double > &v)
Definition: vecmath.cpp:791
void invert()
Inverts the matrix (only for homogenous matrices)
Definition: vecmath.h:461
PRECISION x
Definition: vecmath.h:60
void arrayToMatrix(CMatrix &matrix, std::vector< double > &values)
Definition: utils.cpp:195
void convertMatrix(CMatrix &from, double(*R)[3], double *T)
Definition: vecmath.cpp:1119
double sign(double value)
int roundToInt(double value)

asr_kinematic_chain_optimizer
Author(s): Aumann Florian, Heller Florian, Jäkel Rainer, Wittenbeck Valerij
autogenerated on Mon Jun 10 2019 12:35:36