Line data Source code
1 : #include "Quaternion.h"
2 :
3 : //------------------------------------------------------------------------------
4 528 : Quaternion::Quaternion() {
5 264 : x = y = z = w = 0.0f;
6 528 : }
7 :
8 : //------------------------------------------------------------------------------
9 2 : Quaternion::Quaternion(const Vector &v) {
10 1 : buildFromEuler(v);
11 2 : }
12 :
13 : //------------------------------------------------------------------------------
14 2 : Quaternion::Quaternion(const float &x, const float &y, const float &z) {
15 1 : buildFromEuler(x, y, z);
16 2 : }
17 :
18 : //------------------------------------------------------------------------------
19 16 : Quaternion::Quaternion(const Vector &axis, const float &angle) {
20 8 : buildFromAxis(axis, angle);
21 16 : }
22 :
23 : //------------------------------------------------------------------------------
24 32 : Quaternion::Quaternion(const float &qX, const float &qY, const float &qZ, const float &qW) {
25 16 : x = qX;
26 16 : y = qY;
27 16 : z = qZ;
28 16 : w = qW;
29 32 : }
30 :
31 : //------------------------------------------------------------------------------
32 12 : Quaternion::Quaternion(const Quaternion &q) {
33 6 : x = q.x;
34 6 : y = q.y;
35 6 : z = q.z;
36 6 : w = q.w;
37 12 : }
38 :
39 : //------------------------------------------------------------------------------
40 5 : void Quaternion::buildFromEuler(const Vector &v) {
41 : // calculate trig identities ONCE
42 5 : float cosX = cos(v.x / 2.0f);
43 5 : float cosY = cos(v.y / 2.0f);
44 5 : float cosZ = cos(v.z / 2.0f);
45 :
46 5 : float sinX = sin(v.x / 2.0f);
47 5 : float sinY = sin(v.y / 2.0f);
48 5 : float sinZ = sin(v.z / 2.0f);
49 :
50 5 : float cosProduct = cosY * cosZ;
51 5 : float sinProduct = sinY * sinZ;
52 :
53 : // calculate the quaternion
54 5 : w = cosX * cosProduct + sinX * sinProduct;
55 5 : x = sinX * cosProduct - cosX * sinProduct;
56 5 : y = cosX * sinY * cosZ + sinX * cosY * sinZ;
57 5 : z = cosX * cosY * sinZ - sinX * sinY * cosZ;
58 5 : }
59 :
60 : //------------------------------------------------------------------------------
61 106 : void Quaternion::buildFromEuler(const float &angleX, const float &angleY, const float &angleZ) {
62 : // calculate trig identities ONCE
63 106 : float cosX = cos(angleX / 2.0f);
64 106 : float cosY = cos(angleY / 2.0f);
65 106 : float cosZ = cos(angleZ / 2.0f);
66 :
67 106 : float sinX = sin(angleX / 2.0f);
68 106 : float sinY = sin(angleY / 2.0f);
69 106 : float sinZ = sin(angleZ / 2.0f);
70 :
71 106 : float cosProduct = cosY * cosZ;
72 106 : float sinProduct = sinY * sinZ;
73 :
74 : // calculate the quaternion
75 106 : w = cosX * cosProduct + sinX * sinProduct;
76 106 : x = sinX * cosProduct - cosX * sinProduct;
77 106 : y = cosX * sinY * cosZ + sinX * cosY * sinZ;
78 106 : z = cosX * cosY * sinZ - sinX * sinY * cosZ;
79 106 : }
80 :
81 : //------------------------------------------------------------------------------
82 8 : void Quaternion::buildFromAxis(const Vector &v, const float &angle) {
83 : // this function currently assumes a valid unit vector is passed in Vector v
84 8 : float scale = sin( angle / 2.0f );
85 :
86 8 : w = cos( angle / 2.0f);
87 8 : x = v.x * scale;
88 8 : y = v.y * scale;
89 8 : z = v.z * scale;
90 8 : }
91 :
92 : //------------------------------------------------------------------------------
93 10 : void Quaternion::normalize() {
94 10 : float len = sqrtf((x * x) + (y * y) + (z * z) + (w * w));
95 :
96 10 : if (len != 0.0f) {
97 9 : x /= len;
98 9 : y /= len;
99 9 : z /= len;
100 9 : w /= len;
101 9 : }
102 10 : }
103 :
104 : //------------------------------------------------------------------------------
105 148 : void Quaternion::loadMultIdentity() {
106 148 : x = 0.0f;
107 148 : y = 0.0f;
108 148 : z = 0.0f;
109 148 : w = 1.0f;
110 148 : }
111 :
112 : //------------------------------------------------------------------------------
113 1 : void Quaternion::loadAddIdentity() {
114 1 : x = 0.0f;
115 1 : y = 0.0f;
116 1 : z = 0.0f;
117 1 : w = 0.0f;
118 1 : }
119 :
120 : //------------------------------------------------------------------------------
121 3 : void Quaternion::getEulerAngles(Vector &v) {
122 3 : Matrix m;
123 3 : buildRotationMatrix(m);
124 :
125 3 : const float epsilon = 0.0000001;
126 :
127 3 : if (m.data[2] < 1 - epsilon && m.data[2] > -1 + epsilon) {
128 2 : v.y = -asin(m.data[2]);
129 2 : float c = cos(v.y);
130 2 : v.x = atan2(m.data[6] / c, m.data[10] / c);
131 2 : v.z = atan2(m.data[1] / c, m.data[0] / c);
132 2 : }
133 : else {
134 1 : v.z = 0;
135 1 : v.y = -atan2(m.data[2], 0);
136 1 : v.x = atan2(-m.data[9], m.data[5]);
137 : }
138 3 : }
139 :
140 : //------------------------------------------------------------------------------
141 12 : void Quaternion::buildRotationMatrix(Matrix &m) {
142 : // precalculate coefficients
143 12 : float addX = x + x;
144 12 : float addY = y + y;
145 12 : float addZ = z + z;
146 12 : float multXX = x * addX;
147 :
148 12 : float multXY = x * addY;
149 12 : float multXZ = x * addZ;
150 12 : float multYY = y * addY;
151 12 : float multYZ = y * addZ;
152 12 : float multZZ = z * addZ;
153 12 : float multWX = w * addX;
154 12 : float multWY = w * addY;
155 12 : float multWZ = w * addZ;
156 :
157 : // column 1
158 12 : m.data[0] = 1.0f - (multYY + multZZ);
159 12 : m.data[4] = multXY - multWZ;
160 12 : m.data[8] = multXZ + multWY;
161 12 : m.data[12] = 0.0f;
162 :
163 : // column 2
164 12 : m.data[1] = multXY + multWZ;
165 12 : m.data[5] = 1.0 - (multXX + multZZ);
166 12 : m.data[9] = multYZ - multWX;
167 12 : m.data[13] = 0.0f;
168 :
169 : // column 3
170 12 : m.data[2] = multXZ - multWY;
171 12 : m.data[6] = multYZ + multWX;
172 12 : m.data[10] = 1.0f - (multXX + multYY);
173 12 : m.data[14] = 0.0f;
174 :
175 : // column 4
176 12 : m.data[3] = 0.0f;
177 12 : m.data[7] = 0.0f;
178 12 : m.data[11] = 0.0f;
179 12 : m.data[15] = 1.0f;
180 12 : }
181 :
182 : //------------------------------------------------------------------------------
183 6 : void Quaternion::slerp(const Quaternion &sQ, const float &t, const Quaternion &eQ) {
184 : // first make sure t is within tolerance
185 6 : if (t >= 0.0f && t <= 1.0f) {
186 : float tx, ty, tz, tw;
187 :
188 4 : float dotQ = (sQ.x * eQ.x) + (sQ.y * eQ.y) + (sQ.z * eQ.z) + (sQ.w * eQ.w);
189 :
190 : // intermediate values used to calculate final quaternion
191 : float omega, sin1, scale1, scale2;
192 :
193 : // setup the temporary quaternion, if the dot product is negative
194 : // setup the quaternion to negative
195 4 : if (dotQ < 0.0f) {
196 1 : dotQ = -dotQ;
197 1 : tx = -eQ.x;
198 1 : ty = -eQ.y;
199 1 : tz = -eQ.z;
200 1 : tw = -eQ.w;
201 1 : }
202 : else {
203 3 : tx = eQ.x;
204 3 : ty = eQ.y;
205 3 : tz = eQ.z;
206 3 : tw = eQ.w;
207 : }
208 :
209 : // calculate values needed for final calculations
210 4 : if ((1.0f - dotQ) > 0.05f) { // 0.05 tolerance
211 3 : omega = acos(dotQ);
212 3 : sin1 = sin (omega);
213 3 : scale1 = sin (( 1.0f - t) * omega) / sin1;
214 3 : scale2 = sin (t * omega) / sin1;
215 3 : }
216 : else {
217 : // within tolerance, do a standard linear interpolation
218 1 : scale1 = 1.0f - t;
219 1 : scale2 = t;
220 : }
221 :
222 : // calculate result quaternion
223 4 : x = scale1 * sQ.x + scale2 * tx;
224 4 : y = scale1 * sQ.y + scale2 * ty;
225 4 : z = scale1 * sQ.z + scale2 * tz;
226 4 : w = scale1 * sQ.w + scale2 * tw;
227 4 : }
228 6 : }
229 :
230 : //------------------------------------------------------------------------------
231 1 : Quaternion Quaternion::operator+(const Quaternion &q) const {
232 1 : float qx = q.x + x;
233 1 : float qy = q.y + y;
234 1 : float qz = q.z + z;
235 1 : float qw = q.w + w;
236 :
237 1 : return (Quaternion(qx, qy, qz, qw));
238 : }
239 :
240 : //------------------------------------------------------------------------------
241 8 : Quaternion Quaternion::operator*(const Quaternion &q) const {
242 8 : float qx = q.w * x + q.x * w + q.y * z - q.z * y;
243 8 : float qy = q.w * y + q.y * w + q.z * x - q.x * z;
244 8 : float qz = q.w * z + q.z * w + q.x * y - q.y * x;
245 8 : float qw = q.w * w - q.x * x - q.y * y - q.z * z;
246 :
247 8 : return(Quaternion(qx, qy, qz, qw));
248 : }
249 :
250 : //------------------------------------------------------------------------------
251 15 : Quaternion& Quaternion::operator=(const Quaternion &q) {
252 15 : x = q.x;
253 15 : y = q.y;
254 15 : z = q.z;
255 15 : w = q.w;
256 :
257 15 : return(*this);
258 : }
|
