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

vsg 1.1.3 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 - 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
151 VSG_type_name(vsg::quat);
152 VSG_type_name(vsg::dquat);
153
154 template<typename T>
155 constexpr bool operator==(const t_quat<T>& lhs, const t_quat<T>& rhs)
156 {
157 return lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2] && lhs[3] == rhs[3];
158 }
159
160 template<typename T>
161 constexpr bool operator!=(const t_quat<T>& lhs, const t_quat<T>& rhs)
162 {
163 return lhs[0] != rhs[0] || lhs[1] != rhs[1] || lhs[2] != rhs[2] || lhs[3] != rhs[3];
164 }
165
166 template<typename T>
167 constexpr bool operator<(const t_quat<T>& lhs, const t_quat<T>& rhs)
168 {
169 if (lhs[0] < rhs[0]) return true;
170 if (lhs[0] > rhs[0]) return false;
171 if (lhs[1] < rhs[1]) return true;
172 if (lhs[1] > rhs[1]) return false;
173 if (lhs[2] < rhs[2]) return true;
174 if (lhs[2] > rhs[2]) return false;
175 return lhs[3] < rhs[3];
176 }
177
178 template<typename T>
179 constexpr t_quat<T> operator-(const t_quat<T>& lhs, const t_quat<T>& rhs)
180 {
181 return t_quat<T>(lhs[0] - rhs[0], lhs[1] - rhs[1], lhs[2] - rhs[2], lhs[3] - rhs[3]);
182 }
183
184 template<typename T>
185 constexpr t_quat<T> conjugate(const t_quat<T>& v)
186 {
187 return t_quat<T>(-v[0], -v[1], -v[2], v[3]);
188 }
189
190 template<typename T>
191 constexpr t_quat<T> operator-(const t_quat<T>& v)
192 {
193 return conjugate(v);
194 }
195
196 template<typename T>
197 constexpr t_quat<T> operator+(const t_quat<T>& lhs, const t_quat<T>& rhs)
198 {
199 return t_quat<T>(lhs[0] + rhs[0], lhs[1] + rhs[1], lhs[2] + rhs[2], lhs[3] + rhs[3]);
200 }
201
202 // Rotate a quaternion by another quaternion
203 template<typename T>
204 constexpr t_quat<T> operator*(const t_quat<T>& lhs, const t_quat<T>& rhs)
205 {
206 t_quat<T> q(rhs[3] * lhs[0] + rhs[0] * lhs[3] + rhs[1] * lhs[2] - rhs[2] * lhs[1],
207 rhs[3] * lhs[1] - rhs[0] * lhs[2] + rhs[1] * lhs[3] + rhs[2] * lhs[0],
208 rhs[3] * lhs[2] + rhs[0] * lhs[1] - rhs[1] * lhs[0] + rhs[2] * lhs[3],
209 rhs[3] * lhs[3] - rhs[0] * lhs[0] - rhs[1] * lhs[1] - rhs[2] * lhs[2]);
210
211 return q;
212 }
213
214 // Rotate a vector by a quaternion
215 template<typename T>
216 constexpr t_vec3<T> operator*(const t_quat<T>& q, const t_vec3<T>& v)
217 {
218 // nVidia SDK implementation
219 t_vec3<T> uv, uuv;
220 t_vec3<T> qvec(q[0], q[1], q[2]);
221 uv = cross(qvec, v);
222 uuv = cross(qvec, uv);
223 T two(2.0);
224 uv *= (two * q[3]);
225 uuv *= two;
226 return v + uv + uuv;
227 }
228
229 template<typename T>
230 constexpr t_quat<T> operator*(const t_quat<T>& lhs, T rhs)
231 {
232 return t_quat<T>(lhs[0] * rhs, lhs[1] * rhs, lhs[2] * rhs, lhs[3] * rhs);
233 }
234
235 template<typename T>
236 constexpr t_quat<T> operator/(const t_quat<T>& lhs, T rhs)
237 {
238 T inv = static_cast<T>(1.0) / rhs;
239 return t_quat<T>(lhs[0] * inv, lhs[1] * inv, lhs[2] * inv, lhs[3] * inv);
240 }
241
242 template<typename T>
243 constexpr T length(const t_quat<T>& v)
244 {
245 return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
246 }
247
248 template<typename T>
249 constexpr t_quat<T> normalize(const t_quat<T>& v)
250 {
251 T inverse_len = static_cast<T>(1.0) / length(v);
252 return t_quat<T>(v[0] * inverse_len, v[1] * inverse_len, v[2] * inverse_len, v[3] * inverse_len);
253 }
254
255 template<typename T>
256 constexpr T dot(const t_quat<T>& lhs, const t_quat<T>& rhs)
257 {
258 return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + lhs[3] * rhs[3];
259 }
260
261 template<typename T>
262 constexpr t_quat<T> inverse(const t_quat<T>& v)
263 {
264 t_quat<T> c = conjugate(v);
265 T inverse_len = static_cast<T>(1.0) / length(v);
266 return t_quat<T>(c[0] * inverse_len, c[1] * inverse_len, c[2] * inverse_len, c[3] * inverse_len);
267 }
268
269 template<typename T>
270 constexpr t_quat<T> mix(const t_quat<T>& from, t_quat<T> to, T r)
271 {
272 T epsilon = std::numeric_limits<T>::epsilon();
273 T one(1.0);
274
275 T cosomega = dot(from, to);
276 if (cosomega < 0.0)
277 {
278 cosomega = -cosomega;
279 to.x = -to.x;
280 to.y = -to.y;
281 to.z = -to.z;
282 to.w = -to.w;
283 }
284
285 if ((one - cosomega) > epsilon)
286 {
287 T omega = acos(cosomega);
288 T sinomega = sin(omega);
289 T scale_from = sin((one - r) * omega) / sinomega;
290 T scale_to = sin(r * omega) / sinomega;
291 return (from * scale_from) + (to * scale_to);
292 }
293 else
294 {
295 // quaternions are very close so just linearly interpolate
296 return (from * (one - r)) + (to * r);
297 }
298 }
299
300} // namespace vsg
301
302#if defined(__clang__)
303# pragma clang diagnostic pop
304#endif
305#if defined(__GNUC__)
306# pragma GCC diagnostic pop
307#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