~mulder-nebulon/openracing/PhysX

« back to all changes in this revision

Viewing changes to src/libsimulator/includes/3D/Quaternion.h

  • Committer: Keith Curtis
  • Date: 2009-02-17 02:50:38 UTC
  • Revision ID: keithcu@gmail.com-20090217025038-mx33cu7s4sjhiffo
Initial import

Show diffs side-by-side

added added

removed removed

Lines of Context:
 
1
/*
 
2
  3D - C++ Class Library for 3D Transformations
 
3
  Copyright (C) 1996-1998  Gino van den Bergen
 
4
 
 
5
  This library is free software; you can redistribute it and/or
 
6
  modify it under the terms of the GNU Library General Public
 
7
  License as published by the Free Software Foundation; either
 
8
  version 2 of the License, or (at your option) any later version.
 
9
 
 
10
  This library is distributed in the hope that it will be useful,
 
11
  but WITHOUT ANY WARRANTY; without even the implied warranty of
 
12
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 
13
  Library General Public License for more details.
 
14
 
 
15
  You should have received a copy of the GNU Library General Public
 
16
  License along with this library; if not, write to the Free
 
17
  Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 
18
 
 
19
  Please send remarks, questions and bug reports to gino@win.tue.nl,
 
20
  or write to:
 
21
                  Gino van den Bergen
 
22
                  Department of Mathematics and Computing Science
 
23
                  Eindhoven University of Technology
 
24
                  P.O. Box 513, 5600 MB Eindhoven, The Netherlands
 
25
*/
 
26
 
 
27
#ifndef _QUATERNION_H_
 
28
#define _QUATERNION_H_
 
29
 
 
30
#include "Tuple4.h"
 
31
#include "Vector.h"
 
32
 
 
33
class Quaternion : public Tuple4 {
 
34
public:
 
35
  Quaternion() {}
 
36
  Quaternion(const float v[4]) : Tuple4(v) {}
 
37
  Quaternion(const double v[4]) : Tuple4(v) {}
 
38
  Quaternion(Scalar x, Scalar y, Scalar z, Scalar w) : Tuple4(x, y, z, w) {}
 
39
  Quaternion(const Vector& axis, Scalar angle) { setRotation(axis, angle); }
 
40
  Quaternion(Scalar yaw, Scalar pitch, Scalar roll) { 
 
41
    setEuler(yaw, pitch, roll); 
 
42
  }
 
43
 
 
44
  void setRotation(const Vector& axis, Scalar angle) {
 
45
    Scalar d = axis.length();
 
46
    assert(!eqz(d));
 
47
    Scalar s = sin(angle / 2) / d;
 
48
    setValue(axis[X] * s, axis[Y] * s, axis[Z] * s, cos(angle / 2));
 
49
  }
 
50
 
 
51
  void setEuler(Scalar yaw, Scalar pitch, Scalar roll) {
 
52
    Scalar cosYaw = cos(yaw / 2);
 
53
    Scalar sinYaw = sin(yaw / 2);
 
54
    Scalar cosPitch = cos(pitch / 2);
 
55
    Scalar sinPitch = sin(pitch / 2);
 
56
    Scalar cosRoll = cos(roll / 2);
 
57
    Scalar sinRoll = sin(roll / 2);
 
58
    setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
 
59
             cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
 
60
             sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
 
61
             cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
 
62
  }
 
63
  
 
64
  Quaternion& operator+=(const Quaternion& q);
 
65
  Quaternion& operator-=(const Quaternion& q);
 
66
  Quaternion& operator*=(const Quaternion& q);
 
67
  Quaternion& operator*=(Scalar s);
 
68
  Quaternion& operator/=(Scalar s);
 
69
  
 
70
  Scalar length2() const;
 
71
  Scalar length() const;
 
72
 
 
73
  bool approxZero() const;
 
74
 
 
75
  void normalize();
 
76
  Quaternion normalized() const;
 
77
 
 
78
  void conjugate();
 
79
  Quaternion conjugate() const;
 
80
 
 
81
  void invert();
 
82
  Quaternion inverse() const;
 
83
 
 
84
  static Quaternion random();
 
85
};
 
86
 
 
87
Quaternion operator+(const Quaternion& q1, const Quaternion& q2);
 
88
Quaternion operator-(const Quaternion& q1, const Quaternion& q2);
 
89
Quaternion operator-(const Quaternion& q);
 
90
Quaternion operator*(const Quaternion& q1, const Quaternion& q2);
 
91
Quaternion operator*(const Quaternion& q, Scalar s);
 
92
Quaternion operator/(const Quaternion& q, Scalar s);
 
93
 
 
94
Scalar dot(const Quaternion& q1, const Quaternion& q2);
 
95
 
 
96
Scalar length2(const Quaternion& q);
 
97
Scalar length(const Quaternion& q);
 
98
 
 
99
bool approxZero(const Quaternion& q);
 
100
bool approxEqual(const Quaternion& q1, const Quaternion& q2);
 
101
 
 
102
 
 
103
 
 
104
inline Quaternion& Quaternion::operator+=(const Quaternion& q) {
 
105
  comp[X] += q[X]; comp[Y] += q[Y]; comp[Z] += q[Z]; comp[W] += q[W];
 
106
  return *this;
 
107
}
 
108
 
 
109
inline Quaternion& Quaternion::operator-=(const Quaternion& q) {
 
110
  comp[X] -= q[X]; comp[Y] -= q[Y]; comp[Z] -= q[Z]; comp[W] -= q[W];
 
111
  return *this;
 
112
}
 
113
 
 
114
inline Quaternion& Quaternion::operator*=(const Quaternion& q) {
 
115
  setValue(comp[W] * q[X] + comp[X] * q[W] + comp[Y] * q[Z] - comp[Z] * q[Y],
 
116
           comp[W] * q[Y] + comp[Y] * q[W] + comp[Z] * q[X] - comp[X] * q[Z],
 
117
           comp[W] * q[Z] + comp[Z] * q[W] + comp[X] * q[Y] - comp[Y] * q[X],
 
118
           comp[W] * q[W] - comp[X] * q[X] - comp[Y] * q[Y] - comp[Z] * q[Z]);
 
119
  return *this;
 
120
}
 
121
 
 
122
inline Quaternion& Quaternion::operator*=(Scalar s) {
 
123
  comp[X] *= s; comp[Y] *= s; comp[Z] *= s; comp[W] *= s;
 
124
  return *this;
 
125
}
 
126
 
 
127
inline Quaternion& Quaternion::operator/=(Scalar s) {
 
128
  assert(!eqz(s));
 
129
  return *this *= 1 / s;
 
130
}
 
131
 
 
132
inline Quaternion operator+(const Quaternion& q1, const Quaternion& q2) {
 
133
  return Quaternion(q1[X] + q2[X], q1[Y] + q2[Y], q1[Z] + q2[Z], q1[W] + q2[W]);
 
134
}
 
135
 
 
136
inline Quaternion operator-(const Quaternion& q1, const Quaternion& q2) {
 
137
  return Quaternion(q1[X] - q2[X], q1[Y] - q2[Y], q1[Z] - q2[Z], q1[W] - q2[W]);
 
138
}
 
139
 
 
140
inline Quaternion operator-(const Quaternion& q) {
 
141
  return Quaternion(-q[X], -q[Y], -q[Z], -q[W]);
 
142
}
 
143
 
 
144
inline Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
 
145
  return 
 
146
    Quaternion(q1[W] * q2[X] + q1[X] * q2[W] + q1[Y] * q2[Z] - q1[Z] * q2[Y],
 
147
               q1[W] * q2[Y] + q1[Y] * q2[W] + q1[Z] * q2[X] - q1[X] * q2[Z],
 
148
               q1[W] * q2[Z] + q1[Z] * q2[W] + q1[X] * q2[Y] - q1[Y] * q2[X],
 
149
               q1[W] * q2[W] - q1[X] * q2[X] - q1[Y] * q2[Y] - q1[Z] * q2[Z]); 
 
150
}
 
151
 
 
152
inline Quaternion operator*(const Quaternion& q, Scalar s) {
 
153
  return Quaternion(q[X] * s, q[Y] * s, q[Z] * s, q[W] * s);
 
154
}
 
155
 
 
156
inline Quaternion operator*(Scalar s, const Quaternion& q) { return q * s; }
 
157
 
 
158
inline Quaternion operator/(const Quaternion& q, Scalar s) {
 
159
  assert(!eqz(s));
 
160
  return q * (1 / s);
 
161
}
 
162
 
 
163
inline Scalar dot(const Quaternion& q1, const Quaternion& q2) {
 
164
  return q1[X] * q2[X] + q1[Y] * q2[Y] + q1[Z] * q2[Z] + q1[W] * q2[W];
 
165
}
 
166
 
 
167
inline Scalar Quaternion::length2() const { return dot(*this, *this); }
 
168
inline Scalar Quaternion::length() const { return sqrt(length2()); }
 
169
 
 
170
inline bool Quaternion::approxZero() const { return length2() < EPSILON2; }
 
171
 
 
172
inline void Quaternion::normalize() { *this /= length(); }
 
173
inline Quaternion Quaternion::normalized() const { return *this / length(); }
 
174
 
 
175
inline void Quaternion::conjugate() {
 
176
  comp[X] = -comp[X]; comp[Y] = -comp[Y]; comp[Z] = -comp[Z];
 
177
}
 
178
 
 
179
inline Quaternion Quaternion::conjugate() const {
 
180
  return Quaternion(-comp[X], -comp[Y], -comp[Z], comp[W]);
 
181
}
 
182
  
 
183
inline void Quaternion::invert() {
 
184
  conjugate();
 
185
  *this /= length2();
 
186
}
 
187
 
 
188
inline Quaternion Quaternion::inverse() const {
 
189
  return conjugate() / length2();
 
190
}
 
191
 
 
192
inline Scalar length2(const Quaternion& q) { return q.length2(); }
 
193
inline Scalar length(const Quaternion& q) { return q.length(); }
 
194
 
 
195
inline bool approxZero(const Quaternion& q) { return q.approxZero(); }
 
196
inline bool approxEqual(const Quaternion& q1, const Quaternion& q2) { 
 
197
  return approxZero(q1 - q2); 
 
198
}
 
199
 
 
200
// From: "Uniform Random Rotations", Ken Shoemake, Graphics Gems III, 
 
201
//       pg. 124-132
 
202
inline Quaternion Quaternion::random() {
 
203
  Scalar x0 = rnd();
 
204
  Scalar r1 = sqrt(1 - x0), r2 = sqrt(x0);
 
205
  Scalar t1 = TWO_PI * rnd(), t2 = TWO_PI * rnd();
 
206
  Scalar c1 = cos(t1), s1 = sin(t1);
 
207
  Scalar c2 = cos(t2), s2 = sin(t2);
 
208
  return Quaternion(s1 * r1, c1 * r1, s2 * r2, c2 * r2);
 
209
}
 
210
 
 
211
#endif
 
212
 
 
213
 
 
214