WorldSim - 1.4.5

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
wvector.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 WVECTOR_H
21 #define WVECTOR_H
22 
23 #include "worldsimconfig.h"
24 #include "qglviewer/vec.h"
25 #include <QString>
26 #include <cmath>
27 #include "mathutils.h"
28 
29 namespace farsa {
30 
38 template <bool Shared>
39 struct FARSA_WSIM_TEMPLATE InternalData
40 {
41  real data[4];
42 };
43 
51 template <>
52 struct FARSA_WSIM_TEMPLATE InternalData<true>
53 {
54 };
55 
70 template <bool Shared = false>
71 class FARSA_WSIM_TEMPLATE wVectorT : protected InternalData<Shared> {
72 public:
74  wVectorT();
76  template <bool OtherShared>
77  wVectorT( const wVectorT<OtherShared>& );
79  wVectorT( const wVectorT& );
81  wVectorT( const real *ptr );
83  wVectorT( real *ptr );
85  wVectorT( real x, real y, real z, real w=1.0 );
86 
88  operator QString() const;
89 
91  operator qglviewer::Vec() const;
92 
94  wVectorT<false> scale( real s ) const;
98  wVectorT& normalize();
103  wVectorT<false> normalized() const;
104 
106  real& operator[]( int i );
108  const real& operator[]( int i ) const;
109 
111  wVectorT<false> operator-() const;
113  const wVectorT& operator+() const;
114 
116  template <bool OtherShared>
117  wVectorT<false> operator+( const wVectorT<OtherShared> &A ) const;
119  template <bool OtherShared>
120  wVectorT<false> operator-( const wVectorT<OtherShared> &A ) const;
122  wVectorT& operator=( const wVectorT &A );
124  template <bool OtherShared>
125  wVectorT& operator=( const wVectorT<OtherShared> &A );
127  template <bool OtherShared>
128  wVectorT& operator+=( const wVectorT<OtherShared> &A );
130  template <bool OtherShared>
131  wVectorT& operator-=( const wVectorT<OtherShared> &A );
132 
134  template <bool OtherShared>
135  bool operator==( const wVectorT<OtherShared> &A ) const;
136 
138  template <bool OtherShared>
139  real operator%( const wVectorT<OtherShared> &A ) const;
140 
142  template <bool OtherShared>
143  wVectorT<false> operator*( const wVectorT<OtherShared> &B ) const;
144 
146  template <bool OtherShared>
147  wVectorT<false> compProduct( const wVectorT<OtherShared> &A ) const;
148 
150  real norm() const;
151 
153  template <bool SharedA, bool SharedB>
154  static real distance( const wVectorT<SharedA> &A, const wVectorT<SharedB> &B );
155 
161  //template <bool OtherShared>
162  wVectorT& rotateAround( wVectorT<false> axis, real theta );
163 
165  static wVectorT<false> X();
167  static wVectorT<false> Y();
169  static wVectorT<false> Z();
170 
171  real &x;
172  real &y;
173  real &z;
174  real &w;
175 };
176 
178 typedef wVectorT<false> wVector;
179 
180 template <bool Shared>
181 inline wVectorT<false> wVectorT<Shared>::X() {
182  return wVectorT<false>(1,0,0,0);
183 }
184 
185 template <bool Shared>
186 inline wVectorT<false> wVectorT<Shared>::Y() {
187  return wVectorT<false>(0,1,0,0);
188 }
189 
190 template <bool Shared>
191 inline wVectorT<false> wVectorT<Shared>::Z() {
192  return wVectorT<false>(0,0,1,0);
193 }
194 
195 template <>
196 inline wVectorT<false>::wVectorT() :
197  x(InternalData<false>::data[0]),
198  y(InternalData<false>::data[1]),
199  z(InternalData<false>::data[2]),
200  w(InternalData<false>::data[3])
201 {
202  x = 0.0;
203  y = 0.0;
204  z = 0.0;
205  w = 1.0;
206 }
207 
208 template <>
209 template <bool OtherShared>
210 inline wVectorT<false>::wVectorT( const wVectorT<OtherShared>& src ) :
211  x(InternalData<false>::data[0]),
212  y(InternalData<false>::data[1]),
213  z(InternalData<false>::data[2]),
214  w(InternalData<false>::data[3])
215 {
216  x = src.x;
217  y = src.y;
218  z = src.z;
219  w = src.w;
220 }
221 
222 template <>
223 inline wVectorT<false>::wVectorT( const wVectorT<false>& src ) :
224  x(InternalData<false>::data[0]),
225  y(InternalData<false>::data[1]),
226  z(InternalData<false>::data[2]),
227  w(InternalData<false>::data[3])
228 {
229  x = src.x;
230  y = src.y;
231  z = src.z;
232  w = src.w;
233 }
234 
235 template <>
236 inline wVectorT<false>::wVectorT(const real *ptr) :
237  x(InternalData<false>::data[0]),
238  y(InternalData<false>::data[1]),
239  z(InternalData<false>::data[2]),
240  w(InternalData<false>::data[3])
241 {
242  x = ptr[0];
243  y = ptr[1];
244  z = ptr[2];
245  w = 1.0;
246 }
247 
248 template <bool Shared>
249 inline wVectorT<Shared>::wVectorT(real *ptr) :
250  x(InternalData<Shared>::data[0]),
251  y(InternalData<Shared>::data[1]),
252  z(InternalData<Shared>::data[2]),
253  w(InternalData<Shared>::data[3])
254 {
255  x = ptr[0];
256  y = ptr[1];
257  z = ptr[2];
258  w = 1.0;
259 }
260 
261 template <>
262 inline wVectorT<true>::wVectorT(real *ptr) :
263  x(ptr[0]),
264  y(ptr[1]),
265  z(ptr[2]),
266  w(ptr[3])
267 {
268  w = 1.0;
269 }
270 
271 template <>
272 inline wVectorT<false>::wVectorT(real _x, real _y, real _z, real _w) :
273  x(InternalData<false>::data[0]),
274  y(InternalData<false>::data[1]),
275  z(InternalData<false>::data[2]),
276  w(InternalData<false>::data[3])
277 {
278  this->x = _x;
279  this->y = _y;
280  this->z = _z;
281  this->w = _w;
282 }
283 
284 template <bool Shared>
285 inline wVectorT<Shared>::operator QString() const {
286  return QString("[%1, %2, %3, %4]").arg(x).arg(y).arg(z).arg(w);
287 }
288 
289 template <bool Shared>
290 inline wVectorT<Shared>::operator qglviewer::Vec() const {
291  return qglviewer::Vec( x, y, z );
292 }
293 
294 template <bool Shared>
295 inline real& wVectorT<Shared>::operator[](int i) {
296  return (&x)[i];
297 }
298 
299 template <bool Shared>
300 inline const real& wVectorT<Shared>::operator[](int i) const {
301  return (&x)[i];
302 }
303 
304 template <bool Shared>
305 inline wVectorT<false> wVectorT<Shared>::scale(real scale) const {
306  return wVectorT<false>(x*scale, y*scale, z*scale, w);
307 }
308 
309 template <bool Shared>
310 inline wVectorT<Shared>& wVectorT<Shared>::normalize() {
311  wVectorT& self = (*this);
312  const real n = sqrt( self%self );
313  self.x /= n;
314  self.y /= n;
315  self.z /= n;
316  self.w = 0.0;
317  return self;
318 }
319 
320 template <bool Shared>
321 inline wVectorT<false> wVectorT<Shared>::normalized() const {
322  wVector ret;
323  const real n = norm();
324  ret.x = x / n;
325  ret.y = y / n;
326  ret.z = z / n;
327  ret.w = 0.0;
328  return ret;
329 }
330 
331 template <bool Shared>
332 inline wVectorT<false> wVectorT<Shared>::operator-() const {
333  return wVectorT<false>( -x, -y, -z, w );
334 }
335 
336 template <bool Shared>
337 inline const wVectorT<Shared>& wVectorT<Shared>::operator+() const {
338  return (*this);
339 }
340 
341 template <bool Shared>
342 template <bool OtherShared>
343 inline wVectorT<false> wVectorT<Shared>::operator+(const wVectorT<OtherShared> &B) const {
344  return wVectorT<false>(x+B.x, y+B.y, z+B.z, w);
345 }
346 
347 template <bool Shared>
348 template <bool OtherShared>
349 inline wVectorT<false> wVectorT<Shared>::operator-(const wVectorT<OtherShared> &A) const {
350  return wVectorT<false>(x-A.x, y-A.y, z-A.z, w);
351 }
352 
353 template <bool Shared>
354 inline wVectorT<Shared>& wVectorT<Shared>::operator=( const wVectorT &A )
355 {
356  x = A.x;
357  y = A.y;
358  z = A.z;
359  w = A.w;
360  return *this;
361 }
362 
363 template <bool Shared>
364 template <bool OtherShared>
365 inline wVectorT<Shared>& wVectorT<Shared>::operator=(const wVectorT<OtherShared> &A) {
366  x = A.x;
367  y = A.y;
368  z = A.z;
369  w = A.w;
370  return *this;
371 }
372 
373 template <bool Shared>
374 template <bool OtherShared>
375 inline wVectorT<Shared>& wVectorT<Shared>::operator+=(const wVectorT<OtherShared> &A) {
376  x += A.x;
377  y += A.y;
378  z += A.z;
379  w = 1.0;
380  return *this;
381 }
382 
383 template <bool Shared>
384 template <bool OtherShared>
385 inline wVectorT<Shared>& wVectorT<Shared>::operator-=(const wVectorT<OtherShared> &A) {
386  x -= A.x;
387  y -= A.y;
388  z -= A.z;
389  w = 1.0;
390  return *this;
391 }
392 
393 template <bool Shared>
394 template <bool OtherShared>
395 inline bool wVectorT<Shared>::operator==( const wVectorT<OtherShared> &A ) const {
396  return ( x==A.x && y==A.y && z==A.z );
397 }
398 
399 template <bool Shared>
400 template <bool OtherShared>
401 inline real wVectorT<Shared>::operator%(const wVectorT<OtherShared> &A) const {
402  return x*A.x + y*A.y + z*A.z;
403 }
404 
405 template <bool Shared>
406 template <bool OtherShared>
407 inline wVectorT<false> wVectorT<Shared>::operator*(const wVectorT<OtherShared> &B) const {
408  return wVectorT<false>(
409  y*B.z - z*B.y,
410  z*B.x - x*B.z,
411  x*B.y - y*B.x,
412  w);
413 }
414 
415 template <bool Shared>
416 template <bool OtherShared>
417 inline wVectorT<false> wVectorT<Shared>::compProduct(const wVectorT<OtherShared> &A) const {
418  return wVectorT<false>(x*A.x, y*A.y, z*A.z, A.w);
419 }
420 
421 template <bool Shared>
422 inline real wVectorT<Shared>::norm() const {
423  return sqrt( x*x + y*y + z*z );
424 }
425 
426 template <bool Shared>
427 template <bool SharedA, bool SharedB>
428 inline real wVectorT<Shared>::distance( const wVectorT<SharedA> &A, const wVectorT<SharedB> &B ) {
429  return sqrt( (A.x-B.x)*(A.x-B.x) + (A.y-B.y)*(A.y-B.y) + (A.z-B.z)*(A.z-B.z) );
430 }
431 
432 template <bool Shared>
433 //template <bool OtherShared>
434 inline wVectorT<Shared>& wVectorT<Shared>::rotateAround( wVectorT<false> axis, real theta ) {
435  wVector q(0,0,0);
436  double costheta, sintheta;
437  axis.normalize();
438  costheta = cos(theta);
439  sintheta = sin(theta);
440  q.x += (costheta + (1 - costheta) * axis.x * axis.x) * x;
441  q.x += ((1 - costheta) * axis.x * axis.y - axis.z * sintheta) * y;
442  q.x += ((1 - costheta) * axis.x * axis.z + axis.y * sintheta) * z;
443 
444  q.y += ((1 - costheta) * axis.x * axis.y + axis.z * sintheta) * x;
445  q.y += (costheta + (1 - costheta) * axis.y * axis.y) * y;
446  q.y += ((1 - costheta) * axis.y * axis.z - axis.x * sintheta) * z;
447 
448  q.z += ((1 - costheta) * axis.x * axis.z - axis.y * sintheta) * x;
449  q.z += ((1 - costheta) * axis.y * axis.z + axis.x * sintheta) * y;
450  q.z += (costheta + (1 - costheta) * axis.z * axis.z) * z;
451  x = q.x;
452  y = q.y;
453  z = q.z;
454  return (*this);
455 }
456 } // end namespace farsa
457 
458 #endif