Changeset 6616 in orxonox.OLD for trunk/src/lib/math
- Timestamp:
- Jan 19, 2006, 12:27:45 PM (19 years ago)
- Location:
- trunk/src/lib/math
- Files:
-
- 4 edited
- 2 copied
Legend:
- Unmodified
- Added
- Removed
-
trunk/src/lib/math/curve.h
r5231 r6616 11 11 12 12 #include "vector.h" 13 #include "quaternion.h" 13 14 14 15 template<class T> class tList; -
trunk/src/lib/math/quaternion.cc
r6614 r6616 21 21 #define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH 22 22 23 #include " vector.h"23 #include "quaternion.h" 24 24 #ifdef DEBUG 25 25 #include "debug.h" … … 30 30 31 31 using namespace std; 32 33 /////////////34 /* VECTORS */35 /////////////36 /**37 * returns the this-vector normalized to length 1.038 * @todo there is some error in this function, that i could not resolve. it just does not, what it is supposed to do.39 */40 Vector Vector::getNormalized() const41 {42 float l = this->len();43 if(unlikely(l == 1.0 || l == 0.0))44 return *this;45 else46 return (*this / l);47 }48 49 /**50 * Vector is looking in the positive direction on all axes after this51 */52 Vector Vector::abs()53 {54 Vector v(fabs(x), fabs(y), fabs(z));55 return v;56 }57 58 59 60 /**61 * Outputs the values of the Vector62 */63 void Vector::debug() const64 {65 PRINT(0)("x: %f; y: %f; z: %f", x, y, z);66 PRINT(0)(" lenght: %f", len());67 PRINT(0)("\n");68 }69 32 70 33 ///////////////// … … 274 237 PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z ); 275 238 } 276 277 /**278 * create a rotation from a vector279 * @param v: a vector280 */281 Rotation::Rotation (const Vector& v)282 {283 Vector x = Vector( 1, 0, 0);284 Vector axis = x.cross( v);285 axis.normalize();286 float angle = angleRad( x, v);287 float ca = cos(angle);288 float sa = sin(angle);289 m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);290 m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;291 m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;292 m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;293 m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);294 m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;295 m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;296 m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;297 m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);298 }299 300 /**301 * creates a rotation from an axis and an angle (radians!)302 * @param axis: the rotational axis303 * @param angle: the angle in radians304 */305 Rotation::Rotation (const Vector& axis, float angle)306 {307 float ca, sa;308 ca = cos(angle);309 sa = sin(angle);310 m[0] = 1.0f+(1.0f-ca)*(axis.x*axis.x-1.0f);311 m[1] = -axis.z*sa+(1.0f-ca)*axis.x*axis.y;312 m[2] = axis.y*sa+(1.0f-ca)*axis.x*axis.z;313 m[3] = axis.z*sa+(1.0f-ca)*axis.x*axis.y;314 m[4] = 1.0f+(1.0f-ca)*(axis.y*axis.y-1.0f);315 m[5] = -axis.x*sa+(1.0f-ca)*axis.y*axis.z;316 m[6] = -axis.y*sa+(1.0f-ca)*axis.x*axis.z;317 m[7] = axis.x*sa+(1.0f-ca)*axis.y*axis.z;318 m[8] = 1.0f+(1.0f-ca)*(axis.z*axis.z-1.0f);319 }320 321 /**322 * creates a rotation from euler angles (pitch/yaw/roll)323 * @param pitch: rotation around z (in radians)324 * @param yaw: rotation around y (in radians)325 * @param roll: rotation around x (in radians)326 */327 Rotation::Rotation ( float pitch, float yaw, float roll)328 {329 float cy, sy, cr, sr, cp, sp;330 cy = cos(yaw);331 sy = sin(yaw);332 cr = cos(roll);333 sr = sin(roll);334 cp = cos(pitch);335 sp = sin(pitch);336 m[0] = cy*cr;337 m[1] = -cy*sr;338 m[2] = sy;339 m[3] = cp*sr+sp*sy*cr;340 m[4] = cp*cr-sp*sr*sy;341 m[5] = -sp*cy;342 m[6] = sp*sr-cp*sy*cr;343 m[7] = sp*cr+cp*sy*sr;344 m[8] = cp*cy;345 }346 347 /**348 * creates a nullrotation (an identity rotation)349 */350 Rotation::Rotation ()351 {352 m[0] = 1.0f;353 m[1] = 0.0f;354 m[2] = 0.0f;355 m[3] = 0.0f;356 m[4] = 1.0f;357 m[5] = 0.0f;358 m[6] = 0.0f;359 m[7] = 0.0f;360 m[8] = 1.0f;361 }362 363 /**364 * fills the specified buffer with a 4x4 glmatrix365 * @param buffer: Pointer to an array of 16 floats366 367 Use this to get the rotation in a gl-compatible format368 */369 void Rotation::glmatrix (float* buffer)370 {371 buffer[0] = m[0];372 buffer[1] = m[3];373 buffer[2] = m[6];374 buffer[3] = m[0];375 buffer[4] = m[1];376 buffer[5] = m[4];377 buffer[6] = m[7];378 buffer[7] = m[0];379 buffer[8] = m[2];380 buffer[9] = m[5];381 buffer[10] = m[8];382 buffer[11] = m[0];383 buffer[12] = m[0];384 buffer[13] = m[0];385 buffer[14] = m[0];386 buffer[15] = m[1];387 }388 389 /**390 * multiplies two rotational matrices391 * @param r: another Rotation392 * @return the matrix product of the Rotations393 394 Use this to rotate one rotation by another395 */396 Rotation Rotation::operator* (const Rotation& r)397 {398 Rotation p;399 400 p.m[0] = m[0]*r.m[0] + m[1]*r.m[3] + m[2]*r.m[6];401 p.m[1] = m[0]*r.m[1] + m[1]*r.m[4] + m[2]*r.m[7];402 p.m[2] = m[0]*r.m[2] + m[1]*r.m[5] + m[2]*r.m[8];403 404 p.m[3] = m[3]*r.m[0] + m[4]*r.m[3] + m[5]*r.m[6];405 p.m[4] = m[3]*r.m[1] + m[4]*r.m[4] + m[5]*r.m[7];406 p.m[5] = m[3]*r.m[2] + m[4]*r.m[5] + m[5]*r.m[8];407 408 p.m[6] = m[6]*r.m[0] + m[7]*r.m[3] + m[8]*r.m[6];409 p.m[7] = m[6]*r.m[1] + m[7]*r.m[4] + m[8]*r.m[7];410 p.m[8] = m[6]*r.m[2] + m[7]*r.m[5] + m[8]*r.m[8];411 412 return p;413 }414 415 416 /**417 * rotates the vector by the given rotation418 * @param v: a vector419 * @param r: a rotation420 * @return the rotated vector421 */422 Vector rotateVector( const Vector& v, const Rotation& r)423 {424 Vector t;425 426 t.x = v.x * r.m[0] + v.y * r.m[1] + v.z * r.m[2];427 t.y = v.x * r.m[3] + v.y * r.m[4] + v.z * r.m[5];428 t.z = v.x * r.m[6] + v.y * r.m[7] + v.z * r.m[8];429 430 return t;431 }432 433 /**434 * calculate the distance between two lines435 * @param l: the other line436 * @return the distance between the lines437 */438 float Line::distance (const Line& l) const439 {440 float q, d;441 Vector n = a.cross(l.a);442 q = n.dot(r-l.r);443 d = n.len();444 if( d == 0.0) return 0.0;445 return q/d;446 }447 448 /**449 * calculate the distance between a line and a point450 * @param v: the point451 * @return the distance between the Line and the point452 */453 float Line::distancePoint (const Vector& v) const454 {455 Vector d = v-r;456 Vector u = a * d.dot( a);457 return (d - u).len();458 }459 460 /**461 * calculate the distance between a line and a point462 * @param v: the point463 * @return the distance between the Line and the point464 */465 float Line::distancePoint (const sVec3D& v) const466 {467 Vector s(v[0], v[1], v[2]);468 Vector d = s - r;469 Vector u = a * d.dot( a);470 return (d - u).len();471 }472 473 /**474 * calculate the two points of minimal distance of two lines475 * @param l: the other line476 * @return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance477 */478 Vector* Line::footpoints (const Line& l) const479 {480 Vector* fp = new Vector[2];481 Plane p = Plane (r + a.cross(l.a), r, r + a);482 fp[1] = p.intersectLine (l);483 p = Plane (fp[1], l.a);484 fp[0] = p.intersectLine (*this);485 return fp;486 }487 488 /**489 \brief calculate the length of a line490 \return the lenght of the line491 */492 float Line::len() const493 {494 return a.len();495 }496 497 /**498 * rotate the line by given rotation499 * @param rot: a rotation500 */501 void Line::rotate (const Rotation& rot)502 {503 Vector t = a + r;504 t = rotateVector( t, rot);505 r = rotateVector( r, rot),506 a = t - r;507 }508 509 /**510 * create a plane from three points511 * @param a: first point512 * @param b: second point513 * @param c: third point514 */515 Plane::Plane (const Vector& a, const Vector& b, const Vector& c)516 {517 n = (a-b).cross(c-b);518 k = -(n.x*b.x+n.y*b.y+n.z*b.z);519 }520 521 /**522 * create a plane from anchor point and normal523 * @param norm: normal vector524 * @param p: anchor point525 */526 Plane::Plane (const Vector& norm, const Vector& p)527 {528 n = norm;529 k = -(n.x*p.x+n.y*p.y+n.z*p.z);530 }531 532 533 /**534 * create a plane from anchor point and normal535 * @param norm: normal vector536 * @param p: anchor point537 */538 Plane::Plane (const Vector& norm, const sVec3D& g)539 {540 Vector p(g[0], g[1], g[2]);541 n = norm;542 k = -(n.x*p.x+n.y*p.y+n.z*p.z);543 }544 545 546 /**547 * returns the intersection point between the plane and a line548 * @param l: a line549 */550 Vector Plane::intersectLine (const Line& l) const551 {552 if (n.x*l.a.x+n.y*l.a.y+n.z*l.a.z == 0.0) return Vector(0,0,0);553 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);554 return l.r + (l.a * t);555 }556 557 /**558 * returns the distance between the plane and a point559 * @param p: a Point560 * @return the distance between the plane and the point (can be negative)561 */562 float Plane::distancePoint (const Vector& p) const563 {564 float l = n.len();565 if( l == 0.0) return 0.0;566 return (n.dot(p) + k) / n.len();567 }568 569 570 /**571 * returns the distance between the plane and a point572 * @param p: a Point573 * @return the distance between the plane and the point (can be negative)574 */575 float Plane::distancePoint (const sVec3D& p) const576 {577 Vector s(p[0], p[1], p[2]);578 float l = n.len();579 if( l == 0.0) return 0.0;580 return (n.dot(s) + k) / n.len();581 }582 583 584 /**585 * returns the side a point is located relative to a Plane586 * @param p: a Point587 * @return 0 if the point is contained within the Plane, positive(negative) if the point is in the positive(negative) semi-space of the Plane588 */589 float Plane::locatePoint (const Vector& p) const590 {591 return (n.dot(p) + k);592 }593 -
trunk/src/lib/math/quaternion.h
r6614 r6616 15 15 16 16 /*! 17 * @file vector.h18 * A basic 3D math framework17 * @file quaternion.h 18 * A basic 3D quaternion math framework 19 19 * 20 20 * Contains classes to handle vectors, lines, rotations and planes 21 21 */ 22 22 23 #ifndef __ VECTOR_H_24 #define __ VECTOR_H_23 #ifndef __QUATERNION_H_ 24 #define __QUATERNION_H_ 25 25 26 26 #include <math.h> … … 28 28 //! PI the circle-constant 29 29 #define PI 3.14159265359f 30 31 32 //! this is a small and performant 3D vector 33 typedef float sVec3D[3]; 34 35 36 //! small and performant 2D vector 37 typedef float sVec2D[2]; 38 39 40 41 //! 3D Vector 42 /** 43 Class to handle 3D Vectors 44 */ 45 class Vector { 46 public: 47 Vector (float x, float y, float z) : x(x), y(y), z(z) {} //!< assignment constructor 48 Vector () : x(0), y(0), z(0) {} 49 ~Vector () {} 50 51 /** @param v: the Vecor to compare with this one @returns true, if the Vecors are the same, false otherwise */ 52 inline bool operator== (const Vector& v) const { return (this->x==v.x&&this->y==v.y&&this->z==v.z)?true:false; }; 53 /** @param index The index of the "array" @returns the x/y/z coordinate */ 54 inline float operator[] (float index) const {if( index == 0) return this->x; if( index == 1) return this->y; if( index == 2) return this->z; } 55 /** @param v The vector to add @returns the addition between two vectors (this + v) */ 56 inline Vector operator+ (const Vector& v) const { return Vector(x + v.x, y + v.y, z + v.z); }; 57 /** @param v The vector to add @returns the addition between two vectors (this + v) */ 58 inline Vector operator+ (const sVec3D& v) const { return Vector(x + v[0], y + v[1], z + v[2]); }; 59 /** @param v The vector to add @returns the addition between two vectors (this += v) */ 60 inline const Vector& operator+= (const Vector& v) { this->x += v.x; this->y += v.y; this->z += v.z; return *this; }; 61 /** @param v The vector to substract @returns the substraction between two vectors (this - v) */ 62 inline const Vector& operator+= (const sVec3D& v) { this->x += v[0]; this->y += v[1]; this->z += v[2]; return *this; }; 63 /** @param v The vector to substract @returns the substraction between two vectors (this - v) */ 64 inline Vector operator- (const Vector& v) const { return Vector(x - v.x, y - v.y, z - v.z); } 65 /** @param v The vector to substract @returns the substraction between two vectors (this - v) */ 66 inline Vector operator- (const sVec3D& v) const { return Vector(x - v[0], y - v[1], z - v[2]); } 67 /** @param v The vector to substract @returns the substraction between two vectors (this -= v) */ 68 inline const Vector& operator-= (const Vector& v) { this->x -= v.x; this->y -= v.y; this->z -= v.z; return *this; }; 69 /** @param v The vector to substract @returns the substraction between two vectors (this -= v) */ 70 inline const Vector& operator-= (const sVec3D& v) { this->x -= v[0]; this->y -= v[1]; this->z -= v[2]; return *this; }; 71 /** @param v the second vector @returns The dotProduct between two vector (this (dot) v) */ 72 inline float operator* (const Vector& v) const { return x * v.x + y * v.y + z * v.z; }; 73 /** @todo strange */ 74 inline const Vector& operator*= (const Vector& v) { this->x *= v.x; this->y *= v.y; this->z *= v.z; return *this; }; 75 /** @param f a factor to multiply the vector with @returns the vector multiplied by f (this * f) */ 76 inline Vector operator* (float f) const { return Vector(x * f, y * f, z * f); }; 77 /** @param f a factor to multiply the vector with @returns the vector multiplied by f (this *= f) */ 78 inline const Vector& operator*= (float f) { this->x *= f; this->y *= f; this->z *= f; return *this; }; 79 /** @param f a factor to divide the vector with @returns the vector divided by f (this / f) */ 80 inline Vector operator/ (float f) const { return (unlikely(f == 0.0))?Vector(0,0,0):Vector(this->x / f, this->y / f, this->z / f); }; 81 /** @param f a factor to divide the vector with @returns the vector divided by f (this /= f) */ 82 inline const Vector& operator/= (float f) {if (unlikely(f == 0.0)) {this->x=0;this->y=0;this->z=0;} else {this->x /= f; this->y /= f; this->z /= f;} return *this; }; 83 /** copy constructor @todo (i do not know it this is faster) @param v the vector to assign to this vector. @returns the vector v */ 84 inline const Vector& operator= (const Vector& v) { this->x = v.x; this->y = v.y; this->z = v.z; return *this; }; 85 /** copy constructor* @param v the sVec3D to assign to this vector. @returns the vector v */ 86 inline const Vector& operator= (const sVec3D& v) { this->x = v[0]; this->y = v[1]; this->z = v[2]; } 87 /** @param v: the other vector \return the dot product of the vectors */ 88 float dot (const Vector& v) const { return x*v.x+y*v.y+z*v.z; }; 89 /** @param v: the corss-product partner @returns the cross-product between this and v (this (x) v) */ 90 inline Vector cross (const Vector& v) const { return Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x ); } 91 /** scales the this vector with v* @param v the vector to scale this with */ 92 void scale(const Vector& v) { x *= v.x; y *= v.y; z *= v.z; }; 93 /** @returns the length of the vector */ 94 inline float len() const { return sqrt (x*x+y*y+z*z); } 95 /** normalizes the vector */ 96 inline void normalize() { float l = len(); if( unlikely(l == 0.0))return; this->x=this->x/l; this->y=this->y/l; this->z=this->z/l; }; 97 Vector getNormalized() const; 98 Vector abs(); 99 100 void debug() const; 101 102 public: 103 float x; //!< The x Coordinate of the Vector. 104 float y; //!< The y Coordinate of the Vector. 105 float z; //!< The z Coordinate of the Vector. 106 }; 107 108 /** 109 * calculate the angle between two vectors in radiances 110 * @param v1: a vector 111 * @param v2: another vector 112 * @return the angle between the vectors in radians 113 */ 114 inline float angleDeg (const Vector& v1, const Vector& v2) { return acos( v1 * v2 / (v1.len() * v2.len())); }; 115 /** 116 * calculate the angle between two vectors in degrees 117 * @param v1: a vector 118 * @param v2: another vector 119 * @return the angle between the vectors in degrees 120 */ 121 inline float angleRad (const Vector& v1, const Vector& v2) { return acos( v1 * v2 / (v1.len() * v2.len())) * 180/M_PI; }; 122 123 /** an easy way to create a Random Vector @param sideLength the length of the Vector (x not sqrt(x^2...)) */ 124 #define VECTOR_RAND(sideLength) (Vector((float)rand()/RAND_MAX -.5, (float)rand()/RAND_MAX -.5, (float)rand()/RAND_MAX -.5) * sideLength) 125 30 #include "vector.h" 126 31 127 32 //! Quaternion … … 205 110 Vector v; //!< Imaginary Vector 206 111 float w; //!< Real part of the number 207 208 112 }; 209 113 210 114 115 #endif /* __QUATERNION_H_ */ 211 116 212 213 //! 3D rotation (OBSOLETE)214 /**215 Class to handle 3-dimensional rotations.216 Can create a rotation from several inputs, currently stores rotation using a 3x3 Matrix217 */218 class Rotation {219 public:220 221 float m[9]; //!< 3x3 Rotation Matrix222 223 Rotation ( const Vector& v);224 Rotation ( const Vector& axis, float angle);225 Rotation ( float pitch, float yaw, float roll);226 Rotation ();227 ~Rotation () {}228 229 Rotation operator* (const Rotation& r);230 231 void glmatrix (float* buffer);232 };233 234 //!< Apply a rotation to a vector235 Vector rotateVector( const Vector& v, const Rotation& r);236 237 //! 3D line238 /**239 Class to store Lines in 3-dimensional space240 241 Supports line-to-line distance measurements and rotation242 */243 class Line244 {245 public:246 247 Vector r; //!< Offset248 Vector a; //!< Direction249 250 Line ( Vector r, Vector a) : r(r), a(a) {} //!< assignment constructor251 Line () : r(Vector(0,0,0)), a(Vector (1,1,1)) {}252 ~Line () {}253 254 float distance (const Line& l) const;255 float distancePoint (const Vector& v) const;256 float distancePoint (const sVec3D& v) const;257 Vector* footpoints (const Line& l) const;258 float len () const;259 260 void rotate(const Rotation& rot);261 };262 263 //! 3D plane264 /**265 Class to handle planes in 3-dimensional space266 267 Critical for polygon-based collision detection268 */269 class Plane270 {271 public:272 273 Vector n; //!< Normal vector274 float k; //!< Offset constant275 276 Plane (const Vector& a, const Vector& b, const Vector& c);277 Plane (const Vector& norm, const Vector& p);278 Plane (const Vector& norm, const sVec3D& p);279 Plane (const Vector& n, float k) : n(n), k(k) {} //!< assignment constructor280 Plane () : n(Vector(1,1,1)), k(0) {}281 ~Plane () {}282 283 Vector intersectLine (const Line& l) const;284 float distancePoint (const Vector& p) const;285 float distancePoint (const sVec3D& p) const;286 float locatePoint (const Vector& p) const;287 };288 289 290 291 //! A class that represents a rectangle, this is needed for SpatialSeparation292 class Rectangle293 {294 295 public:296 Rectangle() { this->center = Vector(); }297 Rectangle(const Vector ¢er, float len) { this->center = Vector(center.x, center.y, center.z); this->axis[0] = len; this->axis[1] = len; }298 virtual ~Rectangle() {}299 300 /** \brief sets the center of the rectangle to a defined vector @param center the new center */301 inline void setCenter(const Vector ¢er) { this->center = center;}302 /** \brief sets the center of the rectangle to a defined vector @param x coord of the center @param y coord of the center @param z coord of the center */303 inline void setCenter(float x, float y, float z) { this->center.x = x; this->center.y = y; this->center.z = z; }304 /** \brief returns the center of the rectangle to a defined vector @returns center the new center */305 inline const Vector& getCenter() const { return this->center; }306 307 /** \brief sets both axis of the rectangle to a defined vector @param unityLength the new center */308 inline void setAxis(float unityLength) { this->axis[0] = unityLength; this->axis[1] = unityLength; }309 /** \brief sets both axis of the rectangle to a defined vector @param v1 the length of the x axis @param v2 the length of the z axis*/310 inline void setAxis(float v1, float v2) { this->axis[0] = v1; this->axis[1] = v2; }311 /** \brief gets one axis length of the rectangle @returns the length of the axis 0 */312 inline float getAxis() { return this-> axis[0]; }313 314 private:315 Vector center;316 float axis[2];317 };318 319 320 #endif /* __VECTOR_H_ */321 -
trunk/src/lib/math/vector.cc
r5688 r6616 66 66 PRINT(0)(" lenght: %f", len()); 67 67 PRINT(0)("\n"); 68 }69 70 /////////////////71 /* QUATERNIONS */72 /////////////////73 /**74 * calculates a lookAt rotation75 * @param dir: the direction you want to look76 * @param up: specify what direction up should be77 78 Mathematically this determines the rotation a (0,0,1)-Vector has to undergo to point79 the same way as dir. If you want to use this with cameras, you'll have to reverse the80 dir Vector (Vector(0,0,0) - your viewing direction) or you'll point the wrong way. You81 can use this for meshes as well (then you do not have to reverse the vector), but keep82 in mind that if you do that, the model's front has to point in +z direction, and left83 and right should be -x or +x respectively or the mesh wont rotate correctly.84 *85 * @TODO !!! OPTIMIZE THIS !!!86 */87 Quaternion::Quaternion (const Vector& dir, const Vector& up)88 {89 Vector z = dir.getNormalized();90 Vector x = up.cross(z).getNormalized();91 Vector y = z.cross(x);92 93 float m[4][4];94 m[0][0] = x.x;95 m[0][1] = x.y;96 m[0][2] = x.z;97 m[0][3] = 0;98 m[1][0] = y.x;99 m[1][1] = y.y;100 m[1][2] = y.z;101 m[1][3] = 0;102 m[2][0] = z.x;103 m[2][1] = z.y;104 m[2][2] = z.z;105 m[2][3] = 0;106 m[3][0] = 0;107 m[3][1] = 0;108 m[3][2] = 0;109 m[3][3] = 1;110 111 *this = Quaternion (m);112 }113 114 /**115 * calculates a rotation from euler angles116 * @param roll: the roll in radians117 * @param pitch: the pitch in radians118 * @param yaw: the yaw in radians119 */120 Quaternion::Quaternion (float roll, float pitch, float yaw)121 {122 float cr, cp, cy, sr, sp, sy, cpcy, spsy;123 124 // calculate trig identities125 cr = cos(roll/2);126 cp = cos(pitch/2);127 cy = cos(yaw/2);128 129 sr = sin(roll/2);130 sp = sin(pitch/2);131 sy = sin(yaw/2);132 133 cpcy = cp * cy;134 spsy = sp * sy;135 136 w = cr * cpcy + sr * spsy;137 v.x = sr * cpcy - cr * spsy;138 v.y = cr * sp * cy + sr * cp * sy;139 v.z = cr * cp * sy - sr * sp * cy;140 }141 142 /**143 * convert the Quaternion to a 4x4 rotational glMatrix144 * @param m: a buffer to store the Matrix in145 */146 void Quaternion::matrix (float m[4][4]) const147 {148 float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;149 150 // calculate coefficients151 x2 = v.x + v.x;152 y2 = v.y + v.y;153 z2 = v.z + v.z;154 xx = v.x * x2; xy = v.x * y2; xz = v.x * z2;155 yy = v.y * y2; yz = v.y * z2; zz = v.z * z2;156 wx = w * x2; wy = w * y2; wz = w * z2;157 158 m[0][0] = 1.0 - (yy + zz); m[1][0] = xy - wz;159 m[2][0] = xz + wy; m[3][0] = 0.0;160 161 m[0][1] = xy + wz; m[1][1] = 1.0 - (xx + zz);162 m[2][1] = yz - wx; m[3][1] = 0.0;163 164 m[0][2] = xz - wy; m[1][2] = yz + wx;165 m[2][2] = 1.0 - (xx + yy); m[3][2] = 0.0;166 167 m[0][3] = 0; m[1][3] = 0;168 m[2][3] = 0; m[3][3] = 1;169 }170 171 /**172 * performs a smooth move.173 * @param from where174 * @param to where175 * @param t the time this transformation should take value [0..1]176 * @returns the Result of the smooth move177 */178 Quaternion Quaternion::quatSlerp(const Quaternion& from, const Quaternion& to, float t)179 {180 float tol[4];181 double omega, cosom, sinom, scale0, scale1;182 // float DELTA = 0.2;183 184 cosom = from.v.x * to.v.x + from.v.y * to.v.y + from.v.z * to.v.z + from.w * to.w;185 186 if( cosom < 0.0 )187 {188 cosom = -cosom;189 tol[0] = -to.v.x;190 tol[1] = -to.v.y;191 tol[2] = -to.v.z;192 tol[3] = -to.w;193 }194 else195 {196 tol[0] = to.v.x;197 tol[1] = to.v.y;198 tol[2] = to.v.z;199 tol[3] = to.w;200 }201 202 omega = acos(cosom);203 sinom = sin(omega);204 scale0 = sin((1.0 - t) * omega) / sinom;205 scale1 = sin(t * omega) / sinom;206 return Quaternion(Vector(scale0 * from.v.x + scale1 * tol[0],207 scale0 * from.v.y + scale1 * tol[1],208 scale0 * from.v.z + scale1 * tol[2]),209 scale0 * from.w + scale1 * tol[3]);210 }211 212 213 /**214 * convert a rotational 4x4 glMatrix into a Quaternion215 * @param m: a 4x4 matrix in glMatrix order216 */217 Quaternion::Quaternion (float m[4][4])218 {219 220 float tr, s, q[4];221 int i, j, k;222 223 int nxt[3] = {1, 2, 0};224 225 tr = m[0][0] + m[1][1] + m[2][2];226 227 // check the diagonal228 if (tr > 0.0)229 {230 s = sqrt (tr + 1.0);231 w = s / 2.0;232 s = 0.5 / s;233 v.x = (m[1][2] - m[2][1]) * s;234 v.y = (m[2][0] - m[0][2]) * s;235 v.z = (m[0][1] - m[1][0]) * s;236 }237 else238 {239 // diagonal is negative240 i = 0;241 if (m[1][1] > m[0][0]) i = 1;242 if (m[2][2] > m[i][i]) i = 2;243 j = nxt[i];244 k = nxt[j];245 246 s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0);247 248 q[i] = s * 0.5;249 250 if (s != 0.0) s = 0.5 / s;251 252 q[3] = (m[j][k] - m[k][j]) * s;253 q[j] = (m[i][j] + m[j][i]) * s;254 q[k] = (m[i][k] + m[k][i]) * s;255 256 v.x = q[0];257 v.y = q[1];258 v.z = q[2];259 w = q[3];260 }261 }262 263 /**264 * outputs some nice formated debug information about this quaternion265 */266 void Quaternion::debug()267 {268 PRINT(0)("real a=%f; imag: x=%f y=%f z=%f\n", w, v.x, v.y, v.z);269 }270 271 void Quaternion::debug2()272 {273 Vector axis = this->getSpacialAxis();274 PRINT(0)("angle = %f, axis: ax=%f, ay=%f, az=%f\n", this->getSpacialAxisAngle(), axis.x, axis.y, axis.z );275 68 } 276 69 -
trunk/src/lib/math/vector.h
r5982 r6616 125 125 126 126 127 //! Quaternion128 /**129 Class to handle 3-dimensional rotation efficiently130 */131 class Quaternion132 {133 public:134 /** creates a Default quaternion (multiplicational identity Quaternion)*/135 inline Quaternion () { w = 1; v = Vector(0,0,0); }136 /** creates a Quaternion looking into the direction v @param v: the direction @param f: the value */137 inline Quaternion (const Vector& v, float f) { this->w = f; this->v = v; }138 Quaternion (float m[4][4]);139 /** turns a rotation along an axis into a Quaternion @param angle: the amount of radians to rotate @param axis: the axis to rotate around */140 inline Quaternion (float angle, const Vector& axis) { w = cos(angle/2); v = axis * sin(angle/2); }141 Quaternion (const Vector& dir, const Vector& up);142 Quaternion (float roll, float pitch, float yaw);143 144 /** @param q: the Quaternion to compare with this one. @returns true if the Quaternions are the same, false otherwise */145 inline bool operator== (const Quaternion& q) const { return (unlikely(this->v==q.v&&this->w==q.w))?true:false; };146 /** @param f: a real value @return a Quaternion containing the quotient */147 inline Quaternion operator/ (const float& f) const { return (unlikely(f==0.0)) ? Quaternion() : Quaternion(this->v/f, this->w/f); };148 /** @param f: the value to divide by @returns the quaternion devided by f (this /= f) */149 inline const Quaternion& operator/= (const float& f) {*this = *this / f; return *this;}150 /** @param f: a real value @return a Quaternion containing the product */151 inline Quaternion operator* (const float& f) const { return Quaternion(this->v*f, this->w*f); };152 /** @param f: the value to multiply by @returns the quaternion multiplied by f (this *= f) */153 inline const Quaternion& operator*= (const float& f) {*this = *this * f; return *this;}154 /** @param q: another Quaternion to rotate this by @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)) */155 Quaternion operator* (const Quaternion& q) const { return Quaternion(Vector(this->w*q.v.x + this->v.x*q.w + this->v.y*q.v.z - this->v.z*q.v.y,156 this->w*q.v.y + this->v.y*q.w + this->v.z*q.v.x - this->v.x*q.v.z,157 this->w*q.v.z + this->v.z*q.w + this->v.x*q.v.y - this->v.y*q.v.x),158 this->w*q.w - this->v.x*q.v.x - this->v.y*q.v.y - this->v.z*q.v.z); };159 /** @param q: the Quaternion to multiply by @returns the quaternion multiplied by q (this *= q) */160 inline const Quaternion& operator*= (const Quaternion& q) {*this = *this * q; return *this; };161 /** @param q the Quaternion by which to devide @returns the division from this by q (this / q) */162 inline Quaternion operator/ (const Quaternion& q) const { return *this * q.inverse(); };163 /** @param q the Quaternion by which to devide @returns the division from this by q (this /= q) */164 inline const Quaternion& operator/= (const Quaternion& q) { *this = *this * q.inverse(); return *this; };165 /** @param q the Quaternion to add to this @returns the quaternion added with q (this + q) */166 inline Quaternion operator+ (const Quaternion& q) const { return Quaternion(q.v + v, q.w + w); };167 /** @param q the Quaternion to add to this @returns the quaternion added with q (this += q) */168 inline const Quaternion& operator+= (const Quaternion& q) { this->v += q.v; this->w += q.w; return *this; };169 /** @param q the Quaternion to substrace from this @returns the quaternion substracted by q (this - q) */170 inline Quaternion operator- (const Quaternion& q) const { return Quaternion(q.v - v, q.w - w); }171 /** @param q the Quaternion to substrace from this @returns the quaternion substracted by q (this -= q) */172 inline const Quaternion& operator-= (const Quaternion& q) { this->v -= q.v; this->w -= q.w; return *this; };173 /** copy constructor @param q: the Quaternion to set this to. @returns the Quaternion q (or this) */174 inline Quaternion operator= (const Quaternion& q) {this->v = q.v; this->w = q.w; return *this;}175 /** conjugates this Quaternion @returns the conjugate */176 inline Quaternion conjugate () const { return Quaternion(Vector(-v.x, -v.y, -v.z), this->w); };177 /** @returns the norm of The Quaternion */178 inline float norm () const { return sqrt(w*w + v.x*v.x + v.y*v.y + v.z*v.z); };179 /** @returns the inverted Quaterntion of this */180 inline Quaternion inverse () const { return conjugate() / (w*w + v.x*v.x + v.y*v.y + v.z*v.z); };181 /** @returns the dot Product of a Quaternion */182 inline float dot (const Quaternion& q) const { return v.x*q.v.x + v.y*q.v.y + v.z*q.v.z + w*q.w; };183 /** @retuns the Distance between two Quaternions */184 inline float distance(const Quaternion& q) const { return 2*acos(fabsf(this->dot(q))); };185 /** @param v: the Vector @return a new Vector representing v rotated by the Quaternion */186 inline Vector apply (const Vector& v) const { return (*this * Quaternion(v, 0) * conjugate()).v; };187 void matrix (float m[4][4]) const;188 /** @returns the normalized Quaternion (|this|) */189 inline Quaternion getNormalized() const { float n = this->norm(); return Quaternion(this->v/n, this->w/n); };190 /** normalizes the current Quaternion */191 inline void normalize() { float n = this->norm(); this->v /= n; this->w/=n; };192 193 /** @returns the rotational axis of this Quaternion */194 inline Vector getSpacialAxis() const { return this->v / sin(acos(w));/*sqrt(v.x*v.x + v.y*v.y + v.z+v.z);*/ };195 /** @returns the rotational angle of this Quaternion around getSpacialAxis() !! IN DEGREE !! */196 inline float getSpacialAxisAngle() const { return 360.0 / M_PI * acos( this->w ); };197 198 static Quaternion quatSlerp(const Quaternion& from, const Quaternion& to, float t);199 200 void debug();201 void debug2();202 203 204 public:205 Vector v; //!< Imaginary Vector206 float w; //!< Real part of the number207 208 };209 210 211 212 213 127 //! 3D rotation (OBSOLETE) 214 128 /** -
trunk/src/lib/math/vector2D.h
r6615 r6616 10 10 11 11 ### File Specific: 12 main-programmer: Christian Meyer12 main-programmer: Benjamin Grauer 13 13 co-programmer: ... 14 14 */ 15 15 16 16 /*! 17 * @file vector .h18 * A basic 3Dmath framework17 * @file vector2D.h 18 * A basic 2D Vector math framework 19 19 * 20 20 * Contains classes to handle vectors, lines, rotations and planes
Note: See TracChangeset
for help on using the changeset viewer.