00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
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
00201
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