1 /*************************************************************************/
2 /* quat.cpp */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2019 Godot Engine contributors (cf. AUTHORS.md) */
10 /* */
11 /* Permission is hereby granted, free of charge, to any person obtaining */
12 /* a copy of this software and associated documentation files (the */
13 /* "Software"), to deal in the Software without restriction, including */
14 /* without limitation the rights to use, copy, modify, merge, publish, */
15 /* distribute, sublicense, and/or sell copies of the Software, and to */
16 /* permit persons to whom the Software is furnished to do so, subject to */
17 /* the following conditions: */
18 /* */
19 /* The above copyright notice and this permission notice shall be */
20 /* included in all copies or substantial portions of the Software. */
21 /* */
22 /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23 /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24 /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
25 /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26 /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29 /*************************************************************************/
30 #include "quat.h"
31 #include "print_string.h"
32
set_euler(const Vector3 & p_euler)33 void Quat::set_euler(const Vector3 &p_euler) {
34 real_t half_yaw = p_euler.x * 0.5;
35 real_t half_pitch = p_euler.y * 0.5;
36 real_t half_roll = p_euler.z * 0.5;
37 real_t cos_yaw = Math::cos(half_yaw);
38 real_t sin_yaw = Math::sin(half_yaw);
39 real_t cos_pitch = Math::cos(half_pitch);
40 real_t sin_pitch = Math::sin(half_pitch);
41 real_t cos_roll = Math::cos(half_roll);
42 real_t sin_roll = Math::sin(half_roll);
43 set(cos_roll * sin_pitch * cos_yaw + sin_roll * cos_pitch * sin_yaw,
44 cos_roll * cos_pitch * sin_yaw - sin_roll * sin_pitch * cos_yaw,
45 sin_roll * cos_pitch * cos_yaw - cos_roll * sin_pitch * sin_yaw,
46 cos_roll * cos_pitch * cos_yaw + sin_roll * sin_pitch * sin_yaw);
47 }
48
operator *=(const Quat & q)49 void Quat::operator*=(const Quat &q) {
50
51 set(w * q.x + x * q.w + y * q.z - z * q.y,
52 w * q.y + y * q.w + z * q.x - x * q.z,
53 w * q.z + z * q.w + x * q.y - y * q.x,
54 w * q.w - x * q.x - y * q.y - z * q.z);
55 }
56
operator *(const Quat & q) const57 Quat Quat::operator*(const Quat &q) const {
58
59 Quat r = *this;
60 r *= q;
61 return r;
62 }
63
length() const64 real_t Quat::length() const {
65
66 return Math::sqrt(length_squared());
67 }
68
normalize()69 void Quat::normalize() {
70 *this /= length();
71 }
72
normalized() const73 Quat Quat::normalized() const {
74 return *this / length();
75 }
76
inverse() const77 Quat Quat::inverse() const {
78 return Quat(-x, -y, -z, w);
79 }
80
slerp(const Quat & q,const real_t & t) const81 Quat Quat::slerp(const Quat &q, const real_t &t) const {
82
83 #if 0
84
85
86 Quat dst=q;
87 Quat src=*this;
88
89 src.normalize();
90 dst.normalize();
91
92 real_t cosine = dst.dot(src);
93
94 if (cosine < 0 && true) {
95 cosine = -cosine;
96 dst = -dst;
97 } else {
98 dst = dst;
99 }
100
101 if (Math::abs(cosine) < 1 - CMP_EPSILON) {
102 // Standard case (slerp)
103 real_t sine = Math::sqrt(1 - cosine*cosine);
104 real_t angle = Math::atan2(sine, cosine);
105 real_t inv_sine = 1.0f / sine;
106 real_t coeff_0 = Math::sin((1.0f - t) * angle) * inv_sine;
107 real_t coeff_1 = Math::sin(t * angle) * inv_sine;
108 Quat ret= src * coeff_0 + dst * coeff_1;
109
110 return ret;
111 } else {
112 // There are two situations:
113 // 1. "rkP" and "q" are very close (cosine ~= +1), so we can do a linear
114 // interpolation safely.
115 // 2. "rkP" and "q" are almost invedste of each other (cosine ~= -1), there
116 // are an infinite number of possibilities interpolation. but we haven't
117 // have method to fix this case, so just use linear interpolation here.
118 Quat ret = src * (1.0f - t) + dst *t;
119 // taking the complement requires renormalisation
120 ret.normalize();
121 return ret;
122 }
123 #else
124
125 real_t to1[4];
126 real_t omega, cosom, sinom, scale0, scale1;
127
128 // calc cosine
129 cosom = x * q.x + y * q.y + z * q.z + w * q.w;
130
131 // adjust signs (if necessary)
132 if (cosom < 0.0) {
133 cosom = -cosom;
134 to1[0] = -q.x;
135 to1[1] = -q.y;
136 to1[2] = -q.z;
137 to1[3] = -q.w;
138 } else {
139 to1[0] = q.x;
140 to1[1] = q.y;
141 to1[2] = q.z;
142 to1[3] = q.w;
143 }
144
145 // calculate coefficients
146
147 if ((1.0 - cosom) > CMP_EPSILON) {
148 // standard case (slerp)
149 omega = Math::acos(cosom);
150 sinom = Math::sin(omega);
151 scale0 = Math::sin((1.0 - t) * omega) / sinom;
152 scale1 = Math::sin(t * omega) / sinom;
153 } else {
154 // "from" and "to" quaternions are very close
155 // ... so we can do a linear interpolation
156 scale0 = 1.0 - t;
157 scale1 = t;
158 }
159 // calculate final values
160 return Quat(
161 scale0 * x + scale1 * to1[0],
162 scale0 * y + scale1 * to1[1],
163 scale0 * z + scale1 * to1[2],
164 scale0 * w + scale1 * to1[3]);
165 #endif
166 }
167
slerpni(const Quat & q,const real_t & t) const168 Quat Quat::slerpni(const Quat &q, const real_t &t) const {
169
170 const Quat &from = *this;
171
172 float dot = from.dot(q);
173
174 if (Math::absf(dot) > 0.9999f) return from;
175
176 float theta = Math::acos(dot),
177 sinT = 1.0f / Math::sin(theta),
178 newFactor = Math::sin(t * theta) * sinT,
179 invFactor = Math::sin((1.0f - t) * theta) * sinT;
180
181 return Quat(invFactor * from.x + newFactor * q.x,
182 invFactor * from.y + newFactor * q.y,
183 invFactor * from.z + newFactor * q.z,
184 invFactor * from.w + newFactor * q.w);
185
186 #if 0
187 real_t to1[4];
188 real_t omega, cosom, sinom, scale0, scale1;
189
190
191 // calc cosine
192 cosom = x * q.x + y * q.y + z * q.z
193 + w * q.w;
194
195
196 // adjust signs (if necessary)
197 if ( cosom <0.0 && false) {
198 cosom = -cosom; to1[0] = - q.x;
199 to1[1] = - q.y;
200 to1[2] = - q.z;
201 to1[3] = - q.w;
202 } else {
203 to1[0] = q.x;
204 to1[1] = q.y;
205 to1[2] = q.z;
206 to1[3] = q.w;
207 }
208
209
210 // calculate coefficients
211
212 if ( (1.0 - cosom) > CMP_EPSILON ) {
213 // standard case (slerp)
214 omega = Math::acos(cosom);
215 sinom = Math::sin(omega);
216 scale0 = Math::sin((1.0 - t) * omega) / sinom;
217 scale1 = Math::sin(t * omega) / sinom;
218 } else {
219 // "from" and "to" quaternions are very close
220 // ... so we can do a linear interpolation
221 scale0 = 1.0 - t;
222 scale1 = t;
223 }
224 // calculate final values
225 return Quat(
226 scale0 * x + scale1 * to1[0],
227 scale0 * y + scale1 * to1[1],
228 scale0 * z + scale1 * to1[2],
229 scale0 * w + scale1 * to1[3]
230 );
231 #endif
232 }
233
cubic_slerp(const Quat & q,const Quat & prep,const Quat & postq,const real_t & t) const234 Quat Quat::cubic_slerp(const Quat &q, const Quat &prep, const Quat &postq, const real_t &t) const {
235
236 //the only way to do slerp :|
237 float t2 = (1.0 - t) * t * 2;
238 Quat sp = this->slerp(q, t);
239 Quat sq = prep.slerpni(postq, t);
240 return sp.slerpni(sq, t2);
241 }
242
operator String() const243 Quat::operator String() const {
244
245 return String::num(x) + ", " + String::num(y) + ", " + String::num(z) + ", " + String::num(w);
246 }
247
Quat(const Vector3 & axis,const real_t & angle)248 Quat::Quat(const Vector3 &axis, const real_t &angle) {
249 real_t d = axis.length();
250 if (d == 0)
251 set(0, 0, 0, 0);
252 else {
253 real_t s = Math::sin(-angle * 0.5) / d;
254 set(axis.x * s, axis.y * s, axis.z * s,
255 Math::cos(-angle * 0.5));
256 }
257 }
258