torcs - 1.2.2

src/modules/simu/simuv2.sav/SOLID-2.0/include/3D/Quaternion.h

Go to the documentation of this file.
00001 /*
00002   3D - C++ Class Library for 3D Transformations
00003   Copyright (C) 1996-1998  Gino van den Bergen
00004 
00005   This library is free software; you can redistribute it and/or
00006   modify it under the terms of the GNU Library General Public
00007   License as published by the Free Software Foundation; either
00008   version 2 of the License, or (at your option) any later version.
00009 
00010   This library is distributed in the hope that it will be useful,
00011   but WITHOUT ANY WARRANTY; without even the implied warranty of
00012   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013   Library General Public License for more details.
00014 
00015   You should have received a copy of the GNU Library General Public
00016   License along with this library; if not, write to the Free
00017   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00018 
00019   Please send remarks, questions and bug reports to gino@win.tue.nl,
00020   or write to:
00021                   Gino van den Bergen
00022                   Department of Mathematics and Computing Science
00023                   Eindhoven University of Technology
00024                   P.O. Box 513, 5600 MB Eindhoven, The Netherlands
00025 */
00026 
00027 #ifndef _QUATERNION_H_
00028 #define _QUATERNION_H_
00029 
00030 #include "Tuple4.h"
00031 #include "Vector.h"
00032 
00033 class Quaternion : public Tuple4 {
00034 public:
00035   Quaternion() {}
00036   Quaternion(const float v[4]) : Tuple4(v) {}
00037   Quaternion(const double v[4]) : Tuple4(v) {}
00038   Quaternion(Scalar x, Scalar y, Scalar z, Scalar w) : Tuple4(x, y, z, w) {}
00039   Quaternion(const Vector& axis, Scalar angle) { setRotation(axis, angle); }
00040   Quaternion(Scalar yaw, Scalar pitch, Scalar roll) { 
00041     setEuler(yaw, pitch, roll); 
00042   }
00043 
00044   void setRotation(const Vector& axis, Scalar angle) {
00045     Scalar d = axis.length();
00046     assert(!eqz(d));
00047     Scalar s = sin(angle / 2) / d;
00048     setValue(axis[X] * s, axis[Y] * s, axis[Z] * s, cos(angle / 2));
00049   }
00050 
00051   void setEuler(Scalar yaw, Scalar pitch, Scalar roll) {
00052     Scalar cosYaw = cos(yaw / 2);
00053     Scalar sinYaw = sin(yaw / 2);
00054     Scalar cosPitch = cos(pitch / 2);
00055     Scalar sinPitch = sin(pitch / 2);
00056     Scalar cosRoll = cos(roll / 2);
00057     Scalar sinRoll = sin(roll / 2);
00058     setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
00059              cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
00060              sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
00061              cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
00062   }
00063   
00064   Quaternion& operator+=(const Quaternion& q);
00065   Quaternion& operator-=(const Quaternion& q);
00066   Quaternion& operator*=(const Quaternion& q);
00067   Quaternion& operator*=(Scalar s);
00068   Quaternion& operator/=(Scalar s);
00069   
00070   Scalar length2() const;
00071   Scalar length() const;
00072 
00073   bool approxZero() const;
00074 
00075   void normalize();
00076   Quaternion normalized() const;
00077 
00078   void conjugate();
00079   Quaternion conjugate() const;
00080 
00081   void invert();
00082   Quaternion inverse() const;
00083 
00084   static Quaternion random();
00085 };
00086 
00087 Quaternion operator+(const Quaternion& q1, const Quaternion& q2);
00088 Quaternion operator-(const Quaternion& q1, const Quaternion& q2);
00089 Quaternion operator-(const Quaternion& q);
00090 Quaternion operator*(const Quaternion& q1, const Quaternion& q2);
00091 Quaternion operator*(const Quaternion& q, Scalar s);
00092 Quaternion operator/(const Quaternion& q, Scalar s);
00093 
00094 Scalar dot(const Quaternion& q1, const Quaternion& q2);
00095 
00096 Scalar length2(const Quaternion& q);
00097 Scalar length(const Quaternion& q);
00098 
00099 bool approxZero(const Quaternion& q);
00100 bool approxEqual(const Quaternion& q1, const Quaternion& q2);
00101 
00102 
00103 
00104 inline Quaternion& Quaternion::operator+=(const Quaternion& q) {
00105   comp[X] += q[X]; comp[Y] += q[Y]; comp[Z] += q[Z]; comp[W] += q[W];
00106   return *this;
00107 }
00108 
00109 inline Quaternion& Quaternion::operator-=(const Quaternion& q) {
00110   comp[X] -= q[X]; comp[Y] -= q[Y]; comp[Z] -= q[Z]; comp[W] -= q[W];
00111   return *this;
00112 }
00113  
00114 inline Quaternion& Quaternion::operator*=(const Quaternion& q) {
00115   setValue(comp[W] * q[X] + comp[X] * q[W] + comp[Y] * q[Z] - comp[Z] * q[Y],
00116            comp[W] * q[Y] + comp[Y] * q[W] + comp[Z] * q[X] - comp[X] * q[Z],
00117            comp[W] * q[Z] + comp[Z] * q[W] + comp[X] * q[Y] - comp[Y] * q[X],
00118            comp[W] * q[W] - comp[X] * q[X] - comp[Y] * q[Y] - comp[Z] * q[Z]);
00119   return *this;
00120 }
00121 
00122 inline Quaternion& Quaternion::operator*=(Scalar s) {
00123   comp[X] *= s; comp[Y] *= s; comp[Z] *= s; comp[W] *= s;
00124   return *this;
00125 }
00126 
00127 inline Quaternion& Quaternion::operator/=(Scalar s) {
00128   assert(!eqz(s));
00129   return *this *= 1 / s;
00130 }
00131 
00132 inline Quaternion operator+(const Quaternion& q1, const Quaternion& q2) {
00133   return Quaternion(q1[X] + q2[X], q1[Y] + q2[Y], q1[Z] + q2[Z], q1[W] + q2[W]);
00134 }
00135 
00136 inline Quaternion operator-(const Quaternion& q1, const Quaternion& q2) {
00137   return Quaternion(q1[X] - q2[X], q1[Y] - q2[Y], q1[Z] - q2[Z], q1[W] - q2[W]);
00138 }
00139 
00140 inline Quaternion operator-(const Quaternion& q) {
00141   return Quaternion(-q[X], -q[Y], -q[Z], -q[W]);
00142 }
00143 
00144 inline Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
00145   return 
00146     Quaternion(q1[W] * q2[X] + q1[X] * q2[W] + q1[Y] * q2[Z] - q1[Z] * q2[Y],
00147                q1[W] * q2[Y] + q1[Y] * q2[W] + q1[Z] * q2[X] - q1[X] * q2[Z],
00148                q1[W] * q2[Z] + q1[Z] * q2[W] + q1[X] * q2[Y] - q1[Y] * q2[X],
00149                q1[W] * q2[W] - q1[X] * q2[X] - q1[Y] * q2[Y] - q1[Z] * q2[Z]); 
00150 }
00151 
00152 inline Quaternion operator*(const Quaternion& q, Scalar s) {
00153   return Quaternion(q[X] * s, q[Y] * s, q[Z] * s, q[W] * s);
00154 }
00155 
00156 inline Quaternion operator*(Scalar s, const Quaternion& q) { return q * s; }
00157 
00158 inline Quaternion operator/(const Quaternion& q, Scalar s) {
00159   assert(!eqz(s));
00160   return q * (1 / s);
00161 }
00162 
00163 inline Scalar dot(const Quaternion& q1, const Quaternion& q2) {
00164   return q1[X] * q2[X] + q1[Y] * q2[Y] + q1[Z] * q2[Z] + q1[W] * q2[W];
00165 }
00166 
00167 inline Scalar Quaternion::length2() const { return dot(*this, *this); }
00168 inline Scalar Quaternion::length() const { return sqrt(length2()); }
00169 
00170 inline bool Quaternion::approxZero() const { return length2() < EPSILON2; }
00171 
00172 inline void Quaternion::normalize() { *this /= length(); }
00173 inline Quaternion Quaternion::normalized() const { return *this / length(); }
00174 
00175 inline void Quaternion::conjugate() {
00176   comp[X] = -comp[X]; comp[Y] = -comp[Y]; comp[Z] = -comp[Z];
00177 }
00178 
00179 inline Quaternion Quaternion::conjugate() const {
00180   return Quaternion(-comp[X], -comp[Y], -comp[Z], comp[W]);
00181 }
00182   
00183 inline void Quaternion::invert() {
00184   conjugate();
00185   *this /= length2();
00186 }
00187 
00188 inline Quaternion Quaternion::inverse() const {
00189   return conjugate() / length2();
00190 }
00191 
00192 inline Scalar length2(const Quaternion& q) { return q.length2(); }
00193 inline Scalar length(const Quaternion& q) { return q.length(); }
00194 
00195 inline bool approxZero(const Quaternion& q) { return q.approxZero(); }
00196 inline bool approxEqual(const Quaternion& q1, const Quaternion& q2) { 
00197   return approxZero(q1 - q2); 
00198 }
00199 
00200 // From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 
00201 //       pg. 124-132
00202 inline Quaternion Quaternion::random() {
00203   Scalar x0 = rnd();
00204   Scalar r1 = sqrt(1 - x0), r2 = sqrt(x0);
00205   Scalar t1 = TWO_PI * rnd(), t2 = TWO_PI * rnd();
00206   Scalar c1 = cos(t1), s1 = sin(t1);
00207   Scalar c2 = cos(t2), s2 = sin(t2);
00208   return Quaternion(s1 * r1, c1 * r1, s2 * r2, c2 * r2);
00209 }
00210 
00211 #endif
00212 
00213 
00214 

Generated at Thu Feb 26 21:52:31 2004 for torcs by doxygen 1.3.3 written by Dimitri van Heesch, © 1997-1999
TORCS © Eric Espié 1999, 2002.