1 /* ResidualVM - A 3D game interpreter
2 *
3 * ResidualVM is the legal property of its developers, whose names
4 * are too numerous to list here. Please refer to the COPYRIGHT
5 * file distributed with this source distribution.
6 *
7 * This program is free software; you can redistribute it and/or
8 * modify it under the terms of the GNU General Public License
9 * as published by the Free Software Foundation; either version 2
10 * of the License, or (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20 *
21 */
22
23 /*
24 * Quaternion-math originally borrowed from plib http://plib.sourceforge.net/index.html
25 * This code was originally made available under the LGPLv2 license (or later).
26 *
27 * Quaternion routines are Copyright (C) 1999
28 * Kevin B. Thompson <kevinbthompson@yahoo.com>
29 * Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
30 * Largely rewritten by "Negative0" <negative0@earthlink.net>
31 *
32 * This code (and our modifications) is made available here under the GPLv2 (or later).
33 *
34 * Additional changes written based on the math presented in
35 * http://www.swarthmore.edu/NatSci/mzucker1/e27/diebel2006attitude.pdf
36 *
37 */
38
39 #include "common/streamdebug.h"
40
41 #include "common/math.h"
42 #include "math/quat.h"
43
44 namespace Math {
45
Quaternion(const Matrix3 & m)46 Quaternion::Quaternion(const Matrix3 &m) {
47 fromMatrix(m);
48 normalize();
49 }
50
Quaternion(const Matrix4 & m)51 Quaternion::Quaternion(const Matrix4 &m) {
52 fromMatrix(m.getRotation());
53 }
54
Quaternion(const Vector3d & axis,const Angle & angle)55 Quaternion::Quaternion(const Vector3d &axis, const Angle &angle) {
56 float s = (angle / 2).getSine();
57 float c = (angle / 2).getCosine();
58 set(axis.x() * s, axis.y() * s, axis.z() * s, c);
59 }
60
xAxis(const Angle & angle)61 Quaternion Quaternion::xAxis(const Angle &angle) {
62 Quaternion q(Vector3d(1.0f, 0.0f, 0.0f), angle);
63 return q;
64 }
65
yAxis(const Angle & angle)66 Quaternion Quaternion::yAxis(const Angle &angle) {
67 Quaternion q(Vector3d(0.0f, 1.0f, 0.0f), angle);
68 return q;
69 }
70
zAxis(const Angle & angle)71 Quaternion Quaternion::zAxis(const Angle &angle) {
72 Quaternion q(Vector3d(0.0f, 0.0f, 1.0f), angle);
73 return q;
74 }
75
slerpQuat(const Quaternion & to,const float t) const76 Quaternion Quaternion::slerpQuat(const Quaternion& to, const float t) const {
77 Quaternion dst;
78 float scale0, scale1;
79 float flip = 1.0f;
80 float angle = this->dotProduct(to);
81
82 // Make sure the rotation is the short one
83 if (angle < 0.0f) {
84 angle = -angle;
85 flip = -1.0f;
86 }
87
88 // Spherical Interpolation
89 // Threshold of 1e-6
90 if (angle < 1.0f - (float) 1E-6f) {
91 float theta = acosf(angle);
92 float invSineTheta = 1.0f / sinf(theta);
93
94 scale0 = sinf((1.0f - t) * theta) * invSineTheta;
95 scale1 = (sinf(t * theta) * invSineTheta) * flip;
96 // Linear Interpolation
97 } else {
98 scale0 = 1.0f - t;
99 scale1 = t * flip;
100 }
101
102 // Apply the interpolation
103 dst = (*this * scale0) + (to * scale1);
104 return dst;
105 }
106
normalize()107 Quaternion& Quaternion::normalize() {
108 const float scale = sqrtf(square(x()) + square(y()) + square(z()) + square(w()));
109
110 // Already normalized if the scale is 1.0
111 if (scale != 1.0f && scale != 0.0f)
112 set(x() / scale, y() / scale, z() / scale, w() / scale);
113
114 return *this;
115 }
116
transform(Vector3d & v) const117 void Quaternion::transform(Vector3d &v) const {
118 const Vector3d im = Vector3d(x(), y(), z());
119 v += 2.0 * Vector3d::crossProduct(im, Vector3d::crossProduct(im, v) + w() * v);
120 }
121
fromMatrix(const Matrix3 & m)122 void Quaternion::fromMatrix(const Matrix3 &m) {
123 float qx, qy, qz, qw;
124 float tr = m.getValue(0, 0) + m.getValue(1, 1) + m.getValue(2, 2);
125 float s;
126
127 if (tr > 0.0f) {
128 s = sqrtf(tr + 1.0f);
129 qw = s * 0.5f;
130 s = 0.5f / s;
131 qx = (m.getValue(2, 1) - m.getValue(1, 2)) * s;
132 qy = (m.getValue(0, 2) - m.getValue(2, 0)) * s;
133 qz = (m.getValue(1, 0) - m.getValue(0, 1)) * s;
134 } else {
135 int h = 0;
136 if (m.getValue(1, 1) > m.getValue(0, 0))
137 h = 1;
138 if (m.getValue(2, 2) > m.getValue(h, h))
139 h = 2;
140
141 if (h == 0) {
142 s = sqrt(m.getValue(0, 0) - (m.getValue(1,1) + m.getValue(2, 2)) + 1.0f);
143 qx = s * 0.5f;
144 s = 0.5f / s;
145 qy = (m.getValue(0, 1) + m.getValue(1, 0)) * s;
146 qz = (m.getValue(2, 0) + m.getValue(0, 2)) * s;
147 qw = (m.getValue(2, 1) - m.getValue(1, 2)) * s;
148 } else if (h == 1) {
149 s = sqrt(m.getValue(1, 1) - (m.getValue(2,2) + m.getValue(0, 0)) + 1.0f);
150 qy = s * 0.5f;
151 s = 0.5f / s;
152 qz = (m.getValue(1, 2) + m.getValue(2, 1)) * s;
153 qx = (m.getValue(0, 1) + m.getValue(1, 0)) * s;
154 qw = (m.getValue(0, 2) - m.getValue(2, 0)) * s;
155 } else {
156 s = sqrt(m.getValue(2, 2) - (m.getValue(0,0) + m.getValue(1, 1)) + 1.0f);
157 qz = s * 0.5f;
158 s = 0.5f / s;
159 qx = (m.getValue(2, 0) + m.getValue(0, 2)) * s;
160 qy = (m.getValue(1, 2) + m.getValue(2, 1)) * s;
161 qw = (m.getValue(1, 0) - m.getValue(0, 1)) * s;
162 }
163 }
164 set(qx, qy, qz, qw);
165 }
166
toMatrix(Matrix4 & dst) const167 void Quaternion::toMatrix(Matrix4 &dst) const {
168 float two_xx = x() * (x() + x());
169 float two_xy = x() * (y() + y());
170 float two_xz = x() * (z() + z());
171
172 float two_wx = w() * (x() + x());
173 float two_wy = w() * (y() + y());
174 float two_wz = w() * (z() + z());
175
176 float two_yy = y() * (y() + y());
177 float two_yz = y() * (z() + z());
178
179 float two_zz = z() * (z() + z());
180
181 float newMat[16] = {
182 1.0f - (two_yy + two_zz), two_xy - two_wz, two_xz + two_wy, 0.0f,
183 two_xy + two_wz, 1.0f - (two_xx + two_zz), two_yz - two_wx, 0.0f,
184 two_xz - two_wy, two_yz + two_wx, 1.0f - (two_xx + two_yy), 0.0f,
185 0.0f, 0.0f, 0.0f, 1.0f
186 };
187 dst.setData(newMat);
188 }
189
toMatrix() const190 Matrix4 Quaternion::toMatrix() const {
191 Matrix4 dst;
192 toMatrix(dst);
193 return dst;
194 }
195
inverse() const196 Quaternion Quaternion::inverse() const {
197 Quaternion q = *this;
198 q.normalize();
199 q.x() = -q.x();
200 q.y() = -q.y();
201 q.z() = -q.z();
202 return q;
203 }
204
directionVector(const int col) const205 Vector3d Quaternion::directionVector(const int col) const {
206 Matrix4 dirMat = toMatrix();
207 return Vector3d(dirMat.getValue(0, col), dirMat.getValue(1, col), dirMat.getValue(2, col));
208 }
209
getAngleBetween(const Quaternion & to)210 Angle Quaternion::getAngleBetween(const Quaternion &to) {
211 Quaternion q = this->inverse() * to;
212 Angle diff(Common::rad2deg(2 * acos(q.w())));
213 return diff;
214 }
215
fromEuler(const Angle & first,const Angle & second,const Angle & third,EulerOrder order)216 Quaternion Quaternion::fromEuler(const Angle &first, const Angle &second, const Angle &third, EulerOrder order) {
217 // First create a matrix with the rotation
218 Matrix4 rot(first, second, third, order);
219
220 // Convert this rotation matrix to a Quaternion
221 return Quaternion(rot);
222 }
223
getEuler(Angle * first,Angle * second,Angle * third,EulerOrder order) const224 void Quaternion::getEuler(Angle *first, Angle *second, Angle *third, EulerOrder order) const {
225 // Create a matrix from the Quaternion
226 Matrix4 rot = toMatrix();
227
228 // Convert the matrix to Euler Angles
229 Angle f, s, t;
230 rot.getEuler(&f, &s, &t, order);
231
232 // Assign the Angles if we have a reference
233 if (first != nullptr)
234 *first = f;
235 if (second != nullptr)
236 *second = s;
237 if (third != nullptr)
238 *third = t;
239 }
240
operator =(const Quaternion & quat)241 Quaternion& Quaternion::operator=(const Quaternion& quat) {
242 x() = quat.x();
243 y() = quat.y();
244 z() = quat.z();
245 w() = quat.w();
246
247 return *this;
248 }
249
operator *(const Quaternion & o) const250 Quaternion Quaternion::operator*(const Quaternion &o) const {
251 return Quaternion(
252 w() * o.x() + x() * o.w() + y() * o.z() - z() * o.y(),
253 w() * o.y() - x() * o.z() + y() * o.w() + z() * o.x(),
254 w() * o.z() + x() * o.y() - y() * o.x() + z() * o.w(),
255 w() * o.w() - x() * o.x() - y() * o.y() - z() * o.z()
256 );
257 }
258
operator *(const float c) const259 Quaternion Quaternion::operator*(const float c) const {
260 return Quaternion(x() * c, y() * c, z() * c, w() * c);
261 }
262
operator *=(const Quaternion & o)263 Quaternion& Quaternion::operator*=(const Quaternion &o) {
264 *this = *this * o;
265 return *this;
266 }
267
operator +(const Quaternion & o) const268 Quaternion Quaternion::operator+(const Quaternion &o) const {
269 return Quaternion(x() + o.x(), y() + o.y(), z() + o.z(), w() + o.w());
270 }
271
operator +=(const Quaternion & o)272 Quaternion& Quaternion::operator+=(const Quaternion &o) {
273 *this = *this + o;
274 return *this;
275 }
276
operator ==(const Quaternion & o) const277 bool Quaternion::operator==(const Quaternion &o) const {
278 float dw = fabs(w() - o.w());
279 float dx = fabs(x() - o.x());
280 float dy = fabs(y() - o.y());
281 float dz = fabs(z() - o.z());
282 // Threshold of equality
283 float th = 1E-5f;
284
285 if ((dw < th) && (dx < th) && (dy < th) && (dz < th)) {
286 return true;
287 }
288 return false;
289 }
290
operator !=(const Quaternion & o) const291 bool Quaternion::operator!=(const Quaternion &o) const {
292 return !(*this == o);
293 }
294
295 } // End namespace Math
296