vecmath.cpp
Go to the documentation of this file.
1 
28 #include <stdio.h>
29 #include <math.h>
30 #include <iostream>
31 
32 
33 #include "frame.h"
34 #include "vecmath.h"
35 
36 #define SWAP(x,y) tmp = a[x];a[x]=a[y];a[y] = tmp;
37 #define ROUND(x) rnd(x*10)/10.0)
38 
39 using namespace std;
40 
41 namespace robotLibPbD {
42 
43 
44 boost::mt19937 pRngMt19937;
45 boost::uniform_01<double> pRngDist;
46 boost::variate_generator<boost::mt19937&, boost::uniform_01<double> > pRngUniform01(pRngMt19937, pRngDist);
47 
48 //static PRECISION tmpCMatrixMul[16];
49 
50 // sets vector to (x y z 1.0)
51 void CVec::set(PRECISION x, PRECISION y, PRECISION z)
52 {
53  this->x = x;
54  this->y = y;
55  this->z = z;
56  this->w = 1.0;
57 }
58 
59 // prints vector value
60 void CVec::print() const
61 {
62  printf("%f %f %f", x, y, z);
63 }
64 
65 std::string CVec::toString()
66 {
67  char str[255];
68  sprintf(str, "%f %f %f", x, y, z);
69  return std::string(str);
70 }
71 
72 // inits matrix with 1-Matrix
73 CMatrix::CMatrix()
74 {
75  set( 1.0, 0.0, 0.0, 0.0,
76  0.0, 1.0, 0.0, 0.0,
77  0.0, 0.0, 1.0, 0.0,
78  0.0, 0.0, 0.0, 1.0);
79 }
80 
81 void CMatrix::randomOrientation(double max)
82 {
83  CVec tmp;
84  tmp.x = getGaussianProb(0.0, 1.0);
85  tmp.y = getGaussianProb(0.0, 1.0);
86  tmp.z = getGaussianProb(0.0, 1.0);
87  tmp.normalize();
88 
89  CMathLib::getMatrixFromRotation(*this, tmp, getUniform() * max);
90 }
91 
92 void CMatrix::unity()
93 {
94  set( 1.0, 0.0, 0.0, 0.0,
95  0.0, 1.0, 0.0, 0.0,
96  0.0, 0.0, 1.0, 0.0,
97  0.0, 0.0, 0.0, 1.0);
98 }
99 
100 void CMatrix::printText()
101 {
102  double x,y,z,a,b,g;
103  x = this->a[12];
104  y = this->a[13];
105  z = this->a[14];
106 
107  CVec tmp1, tmp2;
108  CMathLib::getOrientation(*this, tmp1, tmp2);
109 
110  a = tmp1.x * 180.0/M_PI;
111  b = tmp1.y * 180.0/M_PI;
112  g = tmp1.z * 180.0/M_PI;
113 
114  printf("%g %g %g %g %g %g\n", x, y, z, a, b, g);
115 }
116 
117 // transposes matrix
118 void CMatrix::transpose()
119 {
120  PRECISION tmp;
121  SWAP(1,4);
122  SWAP(2,8);
123  SWAP(6,9);
124  SWAP(3,12);
125  SWAP(7,13);
126  SWAP(11,14);
127 }
128 
129 
130 
131 // sets matrix to supplied value
132 CMatrix::CMatrix( PRECISION a0, PRECISION a4, PRECISION a8, PRECISION a12,
133  PRECISION a1, PRECISION a5, PRECISION a9, PRECISION a13,
134  PRECISION a2, PRECISION a6, PRECISION a10, PRECISION a14,
135  PRECISION a3, PRECISION a7, PRECISION a11, PRECISION a15)
136 {
137  a[0] = a0; a[4] = a4; a[8] = a8; a[12] = a12;
138  a[1] = a1; a[5] = a5; a[9] = a9; a[13] = a13;
139  a[2] = a2; a[6] = a6; a[10] = a10; a[14] = a14;
140  a[3] = a3; a[7] = a7; a[11] = a11; a[15] = a15;
141 }
142 
143 // sets matrix to supplied value
144 void CMatrix::set( PRECISION a0, PRECISION a4, PRECISION a8, PRECISION a12,
145  PRECISION a1, PRECISION a5, PRECISION a9, PRECISION a13,
146  PRECISION a2, PRECISION a6, PRECISION a10, PRECISION a14,
147  PRECISION a3, PRECISION a7, PRECISION a11 , PRECISION a15)
148 {
149  a[0] = a0; a[4] = a4; a[8] = a8; a[12] = a12;
150  a[1] = a1; a[5] = a5; a[9] = a9; a[13] = a13;
151  a[2] = a2; a[6] = a6; a[10] = a10; a[14] = a14;
152  a[3] = a3; a[7] = a7; a[11] = a11; a[15] = a15;
153 }
154 
155 // creates denavit hartenberg transformation matrix based on CDh struct
156 void CMatrix::setDh(const CDh &dh)
157 {
158  if (dh.useAxis)
159  {
160  if (dh.rotationalDof)
161  {
162  CMathLib::getMatrixFromRotation(*this, dh.axis, dh.rot_z + dh.angle);
163  a[12] = 0.0;
164  a[13] = 0.0;
165  a[14] = 0.0;
166  } else
167  {
168  a[0] = 1.0;
169  a[1] = 0.0;
170  a[2] = 0.0;
171  a[3] = 0.0;
172  a[4] = 0.0;
173  a[5] = 1.0;
174  a[6] = 0.0;
175  a[7] = 0.0;
176  a[8] = 0.0;
177  a[9] = 0.0;
178  a[10] = 1.0;
179  a[11] = 0.0;
180  a[12] = dh.axis.x * (dh.rot_z + dh.angle);
181  a[13] = dh.axis.y * (dh.rot_z + dh.angle);
182  a[14] = dh.axis.z * (dh.rot_z + dh.angle);
183  a[15] = 1.0;
184  }
185  } else if (dh.rotationalDof)
186  {
187  a[0] = cos(dh.rot_z + dh.angle); a[4] = -sin(dh.rot_z + dh.angle)*cos(dh.rot_x); a[8] = sin(dh.rot_x)*sin(dh.rot_z + dh.angle); a[12] = dh.trans_x*cos(dh.rot_z + dh.angle);
188  a[1] = sin(dh.rot_z + dh.angle); a[5] = cos(dh.rot_x)*cos(dh.rot_z + dh.angle); a[9] = -cos(dh.rot_z + dh.angle)*sin(dh.rot_x); a[13] = dh.trans_x*sin(dh.rot_z + dh.angle);
189  a[2] = 0.0; a[6] = sin(dh.rot_x); a[10] = cos(dh.rot_x); a[14] = dh.trans_z;
190  a[3] = 0.0; a[7] = 0.0; a[11] = 0.0; a[15] = 1.0;
191  } else
192  {
193  a[0] = cos(dh.rot_z); a[4] = -sin(dh.rot_z)*cos(dh.rot_x); a[8] = sin(dh.rot_x) * sin(dh.rot_z); a[12] = dh.trans_x*cos(dh.rot_z);
194  a[1] = sin(dh.rot_z); a[5] = cos(dh.rot_x)*cos(dh.rot_z); a[9] = -cos(dh.rot_z) * sin(dh.rot_x); a[13] = dh.trans_x*sin(dh.rot_z);
195  a[2] = 0.0; a[6] = sin(dh.rot_x); a[10] = cos(dh.rot_x); a[14] = dh.trans_z + dh.angle;
196  a[3] = 0.0; a[7] = 0.0; a[11] = 0.0; a[15] = 1.0;
197  }
198 }
199 
200 // trace of rotational 3x3 part of matrix
201 PRECISION CMatrix::trace()
202 {
203  return a[0] + a[5] + a[10];
204 }
205 
206 // print matrix contents
207 void CMatrix::print(bool round) const
208 {
209  //char str[1024];
210 
211  #define RND(x) (round ? rnd(x*10)/10.0 : x)
212  printf(
213  "%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n",
214  RND(a[0]), RND(a[4]), RND(a[8]), RND(a[12]),
215  RND(a[1]), RND(a[5]), RND(a[9]), RND(a[13]),
216  RND(a[2]), RND(a[6]), RND(a[10]), RND(a[14]),
217  RND(a[3]), RND(a[7]), RND(a[11]), RND(a[15]));
218 }
219 
220 
221 std::string CMatrix::toString(bool round)
222 {
223  char str[1024];
224 #define RND(x) (round ? rnd(x*10)/10.0 : x)
225  sprintf( str,
226  "%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n",
227  RND(a[0]), RND(a[4]), RND(a[8]), RND(a[12]),
228  RND(a[1]), RND(a[5]), RND(a[9]), RND(a[13]),
229  RND(a[2]), RND(a[6]), RND(a[10]), RND(a[14]),
230  RND(a[3]), RND(a[7]), RND(a[11]), RND(a[15]));
231  return std::string(str);
232 }
233 
234 // scalar multiplication
235 CMatrix CMatrix::operator * ( PRECISION f) const
236 {
237  return CMatrix( a[0]*f, a[4]*f, a[8]*f, a[12]*f,
238  a[1]*f, a[5]*f, a[9]*f, a[13]*f,
239  a[2]*f, a[6]*f, a[10]*f, a[14]*f,
240  0.0, 0.0, 0.0, 1.0);
241 }
242 
243 // matrix add operator
244 CMatrix CMatrix::operator + (const CMatrix &b) const
245 {
246  return CMatrix( a[0]+b.a[0], a[4]+b.a[4], a[8]+b.a[8], a[12]+b.a[12],
247  a[1]+b.a[1], a[5]+b.a[5], a[9]+b.a[9], a[13]+b.a[13],
248  a[2]+b.a[2], a[6]+b.a[6], a[10]+b.a[10], a[14]+b.a[14],
249  0.0, 0.0, 0.0, 1.0);
250 }
251 
252 // matrix sub operator
253 CMatrix CMatrix::operator - (const CMatrix &b) const
254 {
255  return CMatrix( a[0]-b.a[0], a[4]-b.a[4], a[8]-b.a[8], a[12]-b.a[12],
256  a[1]-b.a[1], a[5]-b.a[5], a[9]-b.a[9], a[13]-b.a[13],
257  a[2]-b.a[2], a[6]-b.a[6], a[10]-b.a[10], a[14]-b.a[14],
258  0.0, 0.0, 0.0, 1.0);
259 }
260 
261 // matrix product operator
262 CMatrix CMatrix::operator * (const CMatrix &m) const
263 {
264  CMatrix tmp;
265  tmp.mul((*this), m);
266  return tmp;
267 }
268 
269 // add a translation to homogenous matrix
270 CMatrix& CMatrix::operator += (const CVec &v)
271 {
272  a[12] += v.x;
273  a[13] += v.y;
274  a[14] += v.z;
275  return *this;
276 }
277 
278 // calculate matrix * vector
279 CVec CMatrix::operator * (const CVec &v) const
280 {
281  PRECISION x, y, z;
282  x = a[0]*v.x + a[4]*v.y + a[8]*v.z + a[12]*v.w;
283  y = a[1]*v.x + a[5]*v.y + a[9]*v.z + a[13]*v.w;
284  z = a[2]*v.x + a[6]*v.y + a[10]*v.z + a[14]*v.w;
285  return CVec(x, y, z);
286 }
287 
288 
289 // array operator, read only
290 const CVec& CMatrix::operator [] ( int i ) const
291 {
292  return *( const CVec*) &a[4*i];
293 }
294 
295 CVec& CMatrix::operator [] ( int i )
296 {
297  return *(CVec*) &a[4*i];
298 }
299 
300 // assignment operator
301 CMatrix& CMatrix::operator = ( const CMatrix& v)
302 {
303  memcpy(a,v.a, 16*sizeof(PRECISION));
304  return *this;
305 }
306 
307 // rounds a value
308 int rnd(PRECISION value)
309 {
310  return (int) (value < 0.0 ? ceil(value - 0.5) : floor(value +0.5));
311 }
312 
313 void CMatrix::matrixToArray(std::vector<double> &values)
314 {
315  values.resize(12);
316  values[0] = a[12];
317  values[1] = a[13];
318  values[2] = a[14];
319 
320 
321  values[3] = a[0];
322  values[4] = a[1];
323  values[5] = a[2];
324  values[6] = a[4];
325  values[7] = a[5];
326  values[8] = a[6];
327  values[9] = a[8];
328  values[10] = a[9];
329  values[11] = a[10];
330 }
331 
332 void CMatrix::arrayToMatrix(std::vector<double> &values)
333 {
334  a[12] = values[0];
335  a[13] = values[1];
336  a[14] = values[2];
337  a[15] = 1.0;
338 
339  a[0] = values[3];
340  a[1] = values[4];
341  a[2] = values[5];
342  a[3] = 0.0;
343 
344  a[4] = values[6];
345  a[5] = values[7];
346  a[6] = values[8];
347  a[7] = 0.0;
348 
349  a[8] = values[9];
350  a[9] = values[10];
351  a[10] = values[11];
352  a[11] = 0.0;
353 }
354 
355 PRECISION CMatrix::length() const
356 {
357  double dist = sqrt(a[12]*a[12]+a[13]*a[13]+a[14]*a[14]);
358  CVec axis;
359  double angle;
360  CMathLib::getRotationFromMatrix(*this, axis, angle);
361 
362  if (2.0*M_PI - angle < angle)
363  angle = 2.0*M_PI-angle;
364 
365  return dist + fabsf(angle) * 180.0/M_PI;
366 }
367 
368 
369 
370 // calculates rotation matrix from quaternion
371 // http://www.flipcode.com/documents/matrfaq.html
372 void CMathLib::matrixFromQuaternion(CVec &quaternion, CMatrix &matrix)
373 {
374  double xx,xy,xz,xw,yy,yz,yw,zz,zw;
375  double X = quaternion.x;
376  double Y = quaternion.y;
377  double Z = quaternion.z;
378  double W = quaternion.w;
379  xx = X * X;
380  xy = X * Y;
381  xz = X * Z;
382  xw = X * W;
383 
384  yy = Y * Y;
385  yz = Y * Z;
386  yw = Y * W;
387 
388  zz = Z * Z;
389  zw = Z * W;
390 
391  matrix.a[0] = 1.0 - 2.0 * ( yy + zz );
392  matrix.a[4] = 2.0 * ( xy - zw );
393  matrix.a[8] = 2.0 * ( xz + yw );
394 
395  matrix.a[1] = 2.0 * ( xy + zw );
396  matrix.a[5] = 1.0 - 2.0 * ( xx + zz );
397  matrix.a[9] = 2.0 * ( yz - xw );
398 
399  matrix.a[2] = 2.0 * ( xz - yw );
400  matrix.a[6] = 2.0 * ( yz + xw );
401  matrix.a[10] = 1.0 - 2.0 * ( xx + yy );
402 
403  matrix.a[3] = matrix.a[7] = matrix.a[11] = matrix.a[12] = matrix.a[13] = matrix.a[14] = 0.0;
404  matrix.a[15] = 1.0;
405 }
406 
407 // calculates matrix from rotation axis and rotation angle
408 // http://www.flipcode.com/documents/matrfaq.html
409 void CMathLib::getMatrixFromRotation(CMatrix &mat, const CVec &axis, double angle)
410 {
411  CVec tmp, quater;
412  double sin_a = sin( angle / 2.0 );
413  double cos_a = cos( angle / 2.0 );
414 
415  tmp.x = axis.x * sin_a;
416  tmp.y = axis.y * sin_a;
417  tmp.z = axis.z * sin_a;
418  tmp.w = cos_a;
419 
420  // normalisieren
421  double tmpf = 1.0/sqrt(tmp.x*tmp.x+
422  tmp.y*tmp.y+
423  tmp.z*tmp.z+
424  tmp.w*tmp.w);
425 
426  //tmpf = 1.0;
427 
428  quater.x = tmp.x * tmpf;
429  quater.y = tmp.y * tmpf;
430  quater.z = tmp.z * tmpf;
431  quater.w = tmp.w * tmpf;
432 
433  matrixFromQuaternion(quater, mat);
434 }
435 
436 void CMathLib::getQuaternionFromRotation(CVec &quater, CVec &axis, double angle)
437 {
438  CVec tmp;
439  double sin_a = sin( angle / 2.0 );
440  double cos_a = cos( angle / 2.0 );
441 
442  tmp.x = axis.x * sin_a;
443  tmp.y = axis.y * sin_a;
444  tmp.z = axis.z * sin_a;
445  tmp.w = cos_a;
446 
447  // normalisieren
448  double tmpf = 1.0/sqrt(tmp.x*tmp.x+
449  tmp.y*tmp.y+
450  tmp.z*tmp.z+
451  tmp.w*tmp.w);
452 
453  //tmpf = 1.0;
454 
455  quater.x = tmp.x * tmpf;
456  quater.y = tmp.y * tmpf;
457  quater.z = tmp.z * tmpf;
458  quater.w = tmp.w * tmpf;
459 }
460 
461 // calculates rotation axis and angle from rotation matrix
462 // http://www.flipcode.com/documents/matrfaq.html
463 void CMathLib::getRotationFromMatrix(const CMatrix &mat, CVec &result, double &angle)
464 {
465  CVec quat, tmp;
466  double rotangle, tmpf, cos_a, sin_a;
467  quaternionFromMatrix(mat, tmp);
468 
469  // normalisieren
470  tmpf = 1.0/sqrt(tmp.x*tmp.x+
471  tmp.y*tmp.y+
472  tmp.z*tmp.z+
473  tmp.w*tmp.w);
474 
475  quat = tmp * tmpf;
476  quat.w = tmp.w * tmpf;
477 
478  cos_a = quat.w;
479  rotangle = acos( cos_a ) * 2;
480  sin_a = sqrt( 1.0 - cos_a * cos_a );
481  if ( fabs( sin_a ) < 0.0005 ) sin_a = 1;
482 
483  result.x = quat.x / sin_a;
484  result.y = quat.y / sin_a;
485  result.z = quat.z / sin_a;
486 
487  angle = rotangle;
488 
489  /*
490  if (result.z < 0.0)
491  {
492  result = -result;
493  angle = -angle;
494  }
495  */
496 }
497 
498 // calculates matrix[n x m] * vector [m]
499 void CMathLib::calcMatrixResult(double a[], double b[], int n, int m, double result[])
500 {
501  int i,j;
502  for (i=0;i<n;i++)
503  {
504  result[i] = 0.0;
505  for (j=0;j<m; j++)
506  result[i] += a[i*n+j]*b[j];
507  }
508 }
509 
510 // calculates dot product a[n] * b[n]
511 double CMathLib::calcDotProduct(double a[], double b[], int n)
512 {
513  int i;
514  double tmp = a[0]*b[0];
515  for (i=1;i<n;i++)
516  tmp += a[i]*b[i];
517 
518  return tmp;
519 }
520 
521 
522 
523 // calculates quaternion from rotation matrix
524 // http://www.flipcode.com/documents/matrfaq.html
525 void CMathLib::quaternionFromMatrix(const CMatrix &mat, CVec &quaternion)
526 {
527  double T = 1 + mat.a[0] + mat.a[5] + mat.a[10];//mat.trace();
528  double S,X,Y,Z,W;
529 
530  if (T > 0.00000001)
531  {
532  S = 2 * sqrt(T);
533  X = (mat.a[6] - mat.a[9])/S;
534  Y = (mat.a[8] - mat.a[2])/S;
535  Z = (mat.a[1] - mat.a[4])/S;
536  W = S*0.25;
537  } else
538  if ( mat.a[0] > mat.a[5] && mat.a[0] > mat.a[10] ) { // Column 0:
539  S = sqrt( 1.0 + mat.a[0] - mat.a[5] - mat.a[10] ) * 2;
540  X = 0.25 * S;
541  Y = (mat.a[4] + mat.a[1] ) / S;
542  Z = (mat.a[2] + mat.a[8] ) / S;
543  W = (mat.a[6] - mat.a[9] ) / S;
544  } else if ( mat.a[5] > mat.a[10] ) { // Column 1:
545  S = sqrt( 1.0 + mat.a[5] - mat.a[0] - mat.a[10] ) * 2;
546  X = (mat.a[4] + mat.a[1] ) / S;
547  Y = 0.25 * S;
548  Z = (mat.a[9] + mat.a[6] ) / S;
549  W = (mat.a[8] - mat.a[2] ) / S;
550  } else { // Column 2:
551  S = sqrt( 1.0 + mat.a[10] - mat.a[0] - mat.a[5] ) * 2;
552  X = (mat.a[2] + mat.a[8] ) / S;
553  Y = (mat.a[9] + mat.a[6] ) / S;
554  Z = 0.25 * S;
555  W = (mat.a[1] - mat.a[4] ) / S;
556  }
557 
558  quaternion.x = X;
559  quaternion.y = Y;
560  quaternion.z = Z;
561  quaternion.w = W;
562 }
563 
564 // transposes matrix a[n x n]
565 void CMathLib::transposeMatrix(double *a, double *result, int n)
566 {
567  int i,j;
568  double tmp;
569  for (i=0;i<n;i++)
570  for (j=0;j<i;j++)
571  {
572  tmp = a[i*n+j];
573  result[i*n+j] = a[j*n+i];
574  result[j*n+i] = tmp;
575  }
576 }
577 
578 // multiplies two matrix a[n x m] * b [m x k]
579 void CMathLib::multiplyMatrices(double *a, double *b, int n, int m, int k, double *res)
580 {
581  int i,j,l;
582  for (i=0; i<n; i++)
583  for (j=0; j<k; j++)
584  {
585  res[i*n + j] = 0.0;
586  for (l=0; l<m; l++)
587  res[i*n + j] += a[i*n + l] * b[l*m + j];
588  }
589 }
590 
591 // calculates euler angles representing rotation matrix
592 /*void CMathLib::getOrientation(CMatrix &mat, CVec &first, CVec &second)
593 {
594  //g = roll, a = pitch, b = yaw
595  double a, b, g;
596 
597 
598  b = atan2(-mat.a[4], sqrt(mat.a[5]*mat.a[5]+mat.a[6]*mat.a[6]));
599 
600  if (fabs(b - M_PI/2.0) < 0.0001)
601  {
602  g = 0;
603  a = atan2(mat.a[9], mat.a[10]);
604  } else if (fabs(b + M_PI/2.0) < 0.0001)
605  {
606  g = 0;
607  a = atan2(-mat.a[9], mat.a[10]);
608  } else
609  {
610  a = atan2(mat.a[8]/cos(b), mat.a[0]/cos(b));
611  g = atan2(mat.a[6]/cos(b), mat.a[5]/cos(b));
612  }
613 
614  first.set(g, a, b);
615  second = first;
616 }*/
617 
618 void CMathLib::getEulerZXZ(CMatrix &mat, CVec &first)
619 {
620  if (mat.a[10] < 1.0)
621  {
622  if (mat.a[10]>-1.0)
623  {
624  first.x = atan2(mat.a[8], -mat.a[9]);
625  first.y = acos(mat.a[10]);
626  first.z = atan2(mat.a[2], mat.a[6]);
627  } else
628  {
629  first.x = -atan2(-mat.a[4], mat.a[0]);
630  first.y = M_PI;
631  first.z = 0;
632  }
633  } else
634  {
635  first.x = 0;
636  first.y = atan2(-mat.a[4], mat.a[0]);
637  first.z = 0;
638  }
639 }
640 
641 // calculates euler angles representing rotation matrix
642 void CMathLib::getOrientation(CMatrix &mat, CVec &first, CVec &second, bool old)
643 {
644  double a, b, g;
645 
646  b = atan2(-mat.a[2], sqrt(mat.a[0]*mat.a[0]+mat.a[1]*mat.a[1]));
647 
648  if (fabsf(b - M_PI/2.0) < 0.000001)
649  {
650  a = 0;
651  g = atan2(mat.a[4], mat.a[5]);
652  } else if (fabsf(b + M_PI/2.0) < 0.000001)
653  {
654  a = 0;
655  g = -atan2(mat.a[4], mat.a[5]);
656  } else
657  {
658  a = atan2(mat.a[1]/cos(b), mat.a[0]/cos(b));
659  g = atan2(mat.a[6]/cos(b), mat.a[10]/cos(b));
660  }
661 
662  if (old)
663  first.set(a, b, g);
664  else
665  first.set(g, b, a);
666 
667  second = first;
668 }
669 
670 
671 // calculates rotation matrix representing euler angles
672 /*void CMathLib::getRotation(CMatrix &mat, double aX, double aY, double aZ)
673 {
674  double a = aX;
675  double b = aZ;
676  double g = aY;
677  mat.a[0] = cos(b)*cos(g);
678  mat.a[1] = cos(a)*sin(b)*cos(g) + sin(a)*sin(g);
679  mat.a[2] = sin(a)*sin(b)*cos(g)-cos(a)*sin(g);
680  mat.a[4] = -sin(b);
681  mat.a[5] = cos(a)*cos(b);
682  mat.a[6] = sin(a)*cos(b);
683 
684  mat.a[8] = cos(b)*sin(g);
685  mat.a[9] = cos(a)*sin(b)*sin(g)-sin(a)*cos(g);
686  mat.a[10] = sin(a)*sin(b)*sin(g)+cos(a)*cos(g);
687 
688 }
689 */
690 
691 // calculates rotation matrix representing euler angles
692 void CMathLib::getRotation(CMatrix &mat, double aX, double aY, double aZ, bool old)
693 {
694  double g = aX;
695  double b = aY;
696  double a = aZ;
697 
698  if (old)
699  {
700  a = aX;
701  b = aY;
702  g = aZ;
703  }
704  if (fabsf(b - M_PI/2.0) < 0.0001)
705  {
706  mat.a[0] = 0.0;
707  mat.a[1] = 0.0;
708  mat.a[2] = -1.0;
709  mat.a[4] = sin(g - a);
710  mat.a[5] = cos(g - a);
711  mat.a[6] = 0.0;
712 
713  mat.a[8] = cos(g - a);
714  mat.a[9] = -sin(g - a);
715  mat.a[10] = 0.0;
716  } else
717  if (fabsf(b + M_PI/2.0) < 0.0001)
718  {
719  mat.a[0] = 0.0;
720  mat.a[1] = 0.0;
721  mat.a[2] = 1.0;
722  mat.a[4] = -sin(g + a);
723  mat.a[5] = cos(g + a);
724  mat.a[6] = 0.0;
725 
726  mat.a[8] = -cos(g + a);
727  mat.a[9] = -sin(g + a);
728  mat.a[10] = 0.0;
729  } else
730  {
731  mat.a[0] = cos(b)*cos(a);
732  mat.a[1] = sin(a)*cos(b);
733  mat.a[2] = -sin(b);
734  mat.a[4] = cos(a)*sin(b)*sin(g) - sin(a)*cos(g);
735  mat.a[5] = sin(a)*sin(b)*sin(g) + cos(a)*cos(g);
736  mat.a[6] = cos(b)*sin(g);
737 
738  mat.a[8] = cos(a)*sin(b)*cos(g)+sin(a)*sin(g);
739  mat.a[9] = sin(a)*sin(b)*cos(g)-cos(a)*sin(g);
740  mat.a[10] = cos(b)*cos(g);
741  }
742 
743  /* set to zero
744  mat.a[12] = 0.0; mat.a[13] = 0.0; mat.a[14] = 0.0;
745  mat.a[3] = 0.0;mat.a[7] = 0.0;mat.a[11] = 0.0;mat.a[15] = 1.0;
746  */
747 }
748 
749 // convenience function
750 void CMathLib::getRotation(CMatrix &mat, CVec &vec, bool old)
751 {
752  getRotation(mat, vec.x, vec.y, vec.z, old);
753 }
754 
755 // calculates inverse kinematics of a two bone leg
756 void CMathLib::calcAngles(double leg1, double leg2, double x, double y, double &first, double &second)
757 {
758  double lambda;
759 
760  // normalize x and y
761  const double factor = 0.9999;
762  if ((x*x +y*y) >= factor*(leg1+leg2)*(leg1+leg2))
763  {
764  x = factor*x*sqrt((leg1+leg2)*(leg1+leg2) /(x*x +y*y));
765  y = factor*y*sqrt((leg1+leg2)*(leg1+leg2) /(x*x +y*y));
766  first = 0.0;
767  second = atan2(-x,-y);
768  return;
769  }
770 
771  lambda = leg1;
772  leg1 = leg2;
773  leg2 = lambda;
774  double cosb, sinb, sinab, cosab, cosa, sina;
775  lambda = sqrt(x*x+y*y);
776  sina = y / lambda;
777  cosa = -x / lambda;
778  cosb = (leg1*leg1 + lambda*lambda - leg2*leg2) / (2*leg1*lambda);
779  sinb = sqrt(1-cosb*cosb);
780  sinab = sina*cosb + cosa*sinb;
781  cosab = cosa*cosb-sina*sinb;
782  second = atan2(sinab, cosab)+0.5*M_PI;
783 
784  cosa = (leg1*leg1 + leg2*leg2 - lambda*lambda)/(2*leg1*leg2);
785  sina = sqrt(1-cosa*cosa);
786  first = 0.5*M_PI-atan2(-cosa, -sina);
787 
788  first = atan2(sina, cosa) - M_PI;
789 }
790 
791 double vectorlength(std::vector<double> &v)
792 {
793  double sum = 0.0;
794  for (unsigned int i=0; i<v.size(); i++)
795  sum += v[i]*v[i];
796  return sqrt(sum);
797 }
798 
799 char rosenbrock_version[] = "rosenbrock 0.99";
800 
801 #define MIN(a,b) ((a)<(b)?(a):(b))
802 #define MAX(a,b) ((a)>(b)?(a):(b))
803 
804 void simulatedannealing(int n, double *x, double *bl, double *bu,
805  double bigbnd, int maxiter, double eps, int verbose,
806  void obj(int,double *,double *,void *), void *extraparams)
807 {
808  double *best = (double*)calloc(n,sizeof(double));
809  double besty, t0, beta, y, delta;;
810 
811  beta = 0.9999;
812  t0 = 3000.0;
813 
814  memcpy(best,x,n*sizeof(double));
815 
816  (*obj)(n,best,&besty,extraparams);
817 
818  unsigned int steps, unchanged;
819 
820  steps = 0;
821  unchanged = 0;
822  while ((int)steps < maxiter && unchanged < 100)
823  {
824  for (int i=0; i<n;i++)
825  {
826  double rnd = getUniform();
827 
828  double lo, hi;
829 
830  if ((x[i] - eps) < bl[i])
831  {
832  lo = bl[i];
833  hi = lo + 2*eps;
834  } else
835  if ((x[i] + eps) > bu[i])
836  {
837  hi = bu[i];
838  lo = hi -2*eps;
839 
840  } else
841  {
842  lo = x[i] - eps;
843  hi = x[i] + eps;
844  }
845 
846  x[i] = lo + (hi -lo) * rnd;
847  }
848 
849  (*obj)(n,x,&y,extraparams);
850  //if (y > 1000)
851  // continue;
852 
853  delta = y - besty;
854 
855  if (delta <= 0 || getUniform() <= exp(-delta/(t0 * pow(beta, steps))))
856  {
857  memcpy(best,x,n*sizeof(double));
858  besty = y;
859 
860  unchanged = 0;
861  } else unchanged++;
862 
863  steps++;
864  }
865 
866  printf("steps: %d best: %g\n", steps, besty);
867 
868  memcpy(x,best,n*sizeof(double));
869 }
870 
871 void rosenbrock(int n, double *x, double *bl, double *bu,
872  double bigbnd, int maxiter, double eps, int verbose,
873  void obj(int,double *,double *,void *), void *extraparams)
874 {
875  double **xi=(double**)calloc(n,sizeof(double*)),
876  *temp1=(double*)calloc(n*n,sizeof(double)),
877  **A=(double**)calloc(n,sizeof(double*)),
878  *temp2=(double*)calloc(n*n,sizeof(double)),
879  *d=(double*)calloc(n,sizeof(double)),
880  *lambda=(double*)calloc(n,sizeof(double)),
881  *xk=(double*)calloc(n,sizeof(double)),
882  *xcurrent=(double*)calloc(n,sizeof(double)),
883  *t=(double*)calloc(n,sizeof(double)),
884  alpha=2,
885  beta=0.5,
886  yfirst,yfirstfirst,ybest,ycurrent,mini,div;
887  int i,k,j,restart,numfeval=0;
888 
889  memset(temp1,0,n*n*sizeof(double));
890  for(i=0; i<n; i++)
891  {
892  temp1[i]=1;
893  xi[i]=temp1;
894  temp1+=n;
895  A[i]=temp2;
896  temp2+=n;
897  };
898  // memcpy(destination,source,nbre_of_byte)
899  memcpy(xk,x,n*sizeof(double));
900  for (i=0; i<n; i++)
901  d[i]=.1;
902  memset(lambda,0,n*sizeof(double));
903  (*obj)(n,x,&yfirstfirst,extraparams);
904  numfeval++;
905 
906  do
907  {
908  ybest=yfirstfirst;
909  do
910  {
911  yfirst=ybest;
912  for (i=0; i<n; i++)
913  {
914  for (j=0; j<n; j++)
915  {
916  xcurrent[j]=xk[j]+d[i]*xi[i][j];
917  if (xcurrent[j] < bl[j])
918  xcurrent[j] = bl[j];
919  else if (xcurrent[j] > bu[j])
920  xcurrent[j] = bu[j];
921  }
922  (*obj)(n,xcurrent,&ycurrent,extraparams);
923  numfeval++;
924 
925  if (ycurrent<ybest)
926  {
927  lambda[i]+=d[i]; // success
928  d[i]*=alpha;
929  ybest=ycurrent;
930  memcpy(xk,xcurrent,n*sizeof(double));
931  } else
932  {
933  d[i]*=-beta; // failure
934  }
935 
936  if (numfeval > maxiter)
937  {
938  memcpy(x,xk,n*sizeof(double));
939  goto LABEL_ROSENBROCK_DONE;
940  }
941  }
942  } while (ybest<yfirst);
943 
944  mini=bigbnd;
945  for (i=0; i<n; i++)
946  mini=MIN(mini,fabs(d[i]));
947  restart=mini>eps;
948 
949  memcpy(x,xk,n*sizeof(double));
950 
951  if (ybest<yfirstfirst)
952  {
953  mini=bigbnd;
954  for (i=0; i<n; i++)
955  mini=MIN(mini,fabs(xk[i]-x[i]));
956  restart=restart||(mini>eps);
957 
958  if (restart)
959  {
960  // nous avons:
961  // xk[j]-x[j]=(somme sur i de) lambda[i]*xi[i][j];
962 
963  for (i=0; i<n; i++)
964  A[n-1][i]=lambda[n-1]*xi[n-1][i];
965 
966  for (k=n-2; k>=0; k--)
967  for (i=0; i<n; i++)
968  A[k][i]=A[k+1][i]+lambda[k]*xi[k][i];
969 
970  t[n-1]=lambda[n-1]*lambda[n-1];
971  for (i=n-2; i>=0; i--)
972  t[i]=t[i+1]+lambda[i]*lambda[i];
973  for (i=n-1; i>0; i--)
974  {
975  div=sqrt(t[i-1]*t[i]);
976  if (div!=0)
977  for (j=0; j<n; j++)
978  xi[i][j]=(lambda[i-1]*A[i][j]-xi[i-1][j]*t[i])/div;
979  }
980  div=sqrt(t[0]);
981  for (i=0; i<n; i++)
982  xi[0][i]=A[0][i]/div;
983 
984  memcpy(x,xk,n*sizeof(double));
985  memset(lambda,0,n*sizeof(double));
986  for (i=0; i<n; i++)
987  d[i]=.1;
988  yfirstfirst=ybest;
989  }
990  }
991 
992  } while ((restart)&&(numfeval<maxiter));
993 
994  LABEL_ROSENBROCK_DONE:
995  // the maximum number of evaluation is approximative
996  // because in 1 iteration there is n function evaluations.
997  if (verbose)
998  {
999  printf("ROSENBROCK method for local optimization (minimization)\n"
1000  "number of evaluation of the objective function= %i\n",numfeval);
1001  }
1002  free(xi[0]);
1003  free(A[0]);
1004  free(d);
1005  free(lambda);
1006  free(xk);
1007  free(xcurrent);
1008  free(t);
1009 }
1010 
1011 void calculateTransformationFromPlane(CVec &plane, CMatrix &transformation)
1012 {
1013  CVec z;
1014  z.x = plane.x; // plane is 4d!
1015  z.y = plane.y;
1016  z.z = plane.z;
1017 
1018  CVec y;
1019 
1020  if (plane.x != 0.0)
1021  {
1022  y.x = (plane.y + plane.z) / plane.x;
1023  y.y = -1.0;
1024  y.z = -1.0;
1025  } else
1026  if (plane.y != 0.0)
1027  {
1028  y.y = (plane.x + plane.z) / plane.y;
1029  y.x = -1.0;
1030  y.z = -1.0;
1031  } else
1032  if (plane.z != 0.0)
1033  {
1034  y.z = (plane.y + plane.x) / plane.z;
1035  y.y = -1.0;
1036  y.x = -1.0;
1037  } else printf("error\n");
1038 
1039  CVec x = y ^ z;
1040 
1041  x.normalize();
1042  y.normalize();
1043  z.normalize();
1044 
1045  transformation.a[0] = x.x;
1046  transformation.a[1] = x.y;
1047  transformation.a[2] = x.z;
1048 
1049  transformation.a[4] = y.x;
1050  transformation.a[5] = y.y;
1051  transformation.a[6] = y.z;
1052 
1053  transformation.a[8] = z.x;
1054  transformation.a[9] = z.y;
1055  transformation.a[10] = z.z;
1056 }
1057 
1058 
1059 void setUniformSeed(unsigned int seed)
1060 {
1061  pRngUniform01.engine().seed(seed);
1062  pRngUniform01.distribution().reset();
1063  //pRngMt19937.seed(seed);
1064 }
1065 
1066 double getSigmoid(double value, double offset, double factor)
1067 {
1068  return 1.0/(1.0+exp(-factor*(value - offset)));
1069 }
1070 
1071 double getGaussian(double value, double mean, double std)
1072 {
1073  // 0.063494 = 1.0 * sqrt(2 * M_PI)
1074 
1075  return 0.063494 / std * exp(-0.5 * (value - mean) * (value - mean) / (std * std) );
1076 }
1077 
1078 double getGaussianWithoutNormalization(double value, double mean, double std)
1079 {
1080  return exp(-0.5 * (value - mean) * (value - mean) / (std * std) );
1081 }
1082 
1083 
1084 
1085 double getLogisticProb(double alpha, double beta)
1086 {
1087  double u = getUniform();
1088 
1089  if (u==0.0)
1090  return 1E30;
1091 
1092  if (u==1.0)
1093  return -1E30;
1094 
1095  return alpha - beta * log(u/(1.0-u));
1096 }
1097 
1098 
1099 
1100 void convertMatrix(double (*R)[3], double *T, CMatrix &to)
1101 {
1102  to.a[12] = T[0];
1103  to.a[13] = T[1];
1104  to.a[14] = T[2];
1105 
1106  to.a[0] = R[0][0];
1107  to.a[4] = R[0][1];
1108  to.a[8] = R[0][2];
1109 
1110  to.a[1] = R[1][0];
1111  to.a[5] = R[1][1];
1112  to.a[9] = R[1][2];
1113 
1114  to.a[2] = R[2][0];
1115  to.a[6] = R[2][1];
1116  to.a[10] = R[2][2];
1117 }
1118 
1119 void convertMatrix(CMatrix &from, double (*R)[3], double *T)
1120 {
1121  R[0][0] = from.a[0];
1122  R[0][1] = from.a[4];
1123  R[0][2] = from.a[8];
1124 
1125  R[1][0] = from.a[1];
1126  R[1][1] = from.a[5];
1127  R[1][2] = from.a[9];
1128 
1129  R[2][0] = from.a[2];
1130  R[2][1] = from.a[6];
1131  R[2][2] = from.a[10];
1132 
1133  T[0] = from.a[12];
1134  T[1] = from.a[13];
1135  T[2] = from.a[14];
1136 }
1137 
1138 
1139 void getMeanFromVectors(std::vector<std::vector<double> > &values, std::vector<double> &mean)
1140 {
1141  if (values.size() == 0)
1142  return;
1143 
1144  mean.clear();
1145  mean.resize(values[0].size(), 0.0);
1146  unsigned int counter = 0;
1147  for (unsigned int i=0; i<values.size(); i++)
1148  {
1149  for (unsigned int j=0; j<mean.size() && j<values[i].size(); j++)
1150  mean[j] += values[i][j];
1151 
1152  counter++;
1153  }
1154 
1155  if (counter > 0)
1156  for (unsigned int j=0; j<mean.size(); j++)
1157  mean[j] /= (double) counter;
1158 }
1159 
1160 
1161 
1163 {
1164  double angle;
1165  CMathLib::getRotationFromMatrix(matrix, axis, angle);
1166  axis *= angle;
1167 }
1168 
1169 void orientation2scaledAxis(CMatrix &matrix, std::vector<double> &axis2)
1170 {
1171  CVec axis;
1172  double angle;
1173  CMathLib::getRotationFromMatrix(matrix, axis, angle);
1174  axis2.resize(3);
1175  axis2[0] = axis.x * angle;
1176  axis2[1] = axis.y * angle;
1177  axis2[2] = axis.z * angle;
1178 }
1179 
1180 void scaledAxis2Orientation(CVec &axis2, CMatrix &matrix)
1181 {
1182  CVec axis;
1183  double angle = axis2.length();
1184  if (angle == 0.0)
1185  axis = axis2;
1186  else
1187  axis = axis2 / angle;
1188 
1189  CMathLib::getMatrixFromRotation(matrix, axis, angle);
1190 }
1191 
1192 void scaledAxis2Orientation(std::vector<double> &axis2, CMatrix &matrix)
1193 {
1194  CVec axis(axis2[0], axis2[1], axis2[2]);
1195  double angle = axis.length();
1196  axis.normalize();
1197 
1198  CMathLib::getMatrixFromRotation(matrix, axis, angle);
1199 }
1200 
1201 
1202 }
bool rotationalDof
Definition: frame.h:246
double trans_x
Translation offset along rotated x-axis.
Definition: frame.h:252
PRECISION z
Definition: vecmath.h:60
void calculateTransformationFromPlane(CVec &plane, CMatrix &transformation)
Definition: vecmath.cpp:1011
Denavit Hartenberg Link information.
Definition: frame.h:241
PRECISION length() const
Definition: vecmath.h:168
boost::uniform_01< double > pRngDist
Definition: vecmath.cpp:45
double getSigmoid(double value, double offset, double factor)
Definition: vecmath.cpp:1066
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
PRECISION a[16]
Definition: vecmath.h:190
boost::variate_generator< boost::mt19937 &, boost::uniform_01< double > > pRngUniform01(pRngMt19937, pRngDist)
double getLogisticProb(double alpha, double beta)
Definition: vecmath.cpp:1085
double getGaussian(double value, double mean, double std)
Definition: vecmath.cpp:1071
boost::mt19937 pRngMt19937
Definition: vecmath.cpp:44
#define SWAP(x, y)
Definition: vecmath.cpp:36
PRECISION w
Definition: vecmath.h:60
double rot_x
Rotation offset around rotated x-axis.
Definition: frame.h:251
double getGaussianProb(double mean, double std)
Definition: vecmath.h:440
void setUniformSeed(unsigned int seed)
Definition: vecmath.cpp:1059
double rot_z
Rotation offset around z-axis.
Definition: frame.h:249
Homogenous vector.
Definition: vecmath.h:57
void set(PRECISION x, PRECISION y, PRECISION z)
Definition: vecmath.cpp:51
char rosenbrock_version[]
Definition: vecmath.cpp:799
Homogenous matrix.
Definition: vecmath.h:187
bool useAxis
Definition: frame.h:247
double angle
Current angle of rotation aroung z-axis (changes with the real servo position)
Definition: frame.h:253
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
#define MIN(a, b)
Definition: vecmath.cpp:801
double trans_z
Translation offset along z-axis.
Definition: frame.h:250
double getGaussianWithoutNormalization(double value, double mean, double std)
Definition: vecmath.cpp:1078
int rnd(PRECISION value)
Rounds a value.
Definition: vecmath.cpp:308
double getUniform()
Definition: vecmath.h:429
#define RND(x)
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 y
Definition: vecmath.h:60
void scaledAxis2Orientation(CVec &axis, CMatrix &matrix)
Definition: vecmath.cpp:1180
#define PRECISION
Definition: vecmath.h:44
double vectorlength(std::vector< double > &v)
Definition: vecmath.cpp:791
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


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