suwako / VulkanExample

master
ブランチ master
VulkanExample / dep / include / glm / gtx / dual_quaternion.inl
10 KB、334行 生 Blame 履歴
1/// @ref gtx_dual_quaternion
2
3#include "../geometric.hpp"
4#include <limits>
5
6namespace glm
7{
8 // -- Component accesses --
9
10 template<typename T, qualifier Q>
11 GLM_FUNC_QUALIFIER GLM_CONSTEXPR typename tdualquat<T, Q>::part_type & tdualquat<T, Q>::operator[](typename tdualquat<T, Q>::length_type i) noexcept
12 {
13 assert(i >= 0 && i < this->length());
14 return (&real)[i];
15 }
16
17 template<typename T, qualifier Q>
18 GLM_FUNC_QUALIFIER GLM_CONSTEXPR typename tdualquat<T, Q>::part_type const& tdualquat<T, Q>::operator[](typename tdualquat<T, Q>::length_type i) const noexcept
19 {
20 assert(i >= 0 && i < this->length());
21 return (&real)[i];
22 }
23
24 // -- Implicit basic constructors --
25
26# if GLM_CONFIG_CTOR_INIT == GLM_ENABLE
27 template<typename T, qualifier Q>
28 GLM_DEFAULTED_DEFAULT_CTOR_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat()
29 : real(qua<T, Q>())
30 , dual(qua<T, Q>::wxyz(0, 0, 0, 0))
31 {}
32# endif
33
34 template<typename T, qualifier Q>
35 template<qualifier P>
36 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat(tdualquat<T, P> const& d)
37 : real(d.real)
38 , dual(d.dual)
39 {}
40
41 // -- Explicit basic constructors --
42
43 template<typename T, qualifier Q>
44 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat(qua<T, Q> const& r)
45 : real(r), dual(qua<T, Q>::wxyz(0, 0, 0, 0))
46 {}
47
48 template<typename T, qualifier Q>
49 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat(qua<T, Q> const& q, vec<3, T, Q> const& p)
50 : real(q), dual(qua<T, Q>::wxyz(
51 T(-0.5) * ( p.x*q.x + p.y*q.y + p.z*q.z),
52 T(+0.5) * ( p.x*q.w + p.y*q.z - p.z*q.y),
53 T(+0.5) * (-p.x*q.z + p.y*q.w + p.z*q.x),
54 T(+0.5) * ( p.x*q.y - p.y*q.x + p.z*q.w)))
55 {}
56
57 template<typename T, qualifier Q>
58 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat(qua<T, Q> const& r, qua<T, Q> const& d)
59 : real(r), dual(d)
60 {}
61
62 // -- Conversion constructors --
63
64 template<typename T, qualifier Q>
65 template<typename U, qualifier P>
66 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat(tdualquat<U, P> const& q)
67 : real(q.real)
68 , dual(q.dual)
69 {}
70
71 template<typename T, qualifier Q>
72 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat(mat<2, 4, T, Q> const& m)
73 {
74 *this = dualquat_cast(m);
75 }
76
77 template<typename T, qualifier Q>
78 GLM_FUNC_QUALIFIER GLM_CONSTEXPR tdualquat<T, Q>::tdualquat(mat<3, 4, T, Q> const& m)
79 {
80 *this = dualquat_cast(m);
81 }
82
83 // -- Unary arithmetic operators --
84
85 template<typename T, qualifier Q>
86 template<typename U>
87 GLM_FUNC_QUALIFIER tdualquat<T, Q> & tdualquat<T, Q>::operator=(tdualquat<U, Q> const& q)
88 {
89 this->real = q.real;
90 this->dual = q.dual;
91 return *this;
92 }
93
94 template<typename T, qualifier Q>
95 template<typename U>
96 GLM_FUNC_QUALIFIER tdualquat<T, Q> & tdualquat<T, Q>::operator*=(U s)
97 {
98 this->real *= static_cast<T>(s);
99 this->dual *= static_cast<T>(s);
100 return *this;
101 }
102
103 template<typename T, qualifier Q>
104 template<typename U>
105 GLM_FUNC_QUALIFIER tdualquat<T, Q> & tdualquat<T, Q>::operator/=(U s)
106 {
107 this->real /= static_cast<T>(s);
108 this->dual /= static_cast<T>(s);
109 return *this;
110 }
111
112 // -- Unary bit operators --
113
114 template<typename T, qualifier Q>
115 GLM_FUNC_QUALIFIER tdualquat<T, Q> operator+(tdualquat<T, Q> const& q)
116 {
117 return q;
118 }
119
120 template<typename T, qualifier Q>
121 GLM_FUNC_QUALIFIER tdualquat<T, Q> operator-(tdualquat<T, Q> const& q)
122 {
123 return tdualquat<T, Q>(-q.real, -q.dual);
124 }
125
126 // -- Binary operators --
127
128 template<typename T, qualifier Q>
129 GLM_FUNC_QUALIFIER tdualquat<T, Q> operator+(tdualquat<T, Q> const& q, tdualquat<T, Q> const& p)
130 {
131 return tdualquat<T, Q>(q.real + p.real,q.dual + p.dual);
132 }
133
134 template<typename T, qualifier Q>
135 GLM_FUNC_QUALIFIER tdualquat<T, Q> operator*(tdualquat<T, Q> const& p, tdualquat<T, Q> const& o)
136 {
137 return tdualquat<T, Q>(p.real * o.real,p.real * o.dual + p.dual * o.real);
138 }
139
140 template<typename T, qualifier Q>
141 GLM_FUNC_QUALIFIER vec<3, T, Q> operator*(tdualquat<T, Q> const& q, vec<3, T, Q> const& v)
142 {
143 vec<3, T, Q> const real_v3(q.real.x,q.real.y,q.real.z);
144 vec<3, T, Q> const dual_v3(q.dual.x,q.dual.y,q.dual.z);
145 return (cross(real_v3, cross(real_v3,v) + v * q.real.w + dual_v3) + dual_v3 * q.real.w - real_v3 * q.dual.w) * T(2) + v;
146 }
147
148 template<typename T, qualifier Q>
149 GLM_FUNC_QUALIFIER vec<3, T, Q> operator*(vec<3, T, Q> const& v, tdualquat<T, Q> const& q)
150 {
151 return glm::inverse(q) * v;
152 }
153
154 template<typename T, qualifier Q>
155 GLM_FUNC_QUALIFIER vec<4, T, Q> operator*(tdualquat<T, Q> const& q, vec<4, T, Q> const& v)
156 {
157 return vec<4, T, Q>(q * vec<3, T, Q>(v), v.w);
158 }
159
160 template<typename T, qualifier Q>
161 GLM_FUNC_QUALIFIER vec<4, T, Q> operator*(vec<4, T, Q> const& v, tdualquat<T, Q> const& q)
162 {
163 return glm::inverse(q) * v;
164 }
165
166 template<typename T, qualifier Q>
167 GLM_FUNC_QUALIFIER tdualquat<T, Q> operator*(tdualquat<T, Q> const& q, T const& s)
168 {
169 return tdualquat<T, Q>(q.real * s, q.dual * s);
170 }
171
172 template<typename T, qualifier Q>
173 GLM_FUNC_QUALIFIER tdualquat<T, Q> operator*(T const& s, tdualquat<T, Q> const& q)
174 {
175 return q * s;
176 }
177
178 template<typename T, qualifier Q>
179 GLM_FUNC_QUALIFIER tdualquat<T, Q> operator/(tdualquat<T, Q> const& q, T const& s)
180 {
181 return tdualquat<T, Q>(q.real / s, q.dual / s);
182 }
183
184 // -- Boolean operators --
185
186 template<typename T, qualifier Q>
187 GLM_FUNC_QUALIFIER bool operator==(tdualquat<T, Q> const& q1, tdualquat<T, Q> const& q2)
188 {
189 return (q1.real == q2.real) && (q1.dual == q2.dual);
190 }
191
192 template<typename T, qualifier Q>
193 GLM_FUNC_QUALIFIER bool operator!=(tdualquat<T, Q> const& q1, tdualquat<T, Q> const& q2)
194 {
195 return (q1.real != q2.real) || (q1.dual != q2.dual);
196 }
197
198 // -- Operations --
199
200 template<typename T, qualifier Q>
201 GLM_FUNC_QUALIFIER tdualquat<T, Q> dual_quat_identity()
202 {
203 return tdualquat<T, Q>(
204 qua<T, Q>::wxyz(static_cast<T>(1), static_cast<T>(0), static_cast<T>(0), static_cast<T>(0)),
205 qua<T, Q>::wxyz(static_cast<T>(0), static_cast<T>(0), static_cast<T>(0), static_cast<T>(0)));
206 }
207
208 template<typename T, qualifier Q>
209 GLM_FUNC_QUALIFIER tdualquat<T, Q> normalize(tdualquat<T, Q> const& q)
210 {
211 return q / length(q.real);
212 }
213
214 template<typename T, qualifier Q>
215 GLM_FUNC_QUALIFIER tdualquat<T, Q> lerp(tdualquat<T, Q> const& x, tdualquat<T, Q> const& y, T const& a)
216 {
217 // Dual Quaternion Linear blend aka DLB:
218 // Lerp is only defined in [0, 1]
219 assert(a >= static_cast<T>(0));
220 assert(a <= static_cast<T>(1));
221 T const k = dot(x.real,y.real) < static_cast<T>(0) ? -a : a;
222 T const one(1);
223 return tdualquat<T, Q>(x * (one - a) + y * k);
224 }
225
226 template<typename T, qualifier Q>
227 GLM_FUNC_QUALIFIER tdualquat<T, Q> inverse(tdualquat<T, Q> const& q)
228 {
229 const glm::qua<T, Q> real = conjugate(q.real);
230 const glm::qua<T, Q> dual = conjugate(q.dual);
231 return tdualquat<T, Q>(real, dual + (real * (-2.0f * dot(real,dual))));
232 }
233
234 template<typename T, qualifier Q>
235 GLM_FUNC_QUALIFIER mat<2, 4, T, Q> mat2x4_cast(tdualquat<T, Q> const& x)
236 {
237 return mat<2, 4, T, Q>( x[0].x, x[0].y, x[0].z, x[0].w, x[1].x, x[1].y, x[1].z, x[1].w );
238 }
239
240 template<typename T, qualifier Q>
241 GLM_FUNC_QUALIFIER mat<3, 4, T, Q> mat3x4_cast(tdualquat<T, Q> const& x)
242 {
243 qua<T, Q> r = x.real / length2(x.real);
244
245 qua<T, Q> const rr(r.w * x.real.w, r.x * x.real.x, r.y * x.real.y, r.z * x.real.z);
246 r *= static_cast<T>(2);
247
248 T const xy = r.x * x.real.y;
249 T const xz = r.x * x.real.z;
250 T const yz = r.y * x.real.z;
251 T const wx = r.w * x.real.x;
252 T const wy = r.w * x.real.y;
253 T const wz = r.w * x.real.z;
254
255 vec<4, T, Q> const a(
256 rr.w + rr.x - rr.y - rr.z,
257 xy - wz,
258 xz + wy,
259 -(x.dual.w * r.x - x.dual.x * r.w + x.dual.y * r.z - x.dual.z * r.y));
260
261 vec<4, T, Q> const b(
262 xy + wz,
263 rr.w + rr.y - rr.x - rr.z,
264 yz - wx,
265 -(x.dual.w * r.y - x.dual.x * r.z - x.dual.y * r.w + x.dual.z * r.x));
266
267 vec<4, T, Q> const c(
268 xz - wy,
269 yz + wx,
270 rr.w + rr.z - rr.x - rr.y,
271 -(x.dual.w * r.z + x.dual.x * r.y - x.dual.y * r.x - x.dual.z * r.w));
272
273 return mat<3, 4, T, Q>(a, b, c);
274 }
275
276 template<typename T, qualifier Q>
277 GLM_FUNC_QUALIFIER tdualquat<T, Q> dualquat_cast(mat<2, 4, T, Q> const& x)
278 {
279 return tdualquat<T, Q>(
280 qua<T, Q>::wxyz( x[0].w, x[0].x, x[0].y, x[0].z ),
281 qua<T, Q>::wxyz( x[1].w, x[1].x, x[1].y, x[1].z ));
282 }
283
284 template<typename T, qualifier Q>
285 GLM_FUNC_QUALIFIER tdualquat<T, Q> dualquat_cast(mat<3, 4, T, Q> const& x)
286 {
287 qua<T, Q> real;
288
289 T const trace = x[0].x + x[1].y + x[2].z;
290 if(trace > static_cast<T>(0))
291 {
292 T const r = sqrt(T(1) + trace);
293 T const invr = static_cast<T>(0.5) / r;
294 real.w = static_cast<T>(0.5) * r;
295 real.x = (x[2].y - x[1].z) * invr;
296 real.y = (x[0].z - x[2].x) * invr;
297 real.z = (x[1].x - x[0].y) * invr;
298 }
299 else if(x[0].x > x[1].y && x[0].x > x[2].z)
300 {
301 T const r = sqrt(T(1) + x[0].x - x[1].y - x[2].z);
302 T const invr = static_cast<T>(0.5) / r;
303 real.x = static_cast<T>(0.5)*r;
304 real.y = (x[1].x + x[0].y) * invr;
305 real.z = (x[0].z + x[2].x) * invr;
306 real.w = (x[2].y - x[1].z) * invr;
307 }
308 else if(x[1].y > x[2].z)
309 {
310 T const r = sqrt(T(1) + x[1].y - x[0].x - x[2].z);
311 T const invr = static_cast<T>(0.5) / r;
312 real.x = (x[1].x + x[0].y) * invr;
313 real.y = static_cast<T>(0.5) * r;
314 real.z = (x[2].y + x[1].z) * invr;
315 real.w = (x[0].z - x[2].x) * invr;
316 }
317 else
318 {
319 T const r = sqrt(T(1) + x[2].z - x[0].x - x[1].y);
320 T const invr = static_cast<T>(0.5) / r;
321 real.x = (x[0].z + x[2].x) * invr;
322 real.y = (x[2].y + x[1].z) * invr;
323 real.z = static_cast<T>(0.5) * r;
324 real.w = (x[1].x - x[0].y) * invr;
325 }
326
327 qua<T, Q> dual;
328 dual.x = static_cast<T>(0.5) * ( x[0].w * real.w + x[1].w * real.z - x[2].w * real.y);
329 dual.y = static_cast<T>(0.5) * (-x[0].w * real.z + x[1].w * real.w + x[2].w * real.x);
330 dual.z = static_cast<T>(0.5) * ( x[0].w * real.y - x[1].w * real.x + x[2].w * real.w);
331 dual.w = -static_cast<T>(0.5) * ( x[0].w * real.x + x[1].w * real.y + x[2].w * real.z);
332 return tdualquat<T, Q>(real, dual);
333 }
334}//namespace glm