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

vsg 1.1.13 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{numbers<value_type>::zero(), numbers<value_type>::zero(), numbers<value_type>::zero(), numbers<value_type>::one()} {}
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 value_type len = length(axis);
97 if (len < numbers<value_type>::epsilon())
98 {
99 // ~zero length axis, so reset rotation to zero.
100 *this = {};
101 return;
102 }
103
104 value_type inversenorm = numbers<value_type>::one() / len;
105 value_type coshalfangle = cos(numbers<value_type>::half() * angle_radians);
106 value_type sinhalfangle = sin(numbers<value_type>::half() * angle_radians);
107
108 x = axis.x * sinhalfangle * inversenorm;
109 y = axis.y * sinhalfangle * inversenorm;
110 z = axis.z * sinhalfangle * inversenorm;
111 w = coshalfangle;
112 }
113
114 void set(const t_vec3<value_type>& from, const t_vec3<value_type>& to)
115 {
116 value_type dot_pd = vsg::dot(from, to);
117 value_type div = std::sqrt(length2(from) * length2(to));
118 vsg::dvec3 axis;
119 if (div - std::abs(dot_pd) < numbers<value_type>::epsilon())
120 {
121 axis = orthogonal(from);
122 }
123 else
124 {
125 axis = cross(from, to);
126 }
127
128 value_type len = length(axis);
129
130 double angle_radians = acos(dot_pd / div);
131
132 value_type inversenorm = numbers<value_type>::one() / len;
133 value_type coshalfangle = cos(numbers<value_type>::half() * angle_radians);
134 value_type sinhalfangle = sin(numbers<value_type>::half() * angle_radians);
135
136 x = axis.x * sinhalfangle * inversenorm;
137 y = axis.y * sinhalfangle * inversenorm;
138 z = axis.z * sinhalfangle * inversenorm;
139 w = coshalfangle;
140 }
141
142 explicit operator bool() const noexcept { return value[0] != numbers<value_type>::zero() || value[1] != numbers<value_type>::zero() || value[2] != numbers<value_type>::zero() || value[3] != numbers<value_type>::zero(); }
143 };
144
145 using quat = t_quat<float>;
146 using dquat = t_quat<double>;
147 using ldquat = t_quat<long double>;
148
149 VSG_type_name(vsg::quat);
150 VSG_type_name(vsg::dquat);
151 VSG_type_name(vsg::ldquat);
152
153 template<typename T>
154 constexpr bool operator==(const t_quat<T>& lhs, const t_quat<T>& rhs)
155 {
156 return lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2] && lhs[3] == rhs[3];
157 }
158
159 template<typename T>
160 constexpr bool operator!=(const t_quat<T>& lhs, const t_quat<T>& rhs)
161 {
162 return lhs[0] != rhs[0] || lhs[1] != rhs[1] || lhs[2] != rhs[2] || lhs[3] != rhs[3];
163 }
164
165 template<typename T>
166 constexpr bool operator<(const t_quat<T>& lhs, const t_quat<T>& rhs)
167 {
168 if (lhs[0] < rhs[0]) return true;
169 if (lhs[0] > rhs[0]) return false;
170 if (lhs[1] < rhs[1]) return true;
171 if (lhs[1] > rhs[1]) return false;
172 if (lhs[2] < rhs[2]) return true;
173 if (lhs[2] > rhs[2]) return false;
174 return lhs[3] < rhs[3];
175 }
176
177 template<typename T>
178 constexpr t_quat<T> operator-(const t_quat<T>& lhs, const t_quat<T>& rhs)
179 {
180 return t_quat<T>(lhs[0] - rhs[0], lhs[1] - rhs[1], lhs[2] - rhs[2], lhs[3] - rhs[3]);
181 }
182
183 template<typename T>
184 constexpr t_quat<T> conjugate(const t_quat<T>& v)
185 {
186 return t_quat<T>(-v[0], -v[1], -v[2], v[3]);
187 }
188
189 template<typename T>
190 constexpr t_quat<T> operator-(const t_quat<T>& v)
191 {
192 return conjugate(v);
193 }
194
195 template<typename T>
196 constexpr t_quat<T> operator+(const t_quat<T>& lhs, const t_quat<T>& rhs)
197 {
198 return t_quat<T>(lhs[0] + rhs[0], lhs[1] + rhs[1], lhs[2] + rhs[2], lhs[3] + rhs[3]);
199 }
200
201 // Rotate a quaternion by another quaternion
202 template<typename T>
203 constexpr t_quat<T> operator*(const t_quat<T>& lhs, const t_quat<T>& rhs)
204 {
205 t_quat<T> q(rhs[3] * lhs[0] + rhs[0] * lhs[3] + rhs[1] * lhs[2] - rhs[2] * lhs[1],
206 rhs[3] * lhs[1] - rhs[0] * lhs[2] + rhs[1] * lhs[3] + rhs[2] * lhs[0],
207 rhs[3] * lhs[2] + rhs[0] * lhs[1] - rhs[1] * lhs[0] + rhs[2] * lhs[3],
208 rhs[3] * lhs[3] - rhs[0] * lhs[0] - rhs[1] * lhs[1] - rhs[2] * lhs[2]);
209
210 return q;
211 }
212
213 // Rotate a vector by a quaternion
214 template<typename T>
215 constexpr t_vec3<T> operator*(const t_quat<T>& q, const t_vec3<T>& v)
216 {
217 // nVidia SDK implementation
218 t_vec3<T> uv, uuv;
219 t_vec3<T> qvec(q[0], q[1], q[2]);
220 uv = cross(qvec, v);
221 uuv = cross(qvec, uv);
222 uv *= (numbers<T>::two() * q[3]);
223 uuv *= numbers<T>::two();
224 return v + uv + uuv;
225 }
226
227 template<typename T>
228 constexpr t_quat<T> operator*(const t_quat<T>& lhs, T rhs)
229 {
230 return t_quat<T>(lhs[0] * rhs, lhs[1] * rhs, lhs[2] * rhs, lhs[3] * rhs);
231 }
232
233 template<typename T>
234 constexpr t_quat<T> operator/(const t_quat<T>& lhs, T rhs)
235 {
236 T inv = numbers<T>::one() / rhs;
237 return t_quat<T>(lhs[0] * inv, lhs[1] * inv, lhs[2] * inv, lhs[3] * inv);
238 }
239
240 template<typename T>
241 constexpr T length(const t_quat<T>& v)
242 {
243 return std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
244 }
245
246 template<typename T>
247 constexpr t_quat<T> normalize(const t_quat<T>& v)
248 {
249 T inverse_len = numbers<T>::one() / length(v);
250 return t_quat<T>(v[0] * inverse_len, v[1] * inverse_len, v[2] * inverse_len, v[3] * inverse_len);
251 }
252
253 template<typename T>
254 constexpr T dot(const t_quat<T>& lhs, const t_quat<T>& rhs)
255 {
256 return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + lhs[3] * rhs[3];
257 }
258
259 template<typename T>
260 constexpr t_quat<T> inverse(const t_quat<T>& v)
261 {
262 t_quat<T> c = conjugate(v);
263 T inverse_len = numbers<T>::one() / length(v);
264 return t_quat<T>(c[0] * inverse_len, c[1] * inverse_len, c[2] * inverse_len, c[3] * inverse_len);
265 }
266
267 template<typename T>
268 constexpr t_quat<T> mix(const t_quat<T>& from, t_quat<T> to, T r)
269 {
270 T cosomega = dot(from, to);
271 if (cosomega < numbers<T>::zero())
272 {
273 cosomega = -cosomega;
274 to.x = -to.x;
275 to.y = -to.y;
276 to.z = -to.z;
277 to.w = -to.w;
278 }
279
280 if ((numbers<T>::one() - cosomega) > numbers<T>::epsilon())
281 {
282 T omega = acos(cosomega);
283 T sinomega = sin(omega);
284 T scale_from = sin((numbers<T>::one() - r) * omega) / sinomega;
285 T scale_to = sin(r * omega) / sinomega;
286 return (from * scale_from) + (to * scale_to);
287 }
288 else
289 {
290 // quaternions are very close so just linearly interpolate
291 return (from * (numbers<T>::one() - r)) + (to * r);
292 }
293 }
294
295} // namespace vsg
296
297#if defined(__clang__)
298# pragma clang diagnostic pop
299#endif
300#if defined(__GNUC__)
301# pragma GCC diagnostic pop
302#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