Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/orxonox/trunk/src/lib/math/vector.cc @ 4998

Last change on this file since 4998 was 4998, checked in by bensch, 19 years ago

orxonox/trunk: more optimizations of the Quaternion Class.
Now the 3D-rotation is much faster through this code:

Vector tmpRot = this→getAbsDir().getSpacialAxis();
glRotatef (this→getAbsDir().getSpacialAxisAngle(), tmpRot.x, tmpRot.y, tmpRot.z );

instead of the old Matrix-approach. furthermore glRotate is optimized much better in openGL as is clearly stated in the red book

also implemented some other really useless functions for Quaternion

File size: 14.4 KB
Line 
1/*
2   orxonox - the future of 3D-vertical-scrollers
3
4   Copyright (C) 2004 orx
5
6   This program is free software; you can redistribute it and/or modify
7   it under the terms of the GNU General Public License as published by
8   the Free Software Foundation; either version 2, or (at your option)
9   any later version.
10
11   ### File Specific:
12   main-programmer: Christian Meyer
13   co-programmer: Patrick Boenzli : Vector::scale()
14                                    Vector::abs()
15
16   Quaternion code borrowed from an Gamasutra article by Nick Bobick and Ken Shoemake
17*/
18
19#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH
20
21#include "vector.h"
22#include "debug.h"
23
24using namespace std;
25
26/////////////
27/* VECTORS */
28/////////////
29/**
30 *  returns the this-vector normalized to length 1.0
31 * @todo there is some error in this function, that i could not resolve. it just does not, what it is supposed to do.
32*/
33Vector Vector::getNormalized() const
34{
35  float l = this->len();
36  if(unlikely(l == 1.0 || l == 0.0))
37    return *this;
38  else
39    return (*this / l);
40}
41
42/**
43 *  Vector is looking in the positive direction on all axes after this
44*/
45Vector Vector::abs()
46{
47  Vector v(fabs(x), fabs(y), fabs(z));
48  return v;
49}
50
51
52
53/**
54 *  Outputs the values of the Vector
55*/
56void Vector::debug() const
57{
58  PRINT(0)("x: %f; y: %f; z: %f", x, y, z);
59  PRINT(0)(" lenght: %f", len());
60  PRINT(0)("\n");
61}
62
63/////////////////
64/* QUATERNIONS */
65/////////////////
66/**
67 *  calculates a lookAt rotation
68 * @param dir: the direction you want to look
69 * @param up: specify what direction up should be
70
71   Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point
72   the same way as dir. If you want to use this with cameras, you'll have to reverse the
73   dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You
74   can use this for meshes as well (then you do not have to reverse the vector), but keep
75   in mind that if you do that, the model's front has to point in +z direction, and left
76   and right should be -x or +x respectively or the mesh wont rotate correctly.
77*/
78Quaternion::Quaternion (const Vector& dir, const Vector& up)
79{
80  Vector z = dir;
81  z.normalize();
82  Vector x = up.cross(z);
83  x.normalize();
84  Vector y = z.cross(x);
85
86  float m[4][4];
87  m[0][0] = x.x;
88  m[0][1] = x.y;
89  m[0][2] = x.z;
90  m[0][3] = 0;
91  m[1][0] = y.x;
92  m[1][1] = y.y;
93  m[1][2] = y.z;
94  m[1][3] = 0;
95  m[2][0] = z.x;
96  m[2][1] = z.y;
97  m[2][2] = z.z;
98  m[2][3] = 0;
99  m[3][0] = 0;
100  m[3][1] = 0;
101  m[3][2] = 0;
102  m[3][3] = 1;
103
104  *this = Quaternion (m);
105}
106
107/**
108 *  calculates a rotation from euler angles
109 * @param roll: the roll in radians
110 * @param pitch: the pitch in radians
111 * @param yaw: the yaw in radians
112*/
113Quaternion::Quaternion (float roll, float pitch, float yaw)
114{
115  float cr, cp, cy, sr, sp, sy, cpcy, spsy;
116
117  // calculate trig identities
118  cr = cos(roll/2);
119  cp = cos(pitch/2);
120  cy = cos(yaw/2);
121
122  sr = sin(roll/2);
123  sp = sin(pitch/2);
124  sy = sin(yaw/2);
125
126  cpcy = cp * cy;
127  spsy = sp * sy;
128
129  w = cr * cpcy + sr * spsy;
130  v.x = sr * cpcy - cr * spsy;
131  v.y = cr * sp * cy + sr * cp * sy;
132  v.z = cr * cp * sy - sr * sp * cy;
133}
134
135/**
136 *  rotates one Quaternion by another
137 * @param q: another Quaternion to rotate this by
138 * @return a quaternion that represents the first one rotated by the second one (WARUNING: this operation is not commutative! e.g. (A*B) != (B*A))
139*/
140Quaternion Quaternion::operator*(const Quaternion& q) const
141{
142  float A, B, C, D, E, F, G, H;
143
144  A = (w   + v.x)*(q.w   + q.v.x);
145  B = (v.z - v.y)*(q.v.y - q.v.z);
146  C = (w   - v.x)*(q.v.y + q.v.z);
147  D = (v.y + v.z)*(q.w   - q.v.x);
148  E = (v.x + v.z)*(q.v.x + q.v.y);
149  F = (v.x - v.z)*(q.v.x - q.v.y);
150  G = (w   + v.y)*(q.w   - q.v.z);
151  H = (w   - v.y)*(q.w   + q.v.z);
152
153  Quaternion r;
154  r.v.x = A - (E + F + G + H)/2;
155  r.v.y = C + (E - F + G - H)/2;
156  r.v.z = D + (E - F - G + H)/2;
157  r.w = B +  (-E - F + G + H)/2;
158
159  return r;
160}
161
162/**
163 *  convert the Quaternion to a 4x4 rotational glMatrix
164 * @param m: a buffer to store the Matrix in
165*/
166void Quaternion::matrix (float m[4][4]) const
167{
168  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
169
170  // calculate coefficients
171  x2 = v.x + v.x;
172  y2 = v.y + v.y;
173  z2 = v.z + v.z;
174  xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;
175  yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;
176  wx = w * x2; wy = w * y2; wz = w * z2;
177
178  m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;
179  m[2][0] = xz + wy; m[3][0] = 0.0;
180
181  m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);
182  m[2][1] = yz - wx; m[3][1] = 0.0;
183
184  m[0][2] = xz - wy; m[1][2] = yz + wx;
185  m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;
186
187  m[0][3] = 0; m[1][3] = 0;
188  m[2][3] = 0; m[3][3] = 1;
189}
190
191/**
192 *  performs a smooth move.
193 * @param from  where
194 * @param to where
195 * @param t the time this transformation should take value [0..1]
196
197 * @returns the Result of the smooth move
198*/
199Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t)
200{
201  float tol[4];
202  double omega, cosom, sinom, scale0, scale1;
203  //  float DELTA = 0.2;
204
205  cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;
206
207  if( cosom < 0.0 )
208    {
209      cosom = -cosom;
210      tol[0] = -to.v.x;
211      tol[1] = -to.v.y;
212      tol[2] = -to.v.z;
213      tol[3] = -to.w;
214    }
215  else
216    {
217      tol[0] = to.v.x;
218      tol[1] = to.v.y;
219      tol[2] = to.v.z;
220      tol[3] = to.w;
221    }
222
223  //if( (1.0 - cosom) > DELTA )
224  //{
225  omega = acos(cosom);
226  sinom = sin(omega);
227  scale0 = sin((1.0 - t) * omega) / sinom;
228  scale1 = sin(t * omega) / sinom;
229  //}
230  /*
231    else
232    {
233    scale0 = 1.0 - t;
234    scale1 = t;
235    }
236  */
237
238
239  /*
240    Quaternion res;
241    res.v.x = scale0 * from.v.x + scale1 * tol[0];
242    res.v.y = scale0 * from.v.y + scale1 * tol[1];
243    res.v.z = scale0 * from.v.z + scale1 * tol[2];
244    res.w = scale0 * from.w + scale1 * tol[3];
245  */
246  return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],
247                           scale0 * from.v.y + scale1 * tol[1],
248                           scale0 * from.v.z + scale1 * tol[2]),
249                    scale0 * from.w + scale1 * tol[3]);
250}
251
252
253/**
254 *  convert a rotational 4x4 glMatrix into a Quaternion
255 * @param m: a 4x4 matrix in glMatrix order
256*/
257Quaternion::Quaternion (float m[4][4])
258{
259
260  float  tr, s, q[4];
261  int    i, j, k;
262
263  int nxt[3] = {1, 2, 0};
264
265  tr = m[0][0] + m[1][1] + m[2][2];
266
267        // check the diagonal
268  if (tr > 0.0)
269  {
270    s = sqrt (tr + 1.0);
271    w = s / 2.0;
272    s = 0.5 / s;
273    v.x = (m[1][2] - m[2][1]) * s;
274    v.y = (m[2][0] - m[0][2]) * s;
275    v.z = (m[0][1] - m[1][0]) * s;
276        }
277        else
278        {
279                // diagonal is negative
280        i = 0;
281        if (m[1][1] > m[0][0]) i = 1;
282    if (m[2][2] > m[i][i]) i = 2;
283    j = nxt[i];
284    k = nxt[j];
285
286    s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);
287
288    q[i] = s * 0.5;
289
290    if (s != 0.0) s = 0.5 / s;
291
292          q[3] = (m[j][k] - m[k][j]) * s;
293    q[j] = (m[i][j] + m[j][i]) * s;
294    q[k] = (m[i][k] + m[k][i]) * s;
295
296        v.x = q[0];
297        v.y = q[1];
298        v.z = q[2];
299        w = q[3];
300  }
301}
302
303/**
304 *  outputs some nice formated debug information about this quaternion
305*/
306void Quaternion::debug()
307{
308  PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);
309}
310
311/**
312 *  create a rotation from a vector
313 * @param v: a vector
314*/
315Rotation::Rotation (const Vector& v)
316{
317  Vector x = Vector( 1, 0, 0);
318  Vector axis = x.cross( v);
319  axis.normalize();
320  float angle = angleRad( x, v);
321  float ca = cos(angle);
322  float sa = sin(angle);
323  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
324  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
325  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
326  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
327  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
328  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
329  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
330  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
331  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
332}
333
334/**
335 *  creates a rotation from an axis and an angle (radians!)
336 * @param axis: the rotational axis
337 * @param angle: the angle in radians
338*/
339Rotation::Rotation (const Vector& axis, float angle)
340{
341  float ca, sa;
342  ca = cos(angle);
343  sa = sin(angle);
344  m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);
345  m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;
346  m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;
347  m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;
348  m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);
349  m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;
350  m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;
351  m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;
352  m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);
353}
354
355/**
356 *  creates a rotation from euler angles (pitch/yaw/roll)
357 * @param pitch: rotation around z (in radians)
358 * @param yaw: rotation around y (in radians)
359 * @param roll: rotation around x (in radians)
360*/
361Rotation::Rotation ( float pitch, float yaw, float roll)
362{
363  float cy, sy, cr, sr, cp, sp;
364  cy = cos(yaw);
365  sy = sin(yaw);
366  cr = cos(roll);
367  sr = sin(roll);
368  cp = cos(pitch);
369  sp = sin(pitch);
370  m[0] = cy*cr;
371  m[1] = -cy*sr;
372  m[2] = sy;
373  m[3] = cp*sr+sp*sy*cr;
374  m[4] = cp*cr-sp*sr*sy;
375  m[5] = -sp*cy;
376  m[6] = sp*sr-cp*sy*cr;
377  m[7] = sp*cr+cp*sy*sr;
378  m[8] = cp*cy;
379}
380
381/**
382 *  creates a nullrotation (an identity rotation)
383*/
384Rotation::Rotation ()
385{
386  m[0] = 1.0f;
387  m[1] = 0.0f;
388  m[2] = 0.0f;
389  m[3] = 0.0f;
390  m[4] = 1.0f;
391  m[5] = 0.0f;
392  m[6] = 0.0f;
393  m[7] = 0.0f;
394  m[8] = 1.0f;
395}
396
397/**
398 *  fills the specified buffer with a 4x4 glmatrix
399 * @param buffer: Pointer to an array of 16 floats
400
401   Use this to get the rotation in a gl-compatible format
402*/
403void Rotation::glmatrix (float* buffer)
404{
405        buffer[0] = m[0];
406        buffer[1] = m[3];
407        buffer[2] = m[6];
408        buffer[3] = m[0];
409        buffer[4] = m[1];
410        buffer[5] = m[4];
411        buffer[6] = m[7];
412        buffer[7] = m[0];
413        buffer[8] = m[2];
414        buffer[9] = m[5];
415        buffer[10] = m[8];
416        buffer[11] = m[0];
417        buffer[12] = m[0];
418        buffer[13] = m[0];
419        buffer[14] = m[0];
420        buffer[15] = m[1];
421}
422
423/**
424 *  multiplies two rotational matrices
425 * @param r: another Rotation
426 * @return the matrix product of the Rotations
427
428   Use this to rotate one rotation by another
429*/
430Rotation Rotation::operator* (const Rotation& r)
431{
432        Rotation p;
433
434        p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];
435        p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];
436        p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];
437
438        p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];
439        p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];
440        p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];
441
442        p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];
443        p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];
444        p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];
445
446        return p;
447}
448
449
450/**
451 *  rotates the vector by the given rotation
452 * @param v: a vector
453 * @param r: a rotation
454 * @return the rotated vector
455*/
456Vector rotateVector( const Vector& v, const Rotation& r)
457{
458  Vector t;
459
460  t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];
461  t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];
462  t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];
463
464  return t;
465}
466
467/**
468 *  calculate the distance between two lines
469 * @param l: the other line
470 * @return the distance between the lines
471*/
472float Line::distance (const Line& l) const
473{
474  float q, d;
475  Vector n = a.cross(l.a);
476  q = n.dot(r-l.r);
477  d = n.len();
478  if( d == 0.0) return 0.0;
479  return q/d;
480}
481
482/**
483 *  calculate the distance between a line and a point
484 * @param v: the point
485 * @return the distance between the Line and the point
486*/
487float Line::distancePoint (const Vector& v) const
488{
489  Vector d = v-r;
490  Vector u = a * d.dot( a);
491  return (d - u).len();
492}
493
494/**
495 *  calculate the distance between a line and a point
496 * @param v: the point
497 * @return the distance between the Line and the point
498 */
499float Line::distancePoint (const sVec3D& v) const
500{
501  Vector s(v[0], v[1], v[2]);
502  Vector d = s - r;
503  Vector u = a * d.dot( a);
504  return (d - u).len();
505}
506
507/**
508 *  calculate the two points of minimal distance of two lines
509 * @param l: the other line
510 * @return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
511*/
512Vector* Line::footpoints (const Line& l) const
513{
514  Vector* fp = new Vector[2];
515  Plane p = Plane (r + a.cross(l.a), r, r + a);
516  fp[1] = p.intersectLine (l);
517  p = Plane (fp[1], l.a);
518  fp[0] = p.intersectLine (*this);
519  return fp;
520}
521
522/**
523  \brief calculate the length of a line
524  \return the lenght of the line
525*/
526float Line::len() const
527{
528  return a.len();
529}
530
531/**
532 *  rotate the line by given rotation
533 * @param rot: a rotation
534*/
535void Line::rotate (const Rotation& rot)
536{
537  Vector t = a + r;
538  t = rotateVector( t, rot);
539  r = rotateVector( r, rot),
540  a = t - r;
541}
542
543/**
544 *  create a plane from three points
545 * @param a: first point
546 * @param b: second point
547 * @param c: third point
548*/
549Plane::Plane (Vector a, Vector b, Vector c)
550{
551  n = (a-b).cross(c-b);
552  k = -(n.x*b.x+n.y*b.y+n.z*b.z);
553}
554
555/**
556 *  create a plane from anchor point and normal
557 * @param norm: normal vector
558 * @param p: anchor point
559*/
560Plane::Plane (Vector norm, Vector p)
561{
562  n = norm;
563  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
564}
565
566
567/**
568  *  create a plane from anchor point and normal
569  * @param norm: normal vector
570  * @param p: anchor point
571*/
572Plane::Plane (Vector norm, sVec3D g)
573{
574  Vector p(g[0], g[1], g[2]);
575  n = norm;
576  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
577}
578
579
580/**
581 *  returns the intersection point between the plane and a line
582 * @param l: a line
583*/
584Vector Plane::intersectLine (const Line& l) const
585{
586  if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);
587  float t = (n.x*l.r.x+n.y*l.r.y+n.z*l.r.z+k) / (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z);
588  return l.r + (l.a * t);
589}
590
591/**
592 *  returns the distance between the plane and a point
593 * @param p: a Point
594 * @return the distance between the plane and the point (can be negative)
595*/
596float Plane::distancePoint (const Vector& p) const
597{
598  float l = n.len();
599  if( l == 0.0) return 0.0;
600  return (n.dot(p) + k) / n.len();
601}
602
603
604/**
605 *  returns the distance between the plane and a point
606 * @param p: a Point
607 * @return the distance between the plane and the point (can be negative)
608 */
609float Plane::distancePoint (const sVec3D& p) const
610{
611  Vector s(p[0], p[1], p[2]);
612  float l = n.len();
613  if( l == 0.0) return 0.0;
614  return (n.dot(s) + k) / n.len();
615}
616
617
618/**
619 *  returns the side a point is located relative to a Plane
620 * @param p: a Point
621 * @return 0 if the point is contained within the Plane, positive(negative) if the point is in the positive(negative) semi-space of the Plane
622*/
623float Plane::locatePoint (const Vector& p) const
624{
625  return (n.dot(p) + k);
626}
627
Note: See TracBrowser for help on using the repository browser.