Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

orxonox/trunk: Font is now Right, and the Rendering 'should be' faster.

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