WorldSim - 1.4.5

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 <QString>
26 #include <cstring>
27 #include "mathutils.h"
28 
29 #ifdef FARSA_USE_GSL
30  #include <gsl/gsl_blas.h>
31 #endif
32 
33 namespace farsa {
34 
35 class wQuaternion;
36 
48 class FARSA_WSIM_TEMPLATE wMatrix {
49 public:
51  wMatrix();
58  wMatrix( const wVector &X, const wVector &Y, const wVector &Z, const wVector &P );
63  wMatrix( const wQuaternion &rotation, const wVector &position );
69  wMatrix( const real* position, const real* rotation );
70 
72  operator QString() const;
73 
75  wMatrix( const wMatrix &other );
77  wMatrix& operator=( const wMatrix &other );
78 
80  wVectorT<true>& operator[](int i);
82  const wVectorT<true>& operator[](int i) const;
83 
85  wMatrix inverse() const;
87  wMatrix transpose() const;
89  wMatrix transpose4X4() const;
91  wVector getEuleroAngles() const;
97  wMatrix rotateAround( const wVector& axis, const wVector& centre, real angle ) const;
99  wMatrix rotateMatrix( const wMatrix& tm ) const;
101  wVector rotateVector(const wVector &v) const;
103  wVector unrotateVector(const wVector &v) const;
105  wVector transformVector(const wVector &v) const;
107  wVector untransformVector(const wVector &v) const;
109  wVector transformPlane(const wVector &localPlane) const;
111  wVector untransformPlane(const wVector &globalPlane) const;
117  void transformTriplex( real* dst, int dstStrideInBytes, real* src, int srcStrideInBytes, int count) const;
118 
120  wMatrix operator*( const wMatrix &B ) const;
121 
123  bool sanityCheck();
125  void sanitifize();
126 
128  static wMatrix identity();
130  static wMatrix zero();
134  static wMatrix grammSchmidt(const wVector& dir);
136  static wMatrix pitch( real ang );
138  static wMatrix yaw( real ang );
140  static wMatrix roll( real ang );
141 
142  wVectorT<true> x_ax;
143  wVectorT<true> y_ax;
144  wVectorT<true> z_ax;
145  wVectorT<true> w_pos;
146 
147 private:
148  real data[16];
149  wVectorT<true>* vecs[4]; // This is only useful for operator[] functions
150 };
151 
152 } // end namespace farsa
153 
154 //--- this is included here because wMatrix and wQuaternion have mutual dependencies
155 #include "wquaternion.h"
156 
157 namespace farsa {
158 
159 inline wMatrix::wMatrix() : x_ax(&data[0]), y_ax(&data[4]), z_ax(&data[8]), w_pos(&data[12]) {
160  vecs[0] = &x_ax;
161  vecs[1] = &y_ax;
162  vecs[2] = &z_ax;
163  vecs[3] = &w_pos;
164 }
165 
166 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]) {
167  vecs[0] = &x_ax;
168  vecs[1] = &y_ax;
169  vecs[2] = &z_ax;
170  vecs[3] = &w_pos;
171  x_ax = X;
172  y_ax = Y;
173  z_ax = Z;
174  w_pos = P;
175 }
176 
177 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]) {
178  vecs[0] = &x_ax;
179  vecs[1] = &y_ax;
180  vecs[2] = &z_ax;
181  vecs[3] = &w_pos;
182  real x2;
183  real y2;
184  real z2;
185 // real w2;
186  real xy;
187  real xz;
188  real xw;
189  real yz;
190  real yw;
191  real zw;
192 
193 // w2 = 2.0 * rotation.q0 * rotation.q0; COMMENTED OUT BECAUSE IT IS UNUSED
194  x2 = 2.0 * rotation.q1 * rotation.q1;
195  y2 = 2.0 * rotation.q2 * rotation.q2;
196  z2 = 2.0 * rotation.q3 * rotation.q3;
197  // INVARIANT: fabs(w2 + x2 + y2 + z2 - 2.0) < 1.5e-3f;
198 
199  xy = 2.0 * rotation.q1 * rotation.q2;
200  xz = 2.0 * rotation.q1 * rotation.q3;
201  xw = 2.0 * rotation.q1 * rotation.q0;
202  yz = 2.0 * rotation.q2 * rotation.q3;
203  yw = 2.0 * rotation.q2 * rotation.q0;
204  zw = 2.0 * rotation.q3 * rotation.q0;
205 
206  x_ax = wVector( 1.0-y2-z2, xy+zw, xz-yw, 0.0 );
207  y_ax = wVector( xy-zw, 1.0-x2-z2, yz+xw, 0.0 );
208  z_ax = wVector( xz+yw, yz-xw, 1.0-x2-y2, 0.0 );
209 
210  w_pos.x = position.x;
211  w_pos.y = position.y;
212  w_pos.z = position.z;
213  w_pos.w = 1.0;
214 }
215 
216 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]) {
217  vecs[0] = &x_ax;
218  vecs[1] = &y_ax;
219  vecs[2] = &z_ax;
220  vecs[3] = &w_pos;
221  x_ax[0]=rot[0]; x_ax[1]=rot[1]; x_ax[2]=rot[2]; x_ax[3]=0.0 /*rot[3]*/;
222  y_ax[0]=rot[4]; y_ax[1]=rot[5]; y_ax[2]=rot[6]; y_ax[3]=0.0 /*rot[7]*/;
223  z_ax[0]=rot[8]; z_ax[1]=rot[9]; z_ax[2]=rot[10]; z_ax[3]=0.0 /*rot[11]*/;
224  w_pos[0]=pos[0]; w_pos[1]=pos[1]; w_pos[2]=pos[2]; w_pos[3]=1.0 /*pos[3*/;
225 }
226 
227 inline wMatrix::operator QString() const {
228  return QString("[%1, %2, %3, %4]").arg(x_ax).arg(y_ax).arg(z_ax).arg(w_pos);
229 }
230 
231 inline wMatrix::wMatrix( const wMatrix &other ) : x_ax(&data[0]), y_ax(&data[4]), z_ax(&data[8]), w_pos(&data[12]) {
232  vecs[0] = &x_ax;
233  vecs[1] = &y_ax;
234  vecs[2] = &z_ax;
235  vecs[3] = &w_pos;
236  memcpy(data, other.data, 16 * sizeof(real));
237 }
238 
239 inline wMatrix& wMatrix::operator=( const wMatrix &other )
240 {
241  if (&other == this) {
242  return *this;
243  }
244 
245  memcpy(data, other.data, 16 * sizeof(real));
246 
247  return *this;
248 }
249 
250 inline wVectorT<true>& wMatrix::operator[](int i) {
251  return *(vecs[i]);
252 }
253 
254 inline const wVectorT<true>& wMatrix::operator[](int i) const {
255  return *(vecs[i]);
256 }
257 
258 inline wMatrix wMatrix::inverse() const {
259  return wMatrix( wVector(x_ax.x, y_ax.x, z_ax.x, 0.0f),
260  wVector(x_ax.y, y_ax.y, z_ax.y, 0.0f),
261  wVector(x_ax.z, y_ax.z, z_ax.z, 0.0f),
262  wVector(- (w_pos % x_ax), - (w_pos % y_ax), - (w_pos % z_ax), 1.0f));
263 }
264 
265 inline wMatrix wMatrix::transpose() const {
266  return wMatrix( wVector(x_ax.x, y_ax.x, z_ax.x, 0.0f),
267  wVector(x_ax.y, y_ax.y, z_ax.y, 0.0f),
268  wVector(x_ax.z, y_ax.z, z_ax.z, 0.0f),
269  wVector(0.0f, 0.0f, 0.0f, 1.0f));
270 }
271 
272 inline wMatrix wMatrix::transpose4X4() const {
273  return wMatrix( wVector(x_ax.x, y_ax.x, z_ax.x, w_pos.x),
274  wVector(x_ax.y, y_ax.y, z_ax.y, w_pos.y),
275  wVector(x_ax.z, y_ax.z, z_ax.z, w_pos.z),
276  wVector(x_ax.w, y_ax.w, z_ax.w, w_pos.w));
277 }
278 
279 inline wVector wMatrix::getEuleroAngles() const {
280  real yaw;
281  real roll = 0.0;
282  real pitch = 0.0;
283  const real minSin = 0.99995f;
284  const wMatrix& matrix = *this;
285 
286  yaw = asin( -min( max( matrix[0][2], -0.999999 ), 0.999999) );
287  if( matrix[0][2] < minSin ) {
288  if( matrix[0][2] > -minSin ) {
289  roll = atan2( matrix[0][1], matrix[0][0] );
290  pitch = atan2( matrix[1][2], matrix[2][2] );
291  } else {
292  pitch = atan2( matrix[1][0], matrix[1][1] );
293  }
294  } else {
295  pitch = -atan2( matrix[1][0], matrix[1][1] );
296  }
297 
298  return wVector(pitch, yaw, roll, 0.0);
299 }
300 
301 inline wMatrix wMatrix::rotateAround( const wVector& axis, const wVector& centre, real angle ) const {
302  //--- OPTIMIZE_ME this routine is very inefficient. Please Optimize Me
303 /* wMatrix ret = *this;
304  wQuaternion ql( axis, angle );
305  wMatrix rot = wMatrix( ql, wVector(0,0,0) );
306  ret = rot.rotateMatrix( ret );
307  wVector c = w_pos - centre; //centre-w_pos;
308  qDebug() << c[0] << c[1] << c[2];
309  //ret.w_pos += rot.rotateVector( c ) - c;
310  ret.w_pos = centre + rot.rotateVector( c );
311  return ret;*/
312 
313  /*
314  wMatrix ret = *this;
315  wVector centrel = ret.w_pos - centre; //centre - ret.w_pos;
316  ret.w_pos = centrel;
317  wQuaternion ql( axis, angle );
318  ret = ret * wMatrix( ql, wVector(0,0,0) );
319  ret.w_pos += w_pos - centrel;
320  return ret;
321  */
322 
323  // --- using rotateAround of wVector
324  wMatrix ret = *this;
325  ret.x_ax.rotateAround( axis, angle );
326  ret.y_ax.rotateAround( axis, angle );
327  ret.z_ax.rotateAround( axis, angle );
328  wVector diffCentre = ret.w_pos - centre;
329  diffCentre.rotateAround( axis, angle );
330  ret.w_pos = diffCentre + centre;
331  return ret;
332 }
333 
334 inline wMatrix wMatrix::rotateMatrix( const wMatrix& B ) const {
335  const wMatrix& A = *this;
336  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],
337  A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
338  A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
339  A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
340  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],
341  A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
342  A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
343  A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
344  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],
345  A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
346  A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
347  A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
348  wVector ( A[3][3] * B[3][0],
349  A[3][3] * B[3][1],
350  A[3][3] * B[3][2],
351  A[3][3] * B[3][3]));
352 }
353 
354 inline wVector wMatrix::rotateVector(const wVector &v) const {
355  return wVector( v.x * x_ax.x + v.y * y_ax.x + v.z * z_ax.x,
356  v.x * x_ax.y + v.y * y_ax.y + v.z * z_ax.y,
357  v.x * x_ax.z + v.y * y_ax.z + v.z * z_ax.z);
358 }
359 
360 inline wVector wMatrix::unrotateVector(const wVector &v) const {
361  return wVector( v%x_ax, v%y_ax, v%z_ax );
362 }
363 
364 inline wVector wMatrix::transformVector(const wVector &v) const {
365  return w_pos + rotateVector(v);
366 }
367 
368 inline wVector wMatrix::untransformVector(const wVector &v) const {
369  return unrotateVector( v-w_pos );
370 }
371 
372 inline void wMatrix::transformTriplex( real* dst, int dstStrideInBytes, real* src, int srcStrideInBytes, int count ) const {
373  real x;
374  real y;
375  real z;
376 
377  dstStrideInBytes /= sizeof(real);
378  srcStrideInBytes /= sizeof(real);
379 
380  for( int i=0; i<count; i++ ) {
381  x = src[0];
382  y = src[1];
383  z = src[2];
384  dst[0] = x*x_ax.x + y*y_ax.x + z*z_ax.x + w_pos.x;
385  dst[1] = x*x_ax.y + y*y_ax.y + z*z_ax.y + w_pos.y;
386  dst[2] = x*x_ax.z + y*y_ax.z + z*z_ax.z + w_pos.z;
387  dst += dstStrideInBytes;
388  src += srcStrideInBytes;
389  }
390 }
391 
392 inline wMatrix wMatrix::operator*( const wMatrix &B ) const {
393  const wMatrix& A = *this;
394 #ifdef FARSA_USE_GSL
395  wMatrix res;
396  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, 4, 4, 4, 1.0, A.data, 4, B.data, 4, 0.0, res.data, 4);
397  //cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, 4, 4, 4, 1.0, A.data, 4, B.data, 4, 0.0, res.data, 4);
398  return res;
399 #else
400  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],
401  A[0][0] * B[0][1] + A[0][1] * B[1][1] + A[0][2] * B[2][1] + A[0][3] * B[3][1],
402  A[0][0] * B[0][2] + A[0][1] * B[1][2] + A[0][2] * B[2][2] + A[0][3] * B[3][2],
403  A[0][0] * B[0][3] + A[0][1] * B[1][3] + A[0][2] * B[2][3] + A[0][3] * B[3][3]),
404  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],
405  A[1][0] * B[0][1] + A[1][1] * B[1][1] + A[1][2] * B[2][1] + A[1][3] * B[3][1],
406  A[1][0] * B[0][2] + A[1][1] * B[1][2] + A[1][2] * B[2][2] + A[1][3] * B[3][2],
407  A[1][0] * B[0][3] + A[1][1] * B[1][3] + A[1][2] * B[2][3] + A[1][3] * B[3][3]),
408  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],
409  A[2][0] * B[0][1] + A[2][1] * B[1][1] + A[2][2] * B[2][1] + A[2][3] * B[3][1],
410  A[2][0] * B[0][2] + A[2][1] * B[1][2] + A[2][2] * B[2][2] + A[2][3] * B[3][2],
411  A[2][0] * B[0][3] + A[2][1] * B[1][3] + A[2][2] * B[2][3] + A[2][3] * B[3][3]),
412  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],
413  A[3][0] * B[0][1] + A[3][1] * B[1][1] + A[3][2] * B[2][1] + A[3][3] * B[3][1],
414  A[3][0] * B[0][2] + A[3][1] * B[1][2] + A[3][2] * B[2][2] + A[3][3] * B[3][2],
415  A[3][0] * B[0][3] + A[3][1] * B[1][3] + A[3][2] * B[2][3] + A[3][3] * B[3][3]));
416 #endif
417 }
418 
419 inline wVector wMatrix::transformPlane( const wVector &localPlane ) const {
420  wVector tmp( rotateVector(localPlane) );
421  tmp.w = localPlane.w - ( localPlane%unrotateVector( w_pos ) );
422  return tmp;
423 }
424 
425 inline wVector wMatrix::untransformPlane( const wVector &globalPlane ) const {
426  wVector tmp( unrotateVector( globalPlane ) );
427  tmp.w = globalPlane % w_pos + globalPlane.w;
428  return tmp;
429 }
430 
431 inline bool wMatrix::sanityCheck() {
432  // it correspond to (x_ax * y_ax) % z_ax
433  real zz = (x_ax.y*y_ax.z - x_ax.z*y_ax.y)*z_ax.x
434  + (x_ax.z*y_ax.x - x_ax.x*y_ax.z)*z_ax.y
435  + (x_ax.x*y_ax.y - x_ax.y*y_ax.x)*z_ax.z;
436  //--- Gianluca: era <0.9999f e produceva molti errori, cmq nessuno al di sopra
437  // di 0.999f; forse era dovuto solo ad errori numerici per via della
438  // precisione (float)
439  if( fabs( zz ) < 0.99f) {
440  //--- comment out by Gianluca - qDebug() << zz;
441  return false;
442  }
443  x_ax.w = 0.0;
444  y_ax.w = 0.0;
445  z_ax.w = 0.0;
446  w_pos.w = 1.0;
447  return true;
448 }
449 
450 inline void wMatrix::sanitifize() {
451  //x_ax.normalize();
452  //y_ax.normalize();
453  //z_ax.normalize();
454  x_ax.w = 0.0;
455  y_ax.w = 0.0;
456  z_ax.w = 0.0;
457  w_pos.w = 1.0;
458 }
459 
460 inline wMatrix wMatrix::identity() {
461  return wMatrix( wVector (1.0f, 0.0f, 0.0f, 0.0f),
462  wVector (0.0f, 1.0f, 0.0f, 0.0f),
463  wVector (0.0f, 0.0f, 1.0f, 0.0f),
464  wVector (0.0f, 0.0f, 0.0f, 1.0f) );
465 }
466 
467 inline wMatrix wMatrix::zero() {
468  return wMatrix( wVector (0.0f, 0.0f, 0.0f, 0.0f),
469  wVector (0.0f, 0.0f, 0.0f, 0.0f),
470  wVector (0.0f, 0.0f, 0.0f, 0.0f),
471  wVector (0.0f, 0.0f, 0.0f, 0.0f) );
472 }
473 
474 inline wMatrix wMatrix::grammSchmidt(const wVector& dir) {
475  wVector X;
476  wVector Y;
477  wVector Z( dir );
478 
479  Z.normalize();
480  if( fabs(Z.z) > 0.577f ) {
481  X = Z * wVector( -Z.y, Z.z, 0.0f );
482  } else {
483  X = Z * wVector( -Z.y, Z.x, 0.0f );
484  }
485  X.normalize();
486  Y = Z * X;
487 
488  X.w = 0.0f;
489  Y.w = 0.0f;
490  Z.w = 0.0f;
491  return wMatrix( X, Y, Z, wVector(0.0f, 0.0f, 0.0f, 1.0f) );
492 }
493 
494 inline wMatrix wMatrix::pitch( real ang ) {
495  real cosAng = cos(ang);
496  real sinAng = sin(ang);
497  return wMatrix( wVector(1.0f, 0.0f, 0.0f, 0.0f),
498  wVector(0.0f, cosAng, sinAng, 0.0f),
499  wVector(0.0f, -sinAng, cosAng, 0.0f),
500  wVector(0.0f, 0.0f, 0.0f, 1.0f) );
501 }
502 
503 inline wMatrix wMatrix::yaw( real ang) {
504  real cosAng = cos(ang);
505  real sinAng = sin(ang);
506  return wMatrix( wVector (cosAng, 0.0f, -sinAng, 0.0f),
507  wVector (0.0f, 1.0f, 0.0f, 0.0f),
508  wVector (sinAng, 0.0f, cosAng, 0.0f),
509  wVector (0.0f, 0.0f, 0.0f, 1.0f) );
510 }
511 
512 inline wMatrix wMatrix::roll( real ang ) {
513  real cosAng = cos(ang);
514  real sinAng = sin(ang);
515  return wMatrix( wVector ( cosAng, sinAng, 0.0f, 0.0f),
516  wVector (-sinAng, cosAng, 0.0f, 0.0f),
517  wVector ( 0.0f, 0.0f, 1.0f, 0.0f),
518  wVector ( 0.0f, 0.0f, 0.0f, 1.0f) );
519 }
520 
521 } // end namespace farsa
522 
523 #endif
farsa::wMatrix::identity
static wMatrix identity()
create an identity matrix
Definition: wmatrix.h:460
farsa::wMatrix::operator*
wMatrix operator*(const wMatrix &B) const
matrix multiplication
Definition: wmatrix.h:392
farsa::wMatrix::wMatrix
wMatrix()
Construct an unintialized matrix.
Definition: wmatrix.h:159
farsa::wMatrix::grammSchmidt
static wMatrix grammSchmidt(const wVector &dir)
calculate an orthonormal matrix with the Z vector pointing on the dir direction, and the Y and Z are ...
Definition: wmatrix.h:474
farsa::wMatrix::transformPlane
wVector transformPlane(const wVector &localPlane) const
...
Definition: wmatrix.h:419
farsa::wMatrix::getEuleroAngles
wVector getEuleroAngles() const
return the Eulero angle of rotation
Definition: wmatrix.h:279
farsa::wMatrix::untransformPlane
wVector untransformPlane(const wVector &globalPlane) const
...
Definition: wmatrix.h:425
farsa::wMatrix::inverse
wMatrix inverse() const
calculate the inverse of transformation matrix
Definition: wmatrix.h:258
farsa::max
FARSA_UTIL_TEMPLATE const T max(const T &t1, const U &t2)
farsa::wMatrix::operator[]
wVectorT< true > & operator[](int i)
indexing operator
Definition: wmatrix.h:250
farsa::wVectorT::normalize
wVectorT & normalize()
Normalize the vector.
Definition: wvector.h:310
farsa::wMatrix::roll
static wMatrix roll(real ang)
create a rotation around Z axis of ang radians
Definition: wmatrix.h:512
farsa::wMatrix::pitch
static wMatrix pitch(real ang)
create a rotation around X axis of ang radians
Definition: wmatrix.h:494
farsa::wMatrix
wMatrix class
Definition: wmatrix.h:48
farsa::wMatrix::sanityCheck
bool sanityCheck()
return true if the transformation matrix is valid
Definition: wmatrix.h:431
farsa::wMatrix::transpose4X4
wMatrix transpose4X4() const
full transposition of the transformation matrix
Definition: wmatrix.h:272
farsa::wVectorT::rotateAround
wVectorT & rotateAround(wVectorT< false > axis, real theta)
rotate the position indicated by this wVectorT around the axis by the angle theta ...
Definition: wvector.h:434
farsa::wMatrix::operator=
wMatrix & operator=(const wMatrix &other)
Copy operator.
Definition: wmatrix.h:239
farsa::wMatrix::transpose
wMatrix transpose() const
transpose the rotation sub-matrix
Definition: wmatrix.h:265
farsa::wMatrix::sanitifize
void sanitifize()
change the matrix in order to get a valid one that represent the same rotation/translation ...
Definition: wmatrix.h:450
farsa::wMatrix::rotateVector
wVector rotateVector(const wVector &v) const
rotate the vector v, it doesn't apply position transformation
Definition: wmatrix.h:354
farsa::wMatrix::rotateAround
wMatrix rotateAround(const wVector &axis, const wVector &centre, real angle) const
Rotate this matrix around the axis, and position passed of radians specified.
Definition: wmatrix.h:301
farsa::wMatrix::untransformVector
wVector untransformVector(const wVector &v) const
invert the rotation and translation on the vector v
Definition: wmatrix.h:368
farsa::wMatrix::rotateMatrix
wMatrix rotateMatrix(const wMatrix &tm) const
rotate the matrix passed by rotation specified by this and return a copy
Definition: wmatrix.h:334
farsa::real
float real
farsa::wMatrix::yaw
static wMatrix yaw(real ang)
create a rotation around Y axis of ang radians
Definition: wmatrix.h:503
farsa::min
FARSA_UTIL_TEMPLATE const T min(const T &t1, const U &t2)
farsa::wQuaternion
wQuaternion class
Definition: wquaternion.h:41
farsa::wMatrix::transformTriplex
void transformTriplex(real *dst, int dstStrideInBytes, real *src, int srcStrideInBytes, int count) const
apply transformation to a chunck of vectors
Definition: wmatrix.h:372
farsa::wMatrix::unrotateVector
wVector unrotateVector(const wVector &v) const
inverte the rotation of this matrix on the vector v
Definition: wmatrix.h:360
farsa::wMatrix::transformVector
wVector transformVector(const wVector &v) const
apply both rotation and translation to the vector v
Definition: wmatrix.h:364
farsa::wMatrix::zero
static wMatrix zero()
create a zero matrix
Definition: wmatrix.h:467