WorldSim - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
wmatrix.h
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #ifndef WMATRIX_H
21 #define WMATRIX_H
22 
23 #include "worldsimconfig.h"
24 #include "wvector.h"
25 #include <cstring>
26 
27 #ifdef FARSA_USE_GSL
28  #include <gsl/gsl_blas.h>
29 #endif
30 
31 namespace farsa {
32 
33 class wQuaternion;
34 
46 class FARSA_WSIM_TEMPLATE wMatrix {
47 public:
49  wMatrix();
56  wMatrix( const wVector &X, const wVector &Y, const wVector &Z, const wVector &P );
61  wMatrix( const wQuaternion &rotation, const wVector &position );
67  wMatrix( const real* position, const real* rotation );
68 
70  wMatrix( const wMatrix &other );
72  wMatrix& operator=( const wMatrix &other );
73 
75  wVectorT<true>& operator[](int i);
77  const wVectorT<true>& operator[](int i) const;
78 
80  wMatrix inverse() const;
82  wMatrix transpose() const;
84  wMatrix transpose4X4() const;
86  wVector getEuleroAngles() const;
92  wMatrix rotateAround( const wVector& axis, const wVector& centre, real angle ) const;
94  wMatrix rotateMatrix( const wMatrix& tm ) const;
96  wVector rotateVector(const wVector &v) const;
98  wVector unrotateVector(const wVector &v) const;
100  wVector transformVector(const wVector &v) const;
102  wVector untransformVector(const wVector &v) const;
104  wVector transformPlane(const wVector &localPlane) const;
106  wVector untransformPlane(const wVector &globalPlane) const;
112  void transformTriplex( real* dst, int dstStrideInBytes, real* src, int srcStrideInBytes, int count) const;
113 
115  wMatrix operator*( const wMatrix &B ) const;
116 
118  bool sanityCheck();
120  void sanitifize();
121 
123  static wMatrix identity();
125  static wMatrix zero();
129  static wMatrix grammSchmidt(const wVector& dir);
131  static wMatrix pitch( real ang );
133  static wMatrix yaw( real ang );
135  static wMatrix roll( real ang );
136 
137  wVectorT<true> x_ax;
138  wVectorT<true> y_ax;
139  wVectorT<true> z_ax;
140  wVectorT<true> w_pos;
141 
142 private:
143  real data[16];
144  wVectorT<true>* vecs[4]; // This is only useful for operator[] functions
145 };
146 
147 } // end namespace farsa
148 
149 //--- this is included here because wMatrix and wQuaternion have mutual dependencies
150 #include "wquaternion.h"
151 
152 namespace farsa {
153 
154 inline wMatrix::wMatrix() : x_ax(&data[0]), y_ax(&data[4]), z_ax(&data[8]), w_pos(&data[12]) {
155  vecs[0] = &x_ax;
156  vecs[1] = &y_ax;
157  vecs[2] = &z_ax;
158  vecs[3] = &w_pos;
159 }
160 
161 inline wMatrix::wMatrix ( const wVector &X, const wVector &Y, const wVector &Z, const wVector &P) : x_ax(&data[0]), y_ax(&data[4]), z_ax(&data[8]), w_pos(&data[12]) {
162  vecs[0] = &x_ax;
163  vecs[1] = &y_ax;
164  vecs[2] = &z_ax;
165  vecs[3] = &w_pos;
166  x_ax = X;
167  y_ax = Y;
168  z_ax = Z;
169  w_pos = P;
170 }
171 
172 inline wMatrix::wMatrix( const wQuaternion &rotation, const wVector &position ) : x_ax(&data[0]), y_ax(&data[4]), z_ax(&data[8]), w_pos(&data[12]) {
173  vecs[0] = &x_ax;
174  vecs[1] = &y_ax;
175  vecs[2] = &z_ax;
176  vecs[3] = &w_pos;
177  real x2;
178  real y2;
179  real z2;
180 // real w2;
181  real xy;
182  real xz;
183  real xw;
184  real yz;
185  real yw;
186  real zw;
187 
188 // w2 = 2.0 * rotation.q0 * rotation.q0; COMMENTED OUT BECAUSE IT IS UNUSED
189  x2 = 2.0 * rotation.q1 * rotation.q1;
190  y2 = 2.0 * rotation.q2 * rotation.q2;
191  z2 = 2.0 * rotation.q3 * rotation.q3;
192  // INVARIANT: fabs(w2 + x2 + y2 + z2 - 2.0) < 1.5e-3f;
193 
194  xy = 2.0 * rotation.q1 * rotation.q2;
195  xz = 2.0 * rotation.q1 * rotation.q3;
196  xw = 2.0 * rotation.q1 * rotation.q0;
197  yz = 2.0 * rotation.q2 * rotation.q3;
198  yw = 2.0 * rotation.q2 * rotation.q0;
199  zw = 2.0 * rotation.q3 * rotation.q0;
200 
201  x_ax = wVector( 1.0-y2-z2, xy+zw, xz-yw, 0.0 );
202  y_ax = wVector( xy-zw, 1.0-x2-z2, yz+xw, 0.0 );
203  z_ax = wVector( xz+yw, yz-xw, 1.0-x2-y2, 0.0 );
204 
205  w_pos.x = position.x;
206  w_pos.y = position.y;
207  w_pos.z = position.z;
208  w_pos.w = 1.0;
209 }
210 
211 inline wMatrix::wMatrix( const real* pos, const real* rot ) : x_ax(&data[0]), y_ax(&data[4]), z_ax(&data[8]), w_pos(&data[12]) {
212  vecs[0] = &x_ax;
213  vecs[1] = &y_ax;
214  vecs[2] = &z_ax;
215  vecs[3] = &w_pos;
216  x_ax[0]=rot[0]; x_ax[1]=rot[1]; x_ax[2]=rot[2]; x_ax[3]=0.0 /*rot[3]*/;
217  y_ax[0]=rot[4]; y_ax[1]=rot[5]; y_ax[2]=rot[6]; y_ax[3]=0.0 /*rot[7]*/;
218  z_ax[0]=rot[8]; z_ax[1]=rot[9]; z_ax[2]=rot[10]; z_ax[3]=0.0 /*rot[11]*/;
219  w_pos[0]=pos[0]; w_pos[1]=pos[1]; w_pos[2]=pos[2]; w_pos[3]=1.0 /*pos[3*/;
220 }
221 
222 inline wMatrix::wMatrix( const wMatrix &other ) : x_ax(&data[0]), y_ax(&data[4]), z_ax(&data[8]), w_pos(&data[12]) {
223  vecs[0] = &x_ax;
224  vecs[1] = &y_ax;
225  vecs[2] = &z_ax;
226  vecs[3] = &w_pos;
227  memcpy(data, other.data, 16 * sizeof(real));
228 }
229 
230 inline wMatrix& wMatrix::operator=( const wMatrix &other )
231 {
232  if (&other == this) {
233  return *this;
234  }
235 
236  memcpy(data, other.data, 16 * sizeof(real));
237 
238  return *this;
239 }
240 
241 inline wVectorT<true>& wMatrix::operator[](int i) {
242  return *(vecs[i]);
243 }
244 
245 inline const wVectorT<true>& wMatrix::operator[](int i) const {
246  return *(vecs[i]);
247 }
248 
249 inline wMatrix wMatrix::inverse() const {
250  return wMatrix( wVector(x_ax.x, y_ax.x, z_ax.x, 0.0f),
251  wVector(x_ax.y, y_ax.y, z_ax.y, 0.0f),
252  wVector(x_ax.z, y_ax.z, z_ax.z, 0.0f),
253  wVector(- (w_pos % x_ax), - (w_pos % y_ax), - (w_pos % z_ax), 1.0f));
254 }
255 
256 inline wMatrix wMatrix::transpose() const {
257  return wMatrix( wVector(x_ax.x, y_ax.x, z_ax.x, 0.0f),
258  wVector(x_ax.y, y_ax.y, z_ax.y, 0.0f),
259  wVector(x_ax.z, y_ax.z, z_ax.z, 0.0f),
260  wVector(0.0f, 0.0f, 0.0f, 1.0f));
261 }
262 
263 inline wMatrix wMatrix::transpose4X4() const {
264  return wMatrix( wVector(x_ax.x, y_ax.x, z_ax.x, w_pos.x),
265  wVector(x_ax.y, y_ax.y, z_ax.y, w_pos.y),
266  wVector(x_ax.z, y_ax.z, z_ax.z, w_pos.z),
267  wVector(x_ax.w, y_ax.w, z_ax.w, w_pos.w));
268 }
269 
270 inline wVector wMatrix::getEuleroAngles() const {
271  real yaw;
272  real roll = 0.0;
273  real pitch = 0.0;
274  const real minSin = 0.99995f;
275  const wMatrix& matrix = *this;
276 
277  yaw = asin( -min( max( matrix[0][2], -0.999999 ), 0.999999) );
278  if( matrix[0][2] < minSin ) {
279  if( matrix[0][2] > -minSin ) {
280  roll = atan2( matrix[0][1], matrix[0][0] );
281  pitch = atan2( matrix[1][2], matrix[2][2] );
282  } else {
283  pitch = atan2( matrix[1][0], matrix[1][1] );
284  }
285  } else {
286  pitch = -atan2( matrix[1][0], matrix[1][1] );
287  }
288 
289  return wVector(pitch, yaw, roll, 0.0);
290 }
291 
292 inline wMatrix wMatrix::rotateAround( const wVector& axis, const wVector& centre, real angle ) const {
293  //--- OPTIMIZE_ME this routine is very inefficient. Please Optimize Me
294 /* wMatrix ret = *this;
295  wQuaternion ql( axis, angle );
296  wMatrix rot = wMatrix( ql, wVector(0,0,0) );
297  ret = rot.rotateMatrix( ret );
298  wVector c = w_pos - centre; //centre-w_pos;
299  qDebug() << c[0] << c[1] << c[2];
300  //ret.w_pos += rot.rotateVector( c ) - c;
301  ret.w_pos = centre + rot.rotateVector( c );
302  return ret;*/
303 
304  /*
305  wMatrix ret = *this;
306  wVector centrel = ret.w_pos - centre; //centre - ret.w_pos;
307  ret.w_pos = centrel;
308  wQuaternion ql( axis, angle );
309  ret = ret * wMatrix( ql, wVector(0,0,0) );
310  ret.w_pos += w_pos - centrel;
311  return ret;
312  */
313 
314  // --- using rotateAround of wVector
315  wMatrix ret = *this;
316  ret.x_ax.rotateAround( axis, angle );
317  ret.y_ax.rotateAround( axis, angle );
318  ret.z_ax.rotateAround( axis, angle );
319  wVector diffCentre = ret.w_pos - centre;
320  diffCentre.rotateAround( axis, angle );
321  ret.w_pos = diffCentre + centre;
322  return ret;
323 }
324 
325 inline wMatrix wMatrix::rotateMatrix( const wMatrix& B ) const {
326  const wMatrix& A = *this;
327  return wMatrix (wVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0],
328  A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
329  A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
330  A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
331  wVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0],
332  A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
333  A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
334  A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
335  wVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0],
336  A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
337  A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
338  A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
339  wVector ( A[3][3] * B[3][0],
340  A[3][3] * B[3][1],
341  A[3][3] * B[3][2],
342  A[3][3] * B[3][3]));
343 }
344 
345 inline wVector wMatrix::rotateVector(const wVector &v) const {
346  return wVector( v.x * x_ax.x + v.y * y_ax.x + v.z * z_ax.x,
347  v.x * x_ax.y + v.y * y_ax.y + v.z * z_ax.y,
348  v.x * x_ax.z + v.y * y_ax.z + v.z * z_ax.z);
349 }
350 
351 inline wVector wMatrix::unrotateVector(const wVector &v) const {
352  return wVector( v%x_ax, v%y_ax, v%z_ax );
353 }
354 
355 inline wVector wMatrix::transformVector(const wVector &v) const {
356  return w_pos + rotateVector(v);
357 }
358 
359 inline wVector wMatrix::untransformVector(const wVector &v) const {
360  return unrotateVector( v-w_pos );
361 }
362 
363 inline void wMatrix::transformTriplex( real* dst, int dstStrideInBytes, real* src, int srcStrideInBytes, int count ) const {
364  real x;
365  real y;
366  real z;
367 
368  dstStrideInBytes /= sizeof(real);
369  srcStrideInBytes /= sizeof(real);
370 
371  for( int i=0; i<count; i++ ) {
372  x = src[0];
373  y = src[1];
374  z = src[2];
375  dst[0] = x*x_ax.x + y*y_ax.x + z*z_ax.x + w_pos.x;
376  dst[1] = x*x_ax.y + y*y_ax.y + z*z_ax.y + w_pos.y;
377  dst[2] = x*x_ax.z + y*y_ax.z + z*z_ax.z + w_pos.z;
378  dst += dstStrideInBytes;
379  src += srcStrideInBytes;
380  }
381 }
382 
383 inline wMatrix wMatrix::operator*( const wMatrix &B ) const {
384  const wMatrix& A = *this;
385 #ifdef FARSA_USE_GSL
386  wMatrix res;
387  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, 4, 4, 4, 1.0, A.data, 4, B.data, 4, 0.0, res.data, 4);
388  //cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, 4, 4, 4, 1.0, A.data, 4, B.data, 4, 0.0, res.data, 4);
389  return res;
390 #else
391  return wMatrix (wVector (A[0][0] * B[0][0] + A[0][1] * B[1][0] + A[0][2] * B[2][0] + A[0][3] * B[3][0],
392  A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
393  A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
394  A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
395  wVector (A[1][0] * B[0][0] + A[1][1] * B[1][0] + A[1][2] * B[2][0] + A[1][3] * B[3][0],
396  A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
397  A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
398  A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
399  wVector (A[2][0] * B[0][0] + A[2][1] * B[1][0] + A[2][2] * B[2][0] + A[2][3] * B[3][0],
400  A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
401  A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
402  A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
403  wVector (A[3][0] * B[0][0] + A[3][1] * B[1][0] + A[3][2] * B[2][0] + A[3][3] * B[3][0],
404  A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1],
405  A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2],
406  A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3]));
407 #endif
408 }
409 
410 inline wVector wMatrix::transformPlane( const wVector &localPlane ) const {
411  wVector tmp( rotateVector(localPlane) );
412  tmp.w = localPlane.w - ( localPlane%unrotateVector( w_pos ) );
413  return tmp;
414 }
415 
416 inline wVector wMatrix::untransformPlane( const wVector &globalPlane ) const {
417  wVector tmp( unrotateVector( globalPlane ) );
418  tmp.w = globalPlane % w_pos + globalPlane.w;
419  return tmp;
420 }
421 
422 inline bool wMatrix::sanityCheck() {
423  // it correspond to (x_ax * y_ax) % z_ax
424  real zz = (x_ax.y*y_ax.z - x_ax.z*y_ax.y)*z_ax.x
425  + (x_ax.z*y_ax.x - x_ax.x*y_ax.z)*z_ax.y
426  + (x_ax.x*y_ax.y - x_ax.y*y_ax.x)*z_ax.z;
427  //--- Gianluca: era <0.9999f e produceva molti errori, cmq nessuno al di sopra
428  // di 0.999f; forse era dovuto solo ad errori numerici per via della
429  // precisione (float)
430  if( fabs( zz ) < 0.99f) {
431  //--- comment out by Gianluca - qDebug() << zz;
432  return false;
433  }
434  x_ax.w = 0.0;
435  y_ax.w = 0.0;
436  z_ax.w = 0.0;
437  w_pos.w = 1.0;
438  return true;
439 }
440 
441 inline void wMatrix::sanitifize() {
442  //x_ax.normalize();
443  //y_ax.normalize();
444  //z_ax.normalize();
445  x_ax.w = 0.0;
446  y_ax.w = 0.0;
447  z_ax.w = 0.0;
448  w_pos.w = 1.0;
449 }
450 
451 inline wMatrix wMatrix::identity() {
452  return wMatrix( wVector (1.0f, 0.0f, 0.0f, 0.0f),
453  wVector (0.0f, 1.0f, 0.0f, 0.0f),
454  wVector (0.0f, 0.0f, 1.0f, 0.0f),
455  wVector (0.0f, 0.0f, 0.0f, 1.0f) );
456 }
457 
458 inline wMatrix wMatrix::zero() {
459  return wMatrix( wVector (0.0f, 0.0f, 0.0f, 0.0f),
460  wVector (0.0f, 0.0f, 0.0f, 0.0f),
461  wVector (0.0f, 0.0f, 0.0f, 0.0f),
462  wVector (0.0f, 0.0f, 0.0f, 0.0f) );
463 }
464 
465 inline wMatrix wMatrix::grammSchmidt(const wVector& dir) {
466  wVector X;
467  wVector Y;
468  wVector Z( dir );
469 
470  Z.normalize();
471  if( fabs(Z.z) > 0.577f ) {
472  X = Z * wVector( -Z.y, Z.z, 0.0f );
473  } else {
474  X = Z * wVector( -Z.y, Z.x, 0.0f );
475  }
476  X.normalize();
477  Y = Z * X;
478 
479  X.w = 0.0f;
480  Y.w = 0.0f;
481  Z.w = 0.0f;
482  return wMatrix( X, Y, Z, wVector(0.0f, 0.0f, 0.0f, 1.0f) );
483 }
484 
485 inline wMatrix wMatrix::pitch( real ang ) {
486  real cosAng = cos(ang);
487  real sinAng = sin(ang);
488  return wMatrix( wVector(1.0f, 0.0f, 0.0f, 0.0f),
489  wVector(0.0f, cosAng, sinAng, 0.0f),
490  wVector(0.0f, -sinAng, cosAng, 0.0f),
491  wVector(0.0f, 0.0f, 0.0f, 1.0f) );
492 }
493 
494 inline wMatrix wMatrix::yaw( real ang) {
495  real cosAng = cos(ang);
496  real sinAng = sin(ang);
497  return wMatrix( wVector (cosAng, 0.0f, -sinAng, 0.0f),
498  wVector (0.0f, 1.0f, 0.0f, 0.0f),
499  wVector (sinAng, 0.0f, cosAng, 0.0f),
500  wVector (0.0f, 0.0f, 0.0f, 1.0f) );
501 }
502 
503 inline wMatrix wMatrix::roll( real ang ) {
504  real cosAng = cos(ang);
505  real sinAng = sin(ang);
506  return wMatrix( wVector ( cosAng, sinAng, 0.0f, 0.0f),
507  wVector (-sinAng, cosAng, 0.0f, 0.0f),
508  wVector ( 0.0f, 0.0f, 1.0f, 0.0f),
509  wVector ( 0.0f, 0.0f, 0.0f, 1.0f) );
510 }
511 
512 } // end namespace farsa
513 
514 #endif