vsg: include/vsg/maths/quat.h Source File

vsg 1.1.9 VulkanSceneGraph library |

Loading...
Searching...
No Matches
quat.h
1#pragma once
2
3/* <editor-fold desc="MIT License">
4
5Copyright(c) 2018 Robert Osfield
6
7Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
8
9The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
10
11THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
12
13</editor-fold> */
14
15// we can't implement the anonymous union/structs combination without causing warnings, so disable them for just this header
16
17#include <vsg/maths/mat4.h>
18
19#if defined(__GNUC__)
20# pragma GCC diagnostic push
21# pragma GCC diagnostic ignored "-Wpedantic"
22#endif
23#if defined(__clang__)
24# pragma clang diagnostic push
25# pragma clang diagnostic ignored "-Wgnu-anonymous-struct"
26# pragma clang diagnostic ignored "-Wnested-anon-types"
27#endif
28
29namespace vsg
30{
31
33 template<typename T>
34 struct t_quat
35 {
36 using value_type = T;
37
38 union
39 {
40 value_type value[4];
41 struct
42 {
43 value_type x, y, z, w;
44 };
45 };
46
47 constexpr t_quat() :
48 value{0.0, 0.0, 0.0, 1.0} {}
49 constexpr t_quat(const t_quat& v) :
50 value{v.x, v.y, v.z, v.w} {}
51 constexpr t_quat(value_type in_x, value_type in_y, value_type in_z, value_type in_w) :
52 value{in_x, in_y, in_z, in_w} {}
53 constexpr t_quat(value_type angle_radians, const t_vec3<value_type>& axis)
54 {
55 set(angle_radians, axis);
56 }
57 constexpr t_quat(const t_vec3<value_type>& from, const t_vec3<value_type>& to)
58 {
59 set(from, to);
60 }
61
62 template<typename R>
63 constexpr explicit t_quat(const t_quat<R>& v) :
64 value{static_cast<T>(v.x), static_cast<T>(v.y), static_cast<T>(v.z), static_cast<T>(v.w)} {}
65
66 constexpr t_quat& operator=(const t_quat&) = default;
67
68 constexpr std::size_t size() const { return 4; }
69
70 value_type& operator[](std::size_t i) { return value[i]; }
71 value_type operator[](std::size_t i) const { return value[i]; }
72
73 template<typename R>
74 t_quat& operator=(const t_quat<R>& rhs)
75 {
76 value[0] = static_cast<value_type>(rhs[0]);
77 value[1] = static_cast<value_type>(rhs[1]);
78 value[2] = static_cast<value_type>(rhs[2]);
79 value[3] = static_cast<value_type>(rhs[3]);
80 return *this;
81 }
82
83 T* data() { return value; }
84 const T* data() const { return value; }
85
86 void set(value_type in_x, value_type in_y, value_type in_z, value_type in_w)
87 {
88 x = in_x;
89 y = in_y;
90 z = in_z;
91 w = in_w;
92 }
93
94 void set(value_type angle_radians, const t_vec3<value_type>& axis)
95 {
96 const value_type epsilon = 1e-7;
97 value_type len = length(axis);
98 if (len < epsilon)
99 {
100 // ~zero length axis, so reset rotation to zero.
101 *this = {};
102 return;
103 }
104
105 value_type inversenorm = 1.0 / len;
106 value_type coshalfangle = cos(0.5 * angle_radians);
107 value_type sinhalfangle = sin(0.5 * angle_radians);
108
109 x = axis.x * sinhalfangle * inversenorm;
110 y = axis.y * sinhalfangle * inversenorm;
111 z = axis.z * sinhalfangle * inversenorm;
112 w = coshalfangle;
113 }
114
115 void set(const t_vec3<value_type>& from, const t_vec3<value_type>& to)
116 {
117 const value_type epsilon = 1e-7;
118
119 value_type dot_pd = vsg::dot(from, to);
120 value_type div = std::sqrt(length2(from) * length2(to));
121 vsg::dvec3 axis;
122 if (div - std::abs(dot_pd) < epsilon)
123 {
124 axis = orthogonal(from);
125 }
126 else
127 {
128 axis = cross(from, to);
129 }
130
131 value_type len = length(axis);
132
133 double angle_radians = acos(dot_pd / div);
134
135 value_type inversenorm = 1.0 / len;
136 value_type coshalfangle = cos(0.5 * angle_radians);
137 value_type sinhalfangle = sin(0.5 * angle_radians);
138
139 x = axis.x * sinhalfangle * inversenorm;
140 y = axis.y * sinhalfangle * inversenorm;
141 z = axis.z * sinhalfangle * inversenorm;
142 w = coshalfangle;
143 }
144
145 explicit operator bool() const noexcept { return value[0] != 0.0 || value[1] != 0.0 || value[2] != 0.0 || value[3] != 0.0; }
146 };
147
148 using quat = t_quat<float>;
149 using dquat = t_quat<double>;
150 using ldquat = t_quat<long double>;
151
152 VSG_type_name(vsg::quat);
153 VSG_type_name(vsg::dquat);
154 VSG_type_name(vsg::ldquat);
155
156 template<typename T>
157 constexpr bool operator==(const t_quat<T>& lhs, const t_quat<T>& rhs)
158 {
159 return lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2] && lhs[3] == rhs[3];
160 }
161
162 template<typename T>
163 constexpr bool operator!=(const t_quat<T>& lhs, const t_quat<T>& rhs)
164 {
165 return lhs[0] != rhs[0] || lhs[1] != rhs[1] || lhs[2] != rhs[2] || lhs[3] != rhs[3];
166 }
167
168 template<typename T>
169 constexpr bool operator<(const t_quat<T>& lhs, const t_quat<T>& rhs)
170 {
171 if (lhs[0] < rhs[0]) return true;
172 if (lhs[0] > rhs[0]) return false;
173 if (lhs[1] < rhs[1]) return true;
174 if (lhs[1] > rhs[1]) return false;
175 if (lhs[2] < rhs[2]) return true;
176 if (lhs[2] > rhs[2]) return false;
177 return lhs[3] < rhs[3];
178 }
179
180 template<typename T>
181 constexpr t_quat<T> operator-(const t_quat<T>& lhs, const t_quat<T>& rhs)
182 {
183 return t_quat<T>(lhs[0] - rhs[0], lhs[1] - rhs[1], lhs[2] - rhs[2], lhs[3] - rhs[3]);
184 }
185
186 template<typename T>
187 constexpr t_quat<T> conjugate(const t_quat<T>& v)
188 {
189 return t_quat<T>(-v[0], -v[1], -v[2], v[3]);
190 }
191
192 template<typename T>
193 constexpr t_quat<T> operator-(const t_quat<T>& v)
194 {
195 return conjugate(v);
196 }
197
198 template<typename T>
199 constexpr t_quat<T> operator+(const t_quat<T>& lhs, const t_quat<T>& rhs)
200 {
201 return t_quat<T>(lhs[0] + rhs[0], lhs[1] + rhs[1], lhs[2] + rhs[2], lhs[3] + rhs[3]);
202 }
203
204 // Rotate a quaternion by another quaternion
205 template<typename T>
206 constexpr t_quat<T> operator*(const t_quat<T>& lhs, const t_quat<T>& rhs)
207 {
208 t_quat<T> q(rhs[3] * lhs[0] + rhs[0] * lhs[3] + rhs[1] * lhs[2] - rhs[2] * lhs[1],
209 rhs[3] * lhs[1] - rhs[0] * lhs[2] + rhs[1] * lhs[3] + rhs[2] * lhs[0],
210 rhs[3] * lhs[2] + rhs[0] * lhs[1] - rhs[1] * lhs[0] + rhs[2] * lhs[3],
211 rhs[3] * lhs[3] - rhs[0] * lhs[0] - rhs[1] * lhs[1] - rhs[2] * lhs[2]);
212
213 return q;
214 }
215
216 // Rotate a vector by a quaternion
217 template<typename T>
218 constexpr t_vec3<T> operator*(const t_quat<T>& q, const t_vec3<T>& v)
219 {
220 // nVidia SDK implementation
221 t_vec3<T> uv, uuv;
222 t_vec3<T> qvec(q[0], q[1], q[2]);
223 uv = cross(qvec, v);
224 uuv = cross(qvec, uv);
225 T two(2.0);
226 uv *= (two * q[3]);
227 uuv *= two;
228 return v + uv + uuv;
229 }
230
231 template<typename T>
232 constexpr t_quat<T> operator*(const t_quat<T>& lhs, T rhs)
233 {
234 return t_quat<T>(lhs[0] * rhs, lhs[1] * rhs, lhs[2] * rhs, lhs[3] * rhs);
235 }
236
237 template<typename T>
238 constexpr t_quat<T> operator/(const t_quat<T>& lhs, T rhs)
239 {
240 T inv = static_cast<T>(1.0) / rhs;
241 return t_quat<T>(lhs[0] * inv, lhs[1] * inv, lhs[2] * inv, lhs[3] * inv);
242 }
243
244 template<typename T>
245 constexpr T length(const t_quat<T>& v)
246 {
247 return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
248 }
249
250 template<typename T>
251 constexpr t_quat<T> normalize(const t_quat<T>& v)
252 {
253 T inverse_len = static_cast<T>(1.0) / length(v);
254 return t_quat<T>(v[0] * inverse_len, v[1] * inverse_len, v[2] * inverse_len, v[3] * inverse_len);
255 }
256
257 template<typename T>
258 constexpr T dot(const t_quat<T>& lhs, const t_quat<T>& rhs)
259 {
260 return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + lhs[3] * rhs[3];
261 }
262
263 template<typename T>
264 constexpr t_quat<T> inverse(const t_quat<T>& v)
265 {
266 t_quat<T> c = conjugate(v);
267 T inverse_len = static_cast<T>(1.0) / length(v);
268 return t_quat<T>(c[0] * inverse_len, c[1] * inverse_len, c[2] * inverse_len, c[3] * inverse_len);
269 }
270
271 template<typename T>
272 constexpr t_quat<T> mix(const t_quat<T>& from, t_quat<T> to, T r)
273 {
274 T epsilon = std::numeric_limits<T>::epsilon();
275 T one(1.0);
276
277 T cosomega = dot(from, to);
278 if (cosomega < 0.0)
279 {
280 cosomega = -cosomega;
281 to.x = -to.x;
282 to.y = -to.y;
283 to.z = -to.z;
284 to.w = -to.w;
285 }
286
287 if ((one - cosomega) > epsilon)
288 {
289 T omega = acos(cosomega);
290 T sinomega = sin(omega);
291 T scale_from = sin((one - r) * omega) / sinomega;
292 T scale_to = sin(r * omega) / sinomega;
293 return (from * scale_from) + (to * scale_to);
294 }
295 else
296 {
297 // quaternions are very close so just linearly interpolate
298 return (from * (one - r)) + (to * r);
299 }
300 }
301
302} // namespace vsg
303
304#if defined(__clang__)
305# pragma clang diagnostic pop
306#endif
307#if defined(__GNUC__)
308# pragma GCC diagnostic pop
309#endif
vsg::t_quat
t_quat template class that represents a quaternion
Definition quat.h:35
vsg::t_vec3
t_vec3 template class that represents a 3D vector
Definition vec3.h:34