WorldSim - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
wquaternion.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 WQUATERNION_H
21 #define WQUATERNION_H
22 
23 #include "worldsimconfig.h"
24 #include "wvector.h"
25 
26 namespace farsa {
27 
28 class wMatrix;
29 
41 class FARSA_WSIM_TEMPLATE wQuaternion {
42 public:
44  wQuaternion();
46  wQuaternion( const wMatrix &matrix);
48  wQuaternion( real q0, real q1, real q2, real q3 );
50  wQuaternion( const wVector &unit_Axis, real Angle = 0.0 );
51 
52  void scale( real scale );
53  void normalize();
54  real dotProduct( const wQuaternion &QB ) const;
55  wQuaternion inverse() const;
56 
57  wVector rotateVector( const wVector& vector) const;
58  wVector unrotateVector( const wVector& vector ) const;
59 
60  wVector getEuleroAngles() const;
61  wQuaternion slerp( const wQuaternion &q1, real t ) const;
62  wVector calcAverageOmega( const wQuaternion &q1, real dt ) const;
63 
64  wQuaternion operator*( const wQuaternion &B ) const;
65  wQuaternion operator+( const wQuaternion &B ) const;
66  wQuaternion operator-( const wQuaternion &B ) const;
67 
68  real q0;
69  real q1;
70  real q2;
71  real q3;
72 };
73 
74 } // end namespace farsa
75 
76 //--- this is included here because wMatrix and wQuaternion has mutual dependencies
77 #include "wmatrix.h"
78 
79 namespace farsa {
80 
81 inline wQuaternion::wQuaternion() {
82  q0 = 1.0;
83  q1 = 0.0;
84  q2 = 0.0;
85  q3 = 0.0;
86 }
87 
88 inline wQuaternion::wQuaternion( const wMatrix &matrix ) {
89  enum QUAT_INDEX {
90  X_INDEX=0,
91  Y_INDEX=1,
92  Z_INDEX=2
93  };
94  static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
95 
96  real *ptr;
97  real trace;
98  QUAT_INDEX i;
99  QUAT_INDEX j;
100  QUAT_INDEX k;
101 
102  trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
103 
104  if( trace > 0.0 ) {
105  trace = sqrt( trace + 1.0 );
106  q0 = 0.5*trace;
107  trace = 0.5/trace;
108  q1 = (matrix[1][2] - matrix[2][1]) * trace;
109  q2 = (matrix[2][0] - matrix[0][2]) * trace;
110  q3 = (matrix[0][1] - matrix[1][0]) * trace;
111  } else {
112  i = X_INDEX;
113  if( matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX] ) {
114  i = Y_INDEX;
115  }
116  if( matrix[Z_INDEX][Z_INDEX] > matrix[i][i] ) {
117  i = Z_INDEX;
118  }
119  j = QIndex[i];
120  k = QIndex[j];
121 
122  trace = 1.0 + matrix[i][i] - matrix[j][j] - matrix[k][k];
123  trace = sqrt(trace);
124 
125  ptr = &q1;
126  ptr[i] = 0.5*trace;
127  trace = 0.5/trace;
128  q0 = (matrix[j][k] - matrix[k][j]) * trace;
129  ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
130  ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
131  }
132 }
133 
134 inline wQuaternion::wQuaternion(real Q0, real Q1, real Q2, real Q3) {
135  q0 = Q0;
136  q1 = Q1;
137  q2 = Q2;
138  q3 = Q3;
139  // INVARIANT: fabs( dotProduct(*this) - 1.0) < 1.0e-4f;
140 }
141 
142 inline wQuaternion::wQuaternion( const wVector &unitAxis, real angle ) {
143  angle *= 0.5;
144  q0 = cos( angle );
145  real sinAng = sin( angle );
146 
147  q1 = unitAxis.x * sinAng;
148  q2 = unitAxis.y * sinAng;
149  q3 = unitAxis.z * sinAng;
150 }
151 
152 inline void wQuaternion::scale(real scale) {
153  q0 *= scale;
154  q1 *= scale;
155  q2 *= scale;
156  q3 *= scale;
157 }
158 
159 inline void wQuaternion::normalize() {
160  scale( 1.0/sqrt( dotProduct(*this)) );
161 }
162 
163 inline real wQuaternion::dotProduct( const wQuaternion &QB ) const {
164  return q0*QB.q0 + q1*QB.q1 + q2*QB.q2 + q3*QB.q3;
165 }
166 
167 inline wQuaternion wQuaternion::inverse() const {
168  return wQuaternion(q0, -q1, -q2, -q3);
169 }
170 
171 inline wVector wQuaternion::rotateVector( const wVector& vector ) const {
172  wMatrix matrix( *this, wVector( 0.0f, 0.0f, 0.0f, 1.0f ) );
173  return matrix.rotateVector( vector );
174 }
175 
176 inline wVector wQuaternion::unrotateVector( const wVector& vector ) const {
177  wMatrix matrix( *this, wVector( 0.0f, 0.0f, 0.0f, 1.0f ) );
178  return matrix.unrotateVector( vector );
179 }
180 
181 inline wQuaternion wQuaternion::operator+(const wQuaternion &B) const {
182  return wQuaternion( q0+B.q0, q1+B.q1, q2+B.q2, q3+B.q3);
183 }
184 
185 inline wQuaternion wQuaternion::operator-( const wQuaternion &B ) const {
186  return wQuaternion( q0-B.q0, q1-B.q1, q2-B.q2, q3-B.q3);
187 }
188 
189 inline wQuaternion wQuaternion::operator*( const wQuaternion &B ) const {
190  return wQuaternion( B.q0*q0 - B.q1*q1 - B.q2*q2 - B.q3*q3,
191  B.q1*q0 + B.q0*q1 - B.q3*q2 + B.q2*q3,
192  B.q2*q0 + B.q3*q1 + B.q0*q2 - B.q1*q3,
193  B.q3*q0 - B.q2*q1 + B.q1*q2 + B.q0*q3 );
194 }
195 
196 inline wVector wQuaternion::getEuleroAngles() const {
197  real val;
198  real pitch;
199  real yaw;
200  real roll;
201 
202  val = 2.0f*( q2*q0 - q3*q1 );
203  if( val >= 0.99995f ) {
204  pitch = 2.0f*atan2( q1, q0 );
205  yaw = 0.5f*PI_GRECO;
206  roll = 0.0f;
207  } else if( val <= -0.99995f ) {
208  pitch = 2.0f*atan2( q1, q0 );
209  yaw = -0.5f*PI_GRECO;
210  roll = 0.0f;
211  } else {
212  yaw = sin (val);
213  pitch = atan2( 2.0f*( q1*q0 + q3*q2 ), 1.0f-2.0f*( q1*q1 + q2*q2) );
214  roll = atan2( 2.0f*( q3*q0 + q1*q2 ), 1.0f-2.0f*( q2*q2 + q3*q3) );
215  }
216  return wVector(pitch, yaw, roll);
217 }
218 
219 inline wVector wQuaternion::calcAverageOmega( const wQuaternion &QB, real dt ) const {
220  wQuaternion dq( inverse()*QB );
221  wVector omegaDir( dq.q1, dq.q2, dq.q3 );
222  real dirMag2 = omegaDir % omegaDir;
223  if( dirMag2 < 1.0e-10 ) {
224  return wVector( 0.0, 0.0, 0.0, 0.0 );
225  }
226  real dirMagInv = 1.0/sqrt( dirMag2 );
227  real dirMag = dirMag2 * dirMagInv;
228  real omegaMag = 2.0*atan2( dirMag, dq.q0 )/dt;
229 
230  return omegaDir.scale( dirMagInv * omegaMag );
231 }
232 
233 inline wQuaternion wQuaternion::slerp( const wQuaternion &QB, real t ) const {
234  real dot;
235  real ang;
236  real Sclp;
237  real Sclq;
238  real den;
239  real sinAng;
240  wQuaternion Q;
241 
242  dot = dotProduct( QB );
243 
244  if( (dot+1.0) > 1.0e-5f ) {
245  if( dot < 0.995f ) {
246  ang = cos( dot );
247  sinAng = sin( ang );
248  den = 1.0/sinAng;
249  Sclp = sin( (1.0-t) * ang ) * den;
250  Sclq = sin( t*ang ) * den;
251  } else {
252  Sclp = 1.0-t;
253  Sclq = t;
254  }
255  Q.q0 = q0*Sclp + QB.q0*Sclq;
256  Q.q1 = q1*Sclp + QB.q1*Sclq;
257  Q.q2 = q2*Sclp + QB.q2*Sclq;
258  Q.q3 = q3*Sclp + QB.q3*Sclq;
259  } else {
260  Q.q0 = q3;
261  Q.q1 = -q2;
262  Q.q2 = q1;
263  Q.q3 = q0;
264 
265  Sclp = sin(1.0-t) * PI_GRECO * 0.5;
266  Sclq = sin( t * PI_GRECO * 0.5 );
267 
268  Q.q0 = q0*Sclp + Q.q0*Sclq;
269  Q.q1 = q1*Sclp + Q.q1*Sclq;
270  Q.q2 = q2*Sclp + Q.q2*Sclq;
271  Q.q3 = q3*Sclp + Q.q3*Sclq;
272  }
273 
274  dot = Q.dotProduct( Q );
275  if( dot < (1.0-1.0e-4f) ) {
276  dot = 1.0/sqrt( dot );
277  Q.q0 *= dot;
278  Q.q1 *= dot;
279  Q.q2 *= dot;
280  Q.q3 *= dot;
281  }
282  return Q;
283 }
284 
285 } // end namespace farsa
286 
287 #endif