]>
Commit | Line | Data |
---|---|---|
1e59de90 TL |
1 | #ifndef BOOST_QVM_QUAT_OPERATIONS |
2 | #define BOOST_QVM_QUAT_OPERATIONS | |
92f5a8d4 | 3 | |
1e59de90 | 4 | // Copyright 2008-2022 Emil Dotchevski and Reverge Studios, Inc. |
92f5a8d4 | 5 | |
1e59de90 TL |
6 | // Distributed under the Boost Software License, Version 1.0. (See accompanying |
7 | // file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) | |
92f5a8d4 TL |
8 | |
9 | #include <boost/qvm/detail/quat_assign.hpp> | |
10 | #include <boost/qvm/deduce_quat.hpp> | |
11 | #include <boost/qvm/mat_traits.hpp> | |
12 | #include <boost/qvm/scalar_traits.hpp> | |
13 | #include <boost/qvm/math.hpp> | |
14 | #include <boost/qvm/assert.hpp> | |
15 | #include <boost/qvm/error.hpp> | |
16 | #include <boost/qvm/throw_exception.hpp> | |
17 | #include <string> | |
f51cf556 | 18 | #include <cmath> |
92f5a8d4 | 19 | |
1e59de90 TL |
20 | namespace boost { namespace qvm { |
21 | ||
92f5a8d4 | 22 | namespace |
1e59de90 | 23 | qvm_detail |
92f5a8d4 | 24 | { |
1e59de90 TL |
25 | BOOST_QVM_INLINE_CRITICAL |
26 | void const * | |
27 | get_valid_ptr_quat_operations() | |
92f5a8d4 | 28 | { |
1e59de90 TL |
29 | static int const obj=0; |
30 | return &obj; | |
31 | } | |
32 | } | |
92f5a8d4 | 33 | |
1e59de90 | 34 | //////////////////////////////////////////////// |
92f5a8d4 | 35 | |
1e59de90 TL |
36 | namespace |
37 | msvc_parse_bug_workaround | |
38 | { | |
39 | template <class A,class B> | |
40 | struct | |
41 | quats | |
42 | { | |
43 | static bool const value=is_quat<A>::value && is_quat<B>::value; | |
44 | }; | |
45 | } | |
92f5a8d4 | 46 | |
1e59de90 TL |
47 | namespace |
48 | qvm_to_string_detail | |
49 | { | |
50 | template <class T> | |
51 | std::string to_string( T const & x ); | |
52 | } | |
92f5a8d4 | 53 | |
1e59de90 TL |
54 | template <class A> |
55 | inline | |
56 | typename enable_if_c< | |
57 | is_quat<A>::value, | |
58 | std::string>::type | |
59 | to_string( A const & a ) | |
60 | { | |
61 | using namespace qvm_to_string_detail; | |
62 | return '('+ | |
63 | to_string(quat_traits<A>::template read_element<0>(a))+','+ | |
64 | to_string(quat_traits<A>::template read_element<1>(a))+','+ | |
65 | to_string(quat_traits<A>::template read_element<2>(a))+','+ | |
66 | to_string(quat_traits<A>::template read_element<3>(a))+')'; | |
67 | } | |
92f5a8d4 | 68 | |
1e59de90 | 69 | //////////////////////////////////////////////// |
92f5a8d4 | 70 | |
1e59de90 TL |
71 | template <class A,class B,class Cmp> |
72 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
73 | typename enable_if_c< | |
74 | is_quat<A>::value && is_quat<B>::value, | |
75 | bool>::type | |
76 | cmp( A const & a, B const & b, Cmp f ) | |
77 | { | |
78 | typedef typename quat_traits<A>::scalar_type T; | |
79 | typedef typename quat_traits<B>::scalar_type U; | |
80 | T q1[4] = | |
81 | { | |
82 | quat_traits<A>::template read_element<0>(a), | |
83 | quat_traits<A>::template read_element<1>(a), | |
84 | quat_traits<A>::template read_element<2>(a), | |
85 | quat_traits<A>::template read_element<3>(a) | |
86 | }; | |
87 | U q2[4] = | |
88 | { | |
89 | quat_traits<B>::template read_element<0>(b), | |
90 | quat_traits<B>::template read_element<1>(b), | |
91 | quat_traits<B>::template read_element<2>(b), | |
92 | quat_traits<B>::template read_element<3>(b) | |
93 | }; | |
94 | int i=0; | |
95 | for( ; i!=4; ++i ) | |
96 | if( !f(q1[i],q2[i]) ) | |
97 | break; | |
98 | if( i==4 ) | |
99 | return true; | |
100 | for( i=0; i!=4; ++i ) | |
101 | if( !f(q1[i],-q2[i]) ) | |
102 | return false; | |
103 | return true; | |
104 | } | |
92f5a8d4 | 105 | |
1e59de90 | 106 | //////////////////////////////////////////////// |
92f5a8d4 | 107 | |
1e59de90 TL |
108 | template <class R,class A> |
109 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
110 | typename enable_if_c< | |
111 | is_quat<R>::value && is_quat<A>::value, | |
112 | R>::type | |
113 | convert_to( A const & a ) | |
114 | { | |
115 | R r; | |
f51cf556 TL |
116 | write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)); |
117 | write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)); | |
118 | write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)); | |
119 | write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)); | |
1e59de90 TL |
120 | return r; |
121 | } | |
92f5a8d4 | 122 | |
1e59de90 TL |
123 | template <class R,class A> |
124 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
125 | typename enable_if_c< | |
126 | is_quat<R>::value && is_mat<A>::value && | |
127 | mat_traits<A>::rows==3 && mat_traits<A>::cols==3, | |
128 | R>::type | |
129 | convert_to( A const & a ) | |
130 | { | |
131 | typedef typename mat_traits<A>::scalar_type T; | |
132 | T const mat[3][3] = | |
133 | { | |
134 | { mat_traits<A>::template read_element<0,0>(a), mat_traits<A>::template read_element<0,1>(a), mat_traits<A>::template read_element<0,2>(a) }, | |
135 | { mat_traits<A>::template read_element<1,0>(a), mat_traits<A>::template read_element<1,1>(a), mat_traits<A>::template read_element<1,2>(a) }, | |
136 | { mat_traits<A>::template read_element<2,0>(a), mat_traits<A>::template read_element<2,1>(a), mat_traits<A>::template read_element<2,2>(a) } | |
137 | }; | |
138 | R r; | |
139 | if( mat[0][0]+mat[1][1]+mat[2][2] > scalar_traits<T>::value(0) ) | |
140 | { | |
141 | T t = mat[0][0] + mat[1][1] + mat[2][2] + scalar_traits<T>::value(1); | |
142 | T s = (scalar_traits<T>::value(1)/sqrt(t))/2; | |
f51cf556 TL |
143 | write_quat_element<0>(r,s*t); |
144 | write_quat_element<1>(r,(mat[2][1]-mat[1][2])*s); | |
145 | write_quat_element<2>(r,(mat[0][2]-mat[2][0])*s); | |
146 | write_quat_element<3>(r,(mat[1][0]-mat[0][1])*s); | |
1e59de90 TL |
147 | } |
148 | else if( mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2] ) | |
149 | { | |
150 | T t = mat[0][0] - mat[1][1] - mat[2][2] + scalar_traits<T>::value(1); | |
151 | T s = (scalar_traits<T>::value(1)/sqrt(t))/2; | |
f51cf556 TL |
152 | write_quat_element<0>(r,(mat[2][1]-mat[1][2])*s); |
153 | write_quat_element<1>(r,s*t); | |
154 | write_quat_element<2>(r,(mat[1][0]+mat[0][1])*s); | |
155 | write_quat_element<3>(r,(mat[0][2]+mat[2][0])*s); | |
1e59de90 TL |
156 | } |
157 | else if( mat[1][1]>mat[2][2] ) | |
158 | { | |
159 | T t = - mat[0][0] + mat[1][1] - mat[2][2] + scalar_traits<T>::value(1); | |
160 | T s = (scalar_traits<T>::value(1)/sqrt(t))/2; | |
f51cf556 TL |
161 | write_quat_element<0>(r,(mat[0][2]-mat[2][0])*s); |
162 | write_quat_element<1>(r,(mat[1][0]+mat[0][1])*s); | |
163 | write_quat_element<2>(r,s*t); | |
164 | write_quat_element<3>(r,(mat[2][1]+mat[1][2])*s); | |
1e59de90 TL |
165 | } |
166 | else | |
167 | { | |
168 | T t = - mat[0][0] - mat[1][1] + mat[2][2] + scalar_traits<T>::value(1); | |
169 | T s = (scalar_traits<T>::value(1)/sqrt(t))/2; | |
f51cf556 TL |
170 | write_quat_element<0>(r,(mat[1][0]-mat[0][1])*s); |
171 | write_quat_element<1>(r,(mat[0][2]+mat[2][0])*s); | |
172 | write_quat_element<2>(r,(mat[2][1]+mat[1][2])*s); | |
173 | write_quat_element<3>(r,s*t); | |
1e59de90 TL |
174 | } |
175 | return r; | |
176 | } | |
92f5a8d4 | 177 | |
1e59de90 | 178 | //////////////////////////////////////////////// |
92f5a8d4 | 179 | |
1e59de90 TL |
180 | template <class A> |
181 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
182 | typename lazy_enable_if_c< | |
183 | is_quat<A>::value, | |
184 | deduce_quat<A> >::type | |
185 | conjugate( A const & a ) | |
186 | { | |
187 | typedef typename deduce_quat<A>::type R; | |
188 | R r; | |
f51cf556 TL |
189 | write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)); |
190 | write_quat_element<1>(r,-quat_traits<A>::template read_element<1>(a)); | |
191 | write_quat_element<2>(r,-quat_traits<A>::template read_element<2>(a)); | |
192 | write_quat_element<3>(r,-quat_traits<A>::template read_element<3>(a)); | |
1e59de90 TL |
193 | return r; |
194 | } | |
195 | ||
196 | //////////////////////////////////////////////// | |
197 | ||
198 | namespace | |
199 | qvm_detail | |
200 | { | |
201 | template <class T> | |
202 | class | |
203 | identity_quat_ | |
204 | { | |
205 | identity_quat_( identity_quat_ const & ); | |
206 | identity_quat_ & operator=( identity_quat_ const & ); | |
207 | ~identity_quat_(); | |
208 | ||
209 | public: | |
210 | ||
f51cf556 TL |
211 | template <class R |
212 | #if __cplusplus >= 201103L | |
213 | , class = typename enable_if<is_quat<R> >::type | |
214 | #endif | |
215 | > | |
1e59de90 TL |
216 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
217 | operator R() const | |
92f5a8d4 | 218 | { |
92f5a8d4 | 219 | R r; |
1e59de90 | 220 | assign(r,*this); |
92f5a8d4 TL |
221 | return r; |
222 | } | |
1e59de90 TL |
223 | }; |
224 | } | |
92f5a8d4 | 225 | |
1e59de90 TL |
226 | template <class T> |
227 | struct | |
228 | quat_traits< qvm_detail::identity_quat_<T> > | |
229 | { | |
230 | typedef qvm_detail::identity_quat_<T> this_quaternion; | |
231 | typedef T scalar_type; | |
232 | ||
233 | template <int I> | |
234 | static | |
235 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
236 | scalar_type | |
f51cf556 | 237 | read_element( this_quaternion const & ) |
1e59de90 TL |
238 | { |
239 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
240 | BOOST_QVM_STATIC_ASSERT(I<4); | |
241 | return scalar_traits<T>::value(I==0); | |
242 | } | |
92f5a8d4 | 243 | |
1e59de90 TL |
244 | static |
245 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
246 | scalar_type | |
f51cf556 | 247 | read_element_idx( int i, this_quaternion const & ) |
1e59de90 TL |
248 | { |
249 | BOOST_QVM_ASSERT(i>=0); | |
250 | BOOST_QVM_ASSERT(i<4); | |
251 | return scalar_traits<T>::value(i==0); | |
252 | } | |
253 | }; | |
92f5a8d4 | 254 | |
1e59de90 TL |
255 | template <class T> |
256 | struct | |
257 | deduce_quat< qvm_detail::identity_quat_<T> > | |
258 | { | |
259 | typedef quat<T> type; | |
260 | }; | |
92f5a8d4 | 261 | |
1e59de90 TL |
262 | template <class T> |
263 | struct | |
264 | deduce_quat2< qvm_detail::identity_quat_<T>, qvm_detail::identity_quat_<T> > | |
265 | { | |
266 | typedef quat<T> type; | |
267 | }; | |
92f5a8d4 | 268 | |
1e59de90 TL |
269 | template <class T> |
270 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
271 | qvm_detail::identity_quat_<T> const & | |
272 | identity_quat() | |
273 | { | |
274 | return *(qvm_detail::identity_quat_<T> const *)qvm_detail::get_valid_ptr_quat_operations(); | |
275 | } | |
276 | ||
277 | template <class A> | |
278 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
279 | typename enable_if_c< | |
280 | is_quat<A>::value, | |
281 | void>::type | |
282 | set_identity( A & a ) | |
283 | { | |
284 | typedef typename quat_traits<A>::scalar_type T; | |
285 | T const zero=scalar_traits<T>::value(0); | |
286 | T const one=scalar_traits<T>::value(1); | |
f51cf556 TL |
287 | write_quat_element<0>(a,one); |
288 | write_quat_element<1>(a,zero); | |
289 | write_quat_element<2>(a,zero); | |
290 | write_quat_element<3>(a,zero); | |
1e59de90 TL |
291 | } |
292 | ||
293 | //////////////////////////////////////////////// | |
294 | ||
295 | namespace | |
296 | qvm_detail | |
297 | { | |
298 | template <class OriginalType,class Scalar> | |
299 | class | |
300 | quaternion_scalar_cast_ | |
301 | { | |
302 | quaternion_scalar_cast_( quaternion_scalar_cast_ const & ); | |
303 | quaternion_scalar_cast_ & operator=( quaternion_scalar_cast_ const & ); | |
304 | ~quaternion_scalar_cast_(); | |
305 | ||
306 | public: | |
92f5a8d4 TL |
307 | |
308 | template <class T> | |
1e59de90 TL |
309 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
310 | quaternion_scalar_cast_ & | |
311 | operator=( T const & x ) | |
92f5a8d4 | 312 | { |
1e59de90 TL |
313 | assign(*this,x); |
314 | return *this; | |
92f5a8d4 TL |
315 | } |
316 | ||
f51cf556 TL |
317 | template <class R |
318 | #if __cplusplus >= 201103L | |
319 | , class = typename enable_if<is_quat<R> >::type | |
320 | #endif | |
321 | > | |
1e59de90 TL |
322 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
323 | operator R() const | |
92f5a8d4 | 324 | { |
1e59de90 TL |
325 | R r; |
326 | assign(r,*this); | |
327 | return r; | |
92f5a8d4 | 328 | } |
1e59de90 | 329 | }; |
92f5a8d4 | 330 | |
1e59de90 TL |
331 | template <bool> struct scalar_cast_quaternion_filter { }; |
332 | template <> struct scalar_cast_quaternion_filter<true> { typedef int type; }; | |
333 | } | |
92f5a8d4 | 334 | |
1e59de90 TL |
335 | template <class OriginalType,class Scalar> |
336 | struct | |
337 | quat_traits< qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> > | |
338 | { | |
339 | typedef Scalar scalar_type; | |
340 | typedef qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> this_quaternion; | |
341 | ||
342 | template <int I> | |
343 | static | |
344 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
345 | scalar_type | |
346 | read_element( this_quaternion const & x ) | |
347 | { | |
348 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
349 | BOOST_QVM_STATIC_ASSERT(I<4); | |
350 | return scalar_type(quat_traits<OriginalType>::template read_element<I>(reinterpret_cast<OriginalType const &>(x))); | |
351 | } | |
92f5a8d4 | 352 | |
1e59de90 TL |
353 | static |
354 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
355 | scalar_type | |
356 | read_element_idx( int i, this_quaternion const & x ) | |
357 | { | |
358 | BOOST_QVM_ASSERT(i>=0); | |
359 | BOOST_QVM_ASSERT(i<4); | |
360 | return scalar_type(quat_traits<OriginalType>::read_element_idx(i,reinterpret_cast<OriginalType const &>(x))); | |
361 | } | |
362 | }; | |
92f5a8d4 | 363 | |
1e59de90 TL |
364 | template <class Scalar,class T> |
365 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
366 | qvm_detail::quaternion_scalar_cast_<T,Scalar> const & | |
367 | scalar_cast( T const & x, typename qvm_detail::scalar_cast_quaternion_filter<is_quat<T>::value>::type=0 ) | |
368 | { | |
369 | return reinterpret_cast<qvm_detail::quaternion_scalar_cast_<T,Scalar> const &>(x); | |
370 | } | |
92f5a8d4 | 371 | |
1e59de90 | 372 | //////////////////////////////////////////////// |
92f5a8d4 | 373 | |
1e59de90 TL |
374 | template <class A,class B> |
375 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
376 | typename enable_if_c< | |
377 | is_quat<A>::value && is_scalar<B>::value, | |
378 | A &>::type | |
379 | operator/=( A & a, B b ) | |
380 | { | |
f51cf556 TL |
381 | write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)/b); |
382 | write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)/b); | |
383 | write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)/b); | |
384 | write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)/b); | |
1e59de90 TL |
385 | return a; |
386 | } | |
92f5a8d4 | 387 | |
1e59de90 TL |
388 | template <class A,class B> |
389 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
390 | typename lazy_enable_if_c< | |
391 | is_quat<A>::value && is_scalar<B>::value, | |
392 | deduce_quat2<A,B> >::type | |
393 | operator/( A const & a, B b ) | |
394 | { | |
395 | typedef typename deduce_quat2<A,B>::type R; | |
396 | R r; | |
f51cf556 TL |
397 | write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)/b); |
398 | write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)/b); | |
399 | write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)/b); | |
400 | write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)/b); | |
1e59de90 TL |
401 | return r; |
402 | } | |
92f5a8d4 | 403 | |
1e59de90 TL |
404 | template <class A,class B> |
405 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
406 | typename lazy_enable_if_c< | |
407 | is_quat<A>::value && is_quat<B>::value, | |
408 | deduce_scalar<typename quat_traits<A>::scalar_type,typename quat_traits<B>::scalar_type> >::type | |
409 | dot( A const & a, B const & b ) | |
410 | { | |
411 | typedef typename quat_traits<A>::scalar_type Ta; | |
412 | typedef typename quat_traits<B>::scalar_type Tb; | |
413 | typedef typename deduce_scalar<Ta,Tb>::type Tr; | |
414 | Ta const a0=quat_traits<A>::template read_element<0>(a); | |
415 | Ta const a1=quat_traits<A>::template read_element<1>(a); | |
416 | Ta const a2=quat_traits<A>::template read_element<2>(a); | |
417 | Ta const a3=quat_traits<A>::template read_element<3>(a); | |
418 | Tb const b0=quat_traits<B>::template read_element<0>(b); | |
419 | Tb const b1=quat_traits<B>::template read_element<1>(b); | |
420 | Tb const b2=quat_traits<B>::template read_element<2>(b); | |
421 | Tb const b3=quat_traits<B>::template read_element<3>(b); | |
422 | Tr const dp=a0*b0+a1*b1+a2*b2+a3*b3; | |
423 | return dp; | |
424 | } | |
92f5a8d4 | 425 | |
1e59de90 TL |
426 | template <class A,class B> |
427 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
428 | typename enable_if_c< | |
429 | is_quat<A>::value && is_quat<B>::value, | |
430 | bool>::type | |
431 | operator==( A const & a, B const & b ) | |
432 | { | |
433 | return | |
434 | quat_traits<A>::template read_element<0>(a)==quat_traits<B>::template read_element<0>(b) && | |
435 | quat_traits<A>::template read_element<1>(a)==quat_traits<B>::template read_element<1>(b) && | |
436 | quat_traits<A>::template read_element<2>(a)==quat_traits<B>::template read_element<2>(b) && | |
437 | quat_traits<A>::template read_element<3>(a)==quat_traits<B>::template read_element<3>(b); | |
438 | } | |
92f5a8d4 | 439 | |
1e59de90 TL |
440 | template <class A> |
441 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
442 | typename lazy_enable_if_c< | |
443 | is_quat<A>::value, | |
444 | deduce_quat<A> >::type | |
445 | inverse( A const & a ) | |
446 | { | |
447 | typedef typename deduce_quat<A>::type R; | |
448 | typedef typename quat_traits<A>::scalar_type TA; | |
449 | TA aa = quat_traits<A>::template read_element<0>(a); | |
450 | TA ab = quat_traits<A>::template read_element<1>(a); | |
451 | TA ac = quat_traits<A>::template read_element<2>(a); | |
452 | TA ad = quat_traits<A>::template read_element<3>(a); | |
453 | TA m2 = ab*ab + ac*ac + ad*ad + aa*aa; | |
454 | if( m2==scalar_traits<TA>::value(0) ) | |
455 | BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); | |
456 | TA rm=scalar_traits<TA>::value(1)/m2; | |
457 | R r; | |
f51cf556 TL |
458 | write_quat_element<0>(r,aa*rm); |
459 | write_quat_element<1>(r,-ab*rm); | |
460 | write_quat_element<2>(r,-ac*rm); | |
461 | write_quat_element<3>(r,-ad*rm); | |
1e59de90 TL |
462 | return r; |
463 | } | |
92f5a8d4 | 464 | |
1e59de90 TL |
465 | template <class A> |
466 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
467 | typename enable_if_c< | |
468 | is_quat<A>::value, | |
469 | typename quat_traits<A>::scalar_type>::type | |
470 | mag_sqr( A const & a ) | |
471 | { | |
472 | typedef typename quat_traits<A>::scalar_type T; | |
473 | T x=quat_traits<A>::template read_element<0>(a); | |
474 | T y=quat_traits<A>::template read_element<1>(a); | |
475 | T z=quat_traits<A>::template read_element<2>(a); | |
476 | T w=quat_traits<A>::template read_element<3>(a); | |
477 | return x*x+y*y+z*z+w*w; | |
478 | } | |
92f5a8d4 | 479 | |
1e59de90 TL |
480 | template <class A> |
481 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
482 | typename enable_if_c< | |
483 | is_quat<A>::value, | |
484 | typename quat_traits<A>::scalar_type>::type | |
485 | mag( A const & a ) | |
486 | { | |
487 | typedef typename quat_traits<A>::scalar_type T; | |
488 | T x=quat_traits<A>::template read_element<0>(a); | |
489 | T y=quat_traits<A>::template read_element<1>(a); | |
490 | T z=quat_traits<A>::template read_element<2>(a); | |
491 | T w=quat_traits<A>::template read_element<3>(a); | |
492 | return sqrt(x*x+y*y+z*z+w*w); | |
493 | } | |
92f5a8d4 | 494 | |
1e59de90 TL |
495 | template <class A,class B> |
496 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
497 | typename enable_if< | |
498 | msvc_parse_bug_workaround::quats<A,B>, | |
499 | A &>::type | |
500 | operator-=( A & a, B const & b ) | |
501 | { | |
f51cf556 TL |
502 | write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b)); |
503 | write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b)); | |
504 | write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b)); | |
505 | write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b)); | |
1e59de90 TL |
506 | return a; |
507 | } | |
92f5a8d4 | 508 | |
1e59de90 TL |
509 | template <class A,class B> |
510 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
511 | typename lazy_enable_if_c< | |
512 | is_quat<A>::value && is_quat<B>::value, | |
513 | deduce_quat2<A,B> >::type | |
514 | operator-( A const & a, B const & b ) | |
515 | { | |
516 | typedef typename deduce_quat2<A,B>::type R; | |
517 | R r; | |
f51cf556 TL |
518 | write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b)); |
519 | write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b)); | |
520 | write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b)); | |
521 | write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b)); | |
1e59de90 TL |
522 | return r; |
523 | } | |
92f5a8d4 | 524 | |
1e59de90 TL |
525 | template <class A> |
526 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
527 | typename lazy_enable_if_c< | |
528 | is_quat<A>::value, | |
529 | deduce_quat<A> >::type | |
530 | operator-( A const & a ) | |
531 | { | |
532 | typedef typename deduce_quat<A>::type R; | |
533 | R r; | |
f51cf556 TL |
534 | write_quat_element<0>(r,-quat_traits<A>::template read_element<0>(a)); |
535 | write_quat_element<1>(r,-quat_traits<A>::template read_element<1>(a)); | |
536 | write_quat_element<2>(r,-quat_traits<A>::template read_element<2>(a)); | |
537 | write_quat_element<3>(r,-quat_traits<A>::template read_element<3>(a)); | |
1e59de90 TL |
538 | return r; |
539 | } | |
92f5a8d4 | 540 | |
1e59de90 TL |
541 | template <class A,class B> |
542 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
543 | typename enable_if< | |
544 | msvc_parse_bug_workaround::quats<A,B>, | |
545 | A &>::type | |
546 | operator*=( A & a, B const & b ) | |
547 | { | |
548 | typedef typename quat_traits<A>::scalar_type TA; | |
549 | typedef typename quat_traits<B>::scalar_type TB; | |
550 | TA const aa=quat_traits<A>::template read_element<0>(a); | |
551 | TA const ab=quat_traits<A>::template read_element<1>(a); | |
552 | TA const ac=quat_traits<A>::template read_element<2>(a); | |
553 | TA const ad=quat_traits<A>::template read_element<3>(a); | |
554 | TB const ba=quat_traits<B>::template read_element<0>(b); | |
555 | TB const bb=quat_traits<B>::template read_element<1>(b); | |
556 | TB const bc=quat_traits<B>::template read_element<2>(b); | |
557 | TB const bd=quat_traits<B>::template read_element<3>(b); | |
f51cf556 TL |
558 | write_quat_element<0>(a,aa*ba - ab*bb - ac*bc - ad*bd); |
559 | write_quat_element<1>(a,aa*bb + ab*ba + ac*bd - ad*bc); | |
560 | write_quat_element<2>(a,aa*bc + ac*ba + ad*bb - ab*bd); | |
561 | write_quat_element<3>(a,aa*bd + ad*ba + ab*bc - ac*bb); | |
1e59de90 TL |
562 | return a; |
563 | } | |
564 | ||
565 | template <class A,class B> | |
566 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
567 | typename enable_if_c< | |
568 | is_quat<A>::value && is_scalar<B>::value, | |
569 | A &>::type | |
570 | operator*=( A & a, B b ) | |
571 | { | |
f51cf556 TL |
572 | write_quat_element<0>(a, quat_traits<A>::template read_element<0>(a)*b); |
573 | write_quat_element<1>(a, quat_traits<A>::template read_element<1>(a)*b); | |
574 | write_quat_element<2>(a, quat_traits<A>::template read_element<2>(a)*b); | |
575 | write_quat_element<3>(a, quat_traits<A>::template read_element<3>(a)*b); | |
1e59de90 TL |
576 | return a; |
577 | } | |
578 | ||
579 | template <class A,class B> | |
580 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
581 | typename lazy_enable_if_c< | |
582 | is_quat<A>::value && is_quat<B>::value, | |
583 | deduce_quat2<A,B> >::type | |
584 | operator*( A const & a, B const & b ) | |
585 | { | |
586 | typedef typename deduce_quat2<A,B>::type R; | |
587 | typedef typename quat_traits<A>::scalar_type TA; | |
588 | typedef typename quat_traits<B>::scalar_type TB; | |
589 | TA const aa=quat_traits<A>::template read_element<0>(a); | |
590 | TA const ab=quat_traits<A>::template read_element<1>(a); | |
591 | TA const ac=quat_traits<A>::template read_element<2>(a); | |
592 | TA const ad=quat_traits<A>::template read_element<3>(a); | |
593 | TB const ba=quat_traits<B>::template read_element<0>(b); | |
594 | TB const bb=quat_traits<B>::template read_element<1>(b); | |
595 | TB const bc=quat_traits<B>::template read_element<2>(b); | |
596 | TB const bd=quat_traits<B>::template read_element<3>(b); | |
597 | R r; | |
f51cf556 TL |
598 | write_quat_element<0>(r,aa*ba - ab*bb - ac*bc - ad*bd); |
599 | write_quat_element<1>(r,aa*bb + ab*ba + ac*bd - ad*bc); | |
600 | write_quat_element<2>(r,aa*bc + ac*ba + ad*bb - ab*bd); | |
601 | write_quat_element<3>(r,aa*bd + ad*ba + ab*bc - ac*bb); | |
1e59de90 TL |
602 | return r; |
603 | } | |
604 | ||
605 | template <class A,class B> | |
606 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
607 | typename lazy_enable_if_c2< | |
608 | is_quat<A>::value && is_scalar<B>::value, | |
609 | deduce_quat2<A,B> >::type | |
610 | operator*( A const & a, B b ) | |
611 | { | |
612 | typedef typename deduce_quat2<A,B>::type R; | |
613 | R r; | |
f51cf556 TL |
614 | write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)*b); |
615 | write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)*b); | |
616 | write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)*b); | |
617 | write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)*b); | |
1e59de90 TL |
618 | return r; |
619 | } | |
620 | ||
621 | template <class A,class B> | |
622 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
623 | typename enable_if_c< | |
624 | is_quat<A>::value && is_quat<B>::value, | |
625 | bool>::type | |
626 | operator!=( A const & a, B const & b ) | |
627 | { | |
628 | return | |
629 | quat_traits<A>::template read_element<0>(a)!=quat_traits<B>::template read_element<0>(b) || | |
630 | quat_traits<A>::template read_element<1>(a)!=quat_traits<B>::template read_element<1>(b) || | |
631 | quat_traits<A>::template read_element<2>(a)!=quat_traits<B>::template read_element<2>(b) || | |
632 | quat_traits<A>::template read_element<3>(a)!=quat_traits<B>::template read_element<3>(b); | |
633 | } | |
634 | ||
635 | template <class A> | |
636 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
637 | typename lazy_enable_if_c< | |
638 | is_quat<A>::value, | |
639 | deduce_quat<A> >::type | |
640 | normalized( A const & a ) | |
641 | { | |
642 | typedef typename quat_traits<A>::scalar_type T; | |
643 | T const a0=quat_traits<A>::template read_element<0>(a); | |
644 | T const a1=quat_traits<A>::template read_element<1>(a); | |
645 | T const a2=quat_traits<A>::template read_element<2>(a); | |
646 | T const a3=quat_traits<A>::template read_element<3>(a); | |
647 | T const m2=a0*a0+a1*a1+a2*a2+a3*a3; | |
648 | if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) ) | |
649 | BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); | |
650 | T const rm=scalar_traits<T>::value(1)/sqrt(m2); | |
651 | typedef typename deduce_quat<A>::type R; | |
652 | R r; | |
f51cf556 TL |
653 | write_quat_element<0>(r,a0*rm); |
654 | write_quat_element<1>(r,a1*rm); | |
655 | write_quat_element<2>(r,a2*rm); | |
656 | write_quat_element<3>(r,a3*rm); | |
1e59de90 TL |
657 | return r; |
658 | } | |
659 | ||
660 | template <class A> | |
661 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
662 | typename enable_if_c< | |
663 | is_quat<A>::value, | |
664 | void>::type | |
665 | normalize( A & a ) | |
666 | { | |
667 | typedef typename quat_traits<A>::scalar_type T; | |
668 | T const a0=quat_traits<A>::template read_element<0>(a); | |
669 | T const a1=quat_traits<A>::template read_element<1>(a); | |
670 | T const a2=quat_traits<A>::template read_element<2>(a); | |
671 | T const a3=quat_traits<A>::template read_element<3>(a); | |
672 | T const m2=a0*a0+a1*a1+a2*a2+a3*a3; | |
673 | if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) ) | |
674 | BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); | |
675 | T const rm=scalar_traits<T>::value(1)/sqrt(m2); | |
f51cf556 TL |
676 | write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)*rm); |
677 | write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)*rm); | |
678 | write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)*rm); | |
679 | write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)*rm); | |
1e59de90 TL |
680 | } |
681 | ||
682 | template <class A,class B> | |
683 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
684 | typename enable_if< | |
685 | msvc_parse_bug_workaround::quats<A,B>, | |
686 | A &>::type | |
687 | operator+=( A & a, B const & b ) | |
688 | { | |
f51cf556 TL |
689 | write_quat_element<0>(a,quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b)); |
690 | write_quat_element<1>(a,quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b)); | |
691 | write_quat_element<2>(a,quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b)); | |
692 | write_quat_element<3>(a,quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b)); | |
1e59de90 TL |
693 | return a; |
694 | } | |
695 | ||
696 | template <class A,class B> | |
697 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
698 | typename lazy_enable_if_c< | |
699 | is_quat<A>::value && is_quat<B>::value, | |
700 | deduce_quat2<A,B> >::type | |
701 | operator+( A const & a, B const & b ) | |
702 | { | |
703 | typedef typename deduce_quat2<A,B>::type R; | |
704 | R r; | |
f51cf556 TL |
705 | write_quat_element<0>(r,quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b)); |
706 | write_quat_element<1>(r,quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b)); | |
707 | write_quat_element<2>(r,quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b)); | |
708 | write_quat_element<3>(r,quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b)); | |
1e59de90 TL |
709 | return r; |
710 | } | |
711 | ||
712 | template <class A,class B,class C> | |
713 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
714 | typename lazy_enable_if_c< | |
715 | is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value, | |
716 | deduce_quat2<A,B> >::type | |
f51cf556 | 717 | slerp360( A const & a, B const & b, C t ) |
1e59de90 TL |
718 | { |
719 | typedef typename deduce_quat2<A,B>::type R; | |
720 | typedef typename quat_traits<R>::scalar_type TR; | |
721 | TR const one = scalar_traits<TR>::value(1); | |
f51cf556 TL |
722 | TR const threshold = one - one / scalar_traits<TR>::value(2000); //0.9995 |
723 | TR const dp = dot(a,b); | |
724 | TR const abs_dp = abs(dp); | |
725 | if( abs_dp > threshold ) | |
726 | return a*(one-t) + b*t; | |
727 | TR const th = acos(dp); | |
728 | TR const invsinth = one / sin(th); | |
729 | return a * (sin(th * (one-t)) * invsinth) + b * (sin(th * t) * invsinth); | |
730 | } | |
731 | ||
732 | template <class A,class B,class C> | |
733 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
734 | typename lazy_enable_if_c< | |
735 | is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value, | |
736 | deduce_quat2<A,B> >::type | |
737 | slerp180( A const & a, B const & b, C t ) | |
738 | { | |
739 | typedef typename deduce_quat2<A,B>::type R; | |
740 | typedef typename quat_traits<R>::scalar_type TR; | |
741 | TR const one = scalar_traits<TR>::value(1); | |
742 | TR const threshold = one - one / scalar_traits<TR>::value(2000); //0.9995 | |
743 | TR const dp = dot(a,b); | |
744 | TR const abs_dp = abs(dp); | |
745 | if( abs_dp > threshold ) | |
746 | return a*(one-t)*sign(dp) + b*t; | |
747 | TR const th = acos(abs_dp); | |
748 | TR const invsinth = one / sin(th); | |
749 | return a * (sin(th * (one-t)) * invsinth * sign(dp)) + b * (sin(th * t) * invsinth); | |
750 | } | |
751 | ||
752 | template <class A,class B,class C> | |
753 | BOOST_QVM_DEPRECATED("please use slerp180 or slerp360") | |
754 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
755 | typename lazy_enable_if_c< | |
756 | is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value, | |
757 | deduce_quat2<A,B> >::type | |
758 | slerp( A const & a, B const & b, C t ) | |
759 | { | |
760 | return slerp360(a, b, t); | |
1e59de90 TL |
761 | } |
762 | ||
763 | //////////////////////////////////////////////// | |
764 | ||
765 | namespace | |
766 | qvm_detail | |
767 | { | |
768 | template <class T> | |
769 | class | |
770 | qref_ | |
771 | { | |
772 | qref_( qref_ const & ); | |
773 | qref_ & operator=( qref_ const & ); | |
774 | ~qref_(); | |
92f5a8d4 | 775 | |
1e59de90 TL |
776 | public: |
777 | ||
778 | template <class R> | |
779 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
780 | qref_ & | |
781 | operator=( R const & x ) | |
92f5a8d4 | 782 | { |
1e59de90 TL |
783 | assign(*this,x); |
784 | return *this; | |
92f5a8d4 TL |
785 | } |
786 | ||
f51cf556 TL |
787 | template <class R |
788 | #if __cplusplus >= 201103L | |
789 | , class = typename enable_if<is_quat<R> >::type | |
790 | #endif | |
791 | > | |
1e59de90 TL |
792 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
793 | operator R() const | |
92f5a8d4 | 794 | { |
92f5a8d4 | 795 | R r; |
1e59de90 | 796 | assign(r,*this); |
92f5a8d4 TL |
797 | return r; |
798 | } | |
1e59de90 | 799 | }; |
f51cf556 TL |
800 | |
801 | template <class Q,bool WriteElementRef=quat_write_element_ref<Q>::value> | |
802 | struct qref_write_traits; | |
803 | ||
804 | template <class Q> | |
805 | struct | |
806 | qref_write_traits<Q,true> | |
807 | { | |
808 | typedef typename quat_traits<Q>::scalar_type scalar_type; | |
809 | typedef qvm_detail::qref_<Q> this_quaternion; | |
810 | ||
811 | template <int I> | |
812 | static | |
813 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
814 | scalar_type & | |
815 | write_element( this_quaternion & x ) | |
816 | { | |
817 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
818 | BOOST_QVM_STATIC_ASSERT(I<4); | |
819 | return quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x)); | |
820 | } | |
821 | }; | |
822 | ||
823 | template <class Q> | |
824 | struct | |
825 | qref_write_traits<Q,false> | |
826 | { | |
827 | typedef typename quat_traits<Q>::scalar_type scalar_type; | |
828 | typedef qvm_detail::qref_<Q> this_quaternion; | |
829 | ||
830 | template <int I> | |
831 | static | |
832 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
833 | void | |
834 | write_element( this_quaternion & x, scalar_type s ) | |
835 | { | |
836 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
837 | BOOST_QVM_STATIC_ASSERT(I<4); | |
838 | quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x), s); | |
839 | } | |
840 | }; | |
1e59de90 | 841 | } |
92f5a8d4 | 842 | |
1e59de90 TL |
843 | template <class Q> |
844 | struct quat_traits; | |
92f5a8d4 | 845 | |
1e59de90 TL |
846 | template <class Q> |
847 | struct | |
f51cf556 TL |
848 | quat_traits< qvm_detail::qref_<Q> >: |
849 | qvm_detail::qref_write_traits<Q> | |
1e59de90 TL |
850 | { |
851 | typedef typename quat_traits<Q>::scalar_type scalar_type; | |
852 | typedef qvm_detail::qref_<Q> this_quaternion; | |
853 | ||
854 | template <int I> | |
855 | static | |
856 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
857 | scalar_type | |
858 | read_element( this_quaternion const & x ) | |
859 | { | |
860 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
861 | BOOST_QVM_STATIC_ASSERT(I<4); | |
862 | return quat_traits<Q>::template read_element<I>(reinterpret_cast<Q const &>(x)); | |
863 | } | |
1e59de90 TL |
864 | }; |
865 | ||
866 | template <class Q> | |
867 | struct | |
868 | deduce_quat< qvm_detail::qref_<Q> > | |
869 | { | |
870 | typedef quat<typename quat_traits<Q>::scalar_type> type; | |
871 | }; | |
872 | ||
873 | template <class Q> | |
874 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
875 | typename enable_if_c< | |
876 | is_quat<Q>::value, | |
877 | qvm_detail::qref_<Q> const &>::type | |
878 | qref( Q const & a ) | |
879 | { | |
880 | return reinterpret_cast<qvm_detail::qref_<Q> const &>(a); | |
881 | } | |
882 | ||
883 | template <class Q> | |
884 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
885 | typename enable_if_c< | |
886 | is_quat<Q>::value, | |
887 | qvm_detail::qref_<Q> &>::type | |
888 | qref( Q & a ) | |
889 | { | |
890 | return reinterpret_cast<qvm_detail::qref_<Q> &>(a); | |
891 | } | |
892 | ||
893 | //////////////////////////////////////////////// | |
894 | ||
895 | namespace | |
896 | qvm_detail | |
897 | { | |
898 | template <class T> | |
899 | class | |
900 | zero_q_ | |
901 | { | |
902 | zero_q_( zero_q_ const & ); | |
903 | zero_q_ & operator=( zero_q_ const & ); | |
904 | ~zero_q_(); | |
905 | ||
906 | public: | |
907 | ||
f51cf556 TL |
908 | template <class R |
909 | #if __cplusplus >= 201103L | |
910 | , class = typename enable_if<is_quat<R> >::type | |
911 | #endif | |
912 | > | |
1e59de90 TL |
913 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
914 | operator R() const | |
92f5a8d4 | 915 | { |
92f5a8d4 | 916 | R r; |
1e59de90 | 917 | assign(r,*this); |
92f5a8d4 TL |
918 | return r; |
919 | } | |
1e59de90 TL |
920 | }; |
921 | } | |
92f5a8d4 | 922 | |
1e59de90 TL |
923 | template <class T> |
924 | struct | |
925 | quat_traits< qvm_detail::zero_q_<T> > | |
926 | { | |
927 | typedef qvm_detail::zero_q_<T> this_quaternion; | |
928 | typedef T scalar_type; | |
929 | ||
930 | template <int I> | |
931 | static | |
932 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
933 | scalar_type | |
f51cf556 | 934 | read_element( this_quaternion const & ) |
1e59de90 TL |
935 | { |
936 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
937 | BOOST_QVM_STATIC_ASSERT(I<4); | |
938 | return scalar_traits<scalar_type>::value(0); | |
939 | } | |
92f5a8d4 | 940 | |
1e59de90 TL |
941 | static |
942 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
943 | scalar_type | |
f51cf556 | 944 | read_element_idx( int i, this_quaternion const & ) |
1e59de90 TL |
945 | { |
946 | BOOST_QVM_ASSERT(i>=0); | |
947 | BOOST_QVM_ASSERT(i<4); | |
948 | return scalar_traits<scalar_type>::value(0); | |
949 | } | |
950 | }; | |
951 | ||
952 | template <class T> | |
953 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
954 | qvm_detail::zero_q_<T> const & | |
955 | zero_quat() | |
956 | { | |
957 | return *(qvm_detail::zero_q_<T> const *)qvm_detail::get_valid_ptr_quat_operations(); | |
958 | } | |
959 | ||
960 | template <class A> | |
961 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
962 | typename enable_if_c< | |
963 | is_quat<A>::value, | |
964 | void>::type | |
965 | set_zero( A & a ) | |
966 | { | |
967 | typedef typename quat_traits<A>::scalar_type T; | |
968 | T const zero=scalar_traits<T>::value(0); | |
f51cf556 TL |
969 | write_quat_element<0>(a,zero); |
970 | write_quat_element<1>(a,zero); | |
971 | write_quat_element<2>(a,zero); | |
972 | write_quat_element<3>(a,zero); | |
1e59de90 TL |
973 | } |
974 | ||
975 | //////////////////////////////////////////////// | |
976 | ||
977 | namespace | |
978 | qvm_detail | |
979 | { | |
980 | template <class V> | |
981 | struct | |
982 | rot_quat_ | |
983 | { | |
984 | typedef typename vec_traits<V>::scalar_type scalar_type; | |
985 | scalar_type a[4]; | |
986 | ||
987 | template <class Angle> | |
988 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE | |
989 | rot_quat_( V const & axis, Angle angle ) | |
990 | { | |
991 | scalar_type const x=vec_traits<V>::template read_element<0>(axis); | |
992 | scalar_type const y=vec_traits<V>::template read_element<1>(axis); | |
993 | scalar_type const z=vec_traits<V>::template read_element<2>(axis); | |
994 | scalar_type const m2=x*x+y*y+z*z; | |
995 | if( m2==scalar_traits<scalar_type>::value(0) ) | |
996 | BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); | |
997 | scalar_type const rm=scalar_traits<scalar_type>::value(1)/sqrt(m2); | |
998 | angle/=2; | |
999 | scalar_type const s=sin(angle); | |
1000 | a[0] = cos(angle); | |
1001 | a[1] = rm*x*s; | |
1002 | a[2] = rm*y*s; | |
1003 | a[3] = rm*z*s; | |
92f5a8d4 TL |
1004 | } |
1005 | ||
f51cf556 TL |
1006 | template <class R |
1007 | #if __cplusplus >= 201103L | |
1008 | , class = typename enable_if<is_quat<R> >::type | |
1009 | #endif | |
1010 | > | |
1e59de90 TL |
1011 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
1012 | operator R() const | |
92f5a8d4 | 1013 | { |
92f5a8d4 | 1014 | R r; |
1e59de90 | 1015 | assign(r,*this); |
92f5a8d4 TL |
1016 | return r; |
1017 | } | |
1e59de90 TL |
1018 | }; |
1019 | } | |
92f5a8d4 | 1020 | |
1e59de90 TL |
1021 | template <class V> |
1022 | struct | |
1023 | quat_traits< qvm_detail::rot_quat_<V> > | |
1024 | { | |
1025 | typedef qvm_detail::rot_quat_<V> this_quaternion; | |
1026 | typedef typename this_quaternion::scalar_type scalar_type; | |
1027 | ||
1028 | template <int I> | |
1029 | static | |
1030 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1031 | scalar_type | |
1032 | read_element( this_quaternion const & x ) | |
1033 | { | |
1034 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
1035 | BOOST_QVM_STATIC_ASSERT(I<4); | |
1036 | return x.a[I]; | |
1037 | } | |
1038 | }; | |
1039 | ||
1040 | template <class V> | |
1041 | struct | |
1042 | deduce_quat< qvm_detail::rot_quat_<V> > | |
1043 | { | |
1044 | typedef quat<typename vec_traits<V>::scalar_type> type; | |
1045 | }; | |
1046 | ||
1047 | template <class A,class Angle> | |
1048 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE | |
1049 | typename enable_if_c< | |
1050 | is_vec<A>::value && vec_traits<A>::dim==3, | |
1051 | qvm_detail::rot_quat_<A> >::type | |
1052 | rot_quat( A const & axis, Angle angle ) | |
1053 | { | |
1054 | return qvm_detail::rot_quat_<A>(axis,angle); | |
1055 | } | |
92f5a8d4 | 1056 | |
1e59de90 TL |
1057 | template <class A,class B,class Angle> |
1058 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1059 | typename enable_if_c< | |
1060 | is_quat<A>::value && | |
1061 | is_vec<B>::value && vec_traits<B>::dim==3, | |
1062 | void>::type | |
1063 | set_rot( A & a, B const & axis, Angle angle ) | |
1064 | { | |
1065 | assign(a,rot_quat(axis,angle)); | |
1066 | } | |
92f5a8d4 | 1067 | |
1e59de90 TL |
1068 | template <class A,class B,class Angle> |
1069 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1070 | typename enable_if_c< | |
1071 | is_quat<A>::value && | |
1072 | is_vec<B>::value && vec_traits<B>::dim==3, | |
1073 | void>::type | |
1074 | rotate( A & a, B const & axis, Angle angle ) | |
1075 | { | |
1076 | a *= rot_quat(axis,angle); | |
1077 | } | |
92f5a8d4 | 1078 | |
1e59de90 | 1079 | //////////////////////////////////////////////// |
92f5a8d4 | 1080 | |
1e59de90 TL |
1081 | namespace |
1082 | qvm_detail | |
1083 | { | |
1084 | template <class T> | |
1085 | struct | |
1086 | rotx_quat_ | |
1087 | { | |
1088 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
1089 | rotx_quat_() | |
92f5a8d4 | 1090 | { |
92f5a8d4 TL |
1091 | } |
1092 | ||
f51cf556 TL |
1093 | template <class R |
1094 | #if __cplusplus >= 201103L | |
1095 | , class = typename enable_if<is_quat<R> >::type | |
1096 | #endif | |
1097 | > | |
1e59de90 TL |
1098 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
1099 | operator R() const | |
92f5a8d4 | 1100 | { |
1e59de90 TL |
1101 | R r; |
1102 | assign(r,*this); | |
1103 | return r; | |
92f5a8d4 TL |
1104 | } |
1105 | ||
1e59de90 | 1106 | private: |
92f5a8d4 | 1107 | |
1e59de90 TL |
1108 | rotx_quat_( rotx_quat_ const & ); |
1109 | rotx_quat_ & operator=( rotx_quat_ const & ); | |
1110 | ~rotx_quat_(); | |
1111 | }; | |
92f5a8d4 | 1112 | |
1e59de90 TL |
1113 | template <int I> |
1114 | struct | |
1115 | rotx_q_get | |
1116 | { | |
92f5a8d4 | 1117 | template <class T> |
1e59de90 TL |
1118 | static |
1119 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1120 | T | |
1121 | get( T const & ) | |
92f5a8d4 | 1122 | { |
1e59de90 TL |
1123 | return scalar_traits<T>::value(0); |
1124 | } | |
1125 | }; | |
92f5a8d4 | 1126 | |
1e59de90 TL |
1127 | template <> |
1128 | struct | |
1129 | rotx_q_get<1> | |
1130 | { | |
92f5a8d4 | 1131 | template <class T> |
1e59de90 TL |
1132 | static |
1133 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1134 | T | |
1135 | get( T const & angle ) | |
92f5a8d4 | 1136 | { |
1e59de90 | 1137 | return sin(angle/2); |
92f5a8d4 | 1138 | } |
1e59de90 | 1139 | }; |
92f5a8d4 | 1140 | |
1e59de90 TL |
1141 | template <> |
1142 | struct | |
1143 | rotx_q_get<0> | |
1144 | { | |
1145 | template <class T> | |
1146 | static | |
1147 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1148 | T | |
1149 | get( T const & angle ) | |
92f5a8d4 | 1150 | { |
1e59de90 | 1151 | return cos(angle/2); |
92f5a8d4 | 1152 | } |
1e59de90 TL |
1153 | }; |
1154 | } | |
92f5a8d4 | 1155 | |
1e59de90 TL |
1156 | template <class Angle> |
1157 | struct | |
1158 | quat_traits< qvm_detail::rotx_quat_<Angle> > | |
1159 | { | |
1160 | typedef qvm_detail::rotx_quat_<Angle> this_quaternion; | |
1161 | typedef Angle scalar_type; | |
1162 | ||
1163 | template <int I> | |
1164 | static | |
1165 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1166 | scalar_type | |
1167 | read_element( this_quaternion const & x ) | |
1168 | { | |
1169 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
1170 | BOOST_QVM_STATIC_ASSERT(I<4); | |
1171 | return qvm_detail::rotx_q_get<I>::get(reinterpret_cast<Angle const &>(x)); | |
1172 | } | |
1173 | }; | |
92f5a8d4 | 1174 | |
1e59de90 TL |
1175 | template <class Angle> |
1176 | struct | |
1177 | deduce_quat< qvm_detail::rotx_quat_<Angle> > | |
1178 | { | |
1179 | typedef quat<Angle> type; | |
1180 | }; | |
92f5a8d4 | 1181 | |
1e59de90 TL |
1182 | template <class Angle> |
1183 | struct | |
1184 | deduce_quat2< qvm_detail::rotx_quat_<Angle>, qvm_detail::rotx_quat_<Angle> > | |
1185 | { | |
1186 | typedef quat<Angle> type; | |
1187 | }; | |
92f5a8d4 | 1188 | |
1e59de90 TL |
1189 | template <class Angle> |
1190 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
1191 | qvm_detail::rotx_quat_<Angle> const & | |
1192 | rotx_quat( Angle const & angle ) | |
1193 | { | |
1194 | return reinterpret_cast<qvm_detail::rotx_quat_<Angle> const &>(angle); | |
1195 | } | |
92f5a8d4 | 1196 | |
1e59de90 TL |
1197 | template <class A,class Angle> |
1198 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1199 | typename enable_if_c< | |
1200 | is_quat<A>::value, | |
1201 | void>::type | |
1202 | set_rotx( A & a, Angle angle ) | |
1203 | { | |
1204 | assign(a,rotx_quat(angle)); | |
1205 | } | |
1206 | ||
1207 | template <class A,class Angle> | |
1208 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1209 | typename enable_if_c< | |
1210 | is_quat<A>::value, | |
1211 | void>::type | |
1212 | rotate_x( A & a, Angle angle ) | |
1213 | { | |
1214 | a *= rotx_quat(angle); | |
1215 | } | |
92f5a8d4 | 1216 | |
1e59de90 | 1217 | //////////////////////////////////////////////// |
92f5a8d4 | 1218 | |
1e59de90 TL |
1219 | namespace |
1220 | qvm_detail | |
1221 | { | |
1222 | template <class T> | |
1223 | struct | |
1224 | roty_quat_ | |
1225 | { | |
1226 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
1227 | roty_quat_() | |
92f5a8d4 | 1228 | { |
92f5a8d4 TL |
1229 | } |
1230 | ||
f51cf556 TL |
1231 | template <class R |
1232 | #if __cplusplus >= 201103L | |
1233 | , class = typename enable_if<is_quat<R> >::type | |
1234 | #endif | |
1235 | > | |
1e59de90 TL |
1236 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
1237 | operator R() const | |
92f5a8d4 | 1238 | { |
1e59de90 TL |
1239 | R r; |
1240 | assign(r,*this); | |
1241 | return r; | |
1242 | } | |
92f5a8d4 | 1243 | |
1e59de90 | 1244 | private: |
92f5a8d4 | 1245 | |
1e59de90 TL |
1246 | roty_quat_( roty_quat_ const & ); |
1247 | roty_quat_ & operator=( roty_quat_ const & ); | |
1248 | ~roty_quat_(); | |
1249 | }; | |
92f5a8d4 | 1250 | |
1e59de90 TL |
1251 | template <int I> |
1252 | struct | |
1253 | roty_q_get | |
1254 | { | |
1255 | template <class T> | |
1256 | static | |
1257 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1258 | T | |
1259 | get( T const & ) | |
92f5a8d4 | 1260 | { |
1e59de90 | 1261 | return scalar_traits<T>::value(0); |
92f5a8d4 | 1262 | } |
1e59de90 | 1263 | }; |
92f5a8d4 | 1264 | |
1e59de90 TL |
1265 | template <> |
1266 | struct | |
1267 | roty_q_get<2> | |
1268 | { | |
1269 | template <class T> | |
1270 | static | |
1271 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1272 | T | |
1273 | get( T const & angle ) | |
92f5a8d4 | 1274 | { |
1e59de90 | 1275 | return sin(angle/2); |
92f5a8d4 | 1276 | } |
1e59de90 | 1277 | }; |
92f5a8d4 | 1278 | |
1e59de90 TL |
1279 | template <> |
1280 | struct | |
1281 | roty_q_get<0> | |
1282 | { | |
1283 | template <class T> | |
1284 | static | |
1285 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1286 | T | |
1287 | get( T const & angle ) | |
92f5a8d4 | 1288 | { |
1e59de90 | 1289 | return cos(angle/2); |
92f5a8d4 | 1290 | } |
1e59de90 TL |
1291 | }; |
1292 | } | |
92f5a8d4 | 1293 | |
1e59de90 TL |
1294 | template <class Angle> |
1295 | struct | |
1296 | quat_traits< qvm_detail::roty_quat_<Angle> > | |
1297 | { | |
1298 | typedef qvm_detail::roty_quat_<Angle> this_quaternion; | |
1299 | typedef Angle scalar_type; | |
1300 | ||
1301 | template <int I> | |
1302 | static | |
1303 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1304 | scalar_type | |
1305 | read_element( this_quaternion const & x ) | |
1306 | { | |
1307 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
1308 | BOOST_QVM_STATIC_ASSERT(I<4); | |
1309 | return qvm_detail::roty_q_get<I>::get(reinterpret_cast<Angle const &>(x)); | |
1310 | } | |
1311 | }; | |
92f5a8d4 | 1312 | |
1e59de90 TL |
1313 | template <class Angle> |
1314 | struct | |
1315 | deduce_quat< qvm_detail::roty_quat_<Angle> > | |
1316 | { | |
1317 | typedef quat<Angle> type; | |
1318 | }; | |
92f5a8d4 | 1319 | |
1e59de90 TL |
1320 | template <class Angle> |
1321 | struct | |
1322 | deduce_quat2< qvm_detail::roty_quat_<Angle>, qvm_detail::roty_quat_<Angle> > | |
1323 | { | |
1324 | typedef quat<Angle> type; | |
1325 | }; | |
92f5a8d4 | 1326 | |
1e59de90 TL |
1327 | template <class Angle> |
1328 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
1329 | qvm_detail::roty_quat_<Angle> const & | |
1330 | roty_quat( Angle const & angle ) | |
1331 | { | |
1332 | return reinterpret_cast<qvm_detail::roty_quat_<Angle> const &>(angle); | |
1333 | } | |
92f5a8d4 | 1334 | |
1e59de90 TL |
1335 | template <class A,class Angle> |
1336 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1337 | typename enable_if_c< | |
1338 | is_quat<A>::value, | |
1339 | void>::type | |
1340 | set_roty( A & a, Angle angle ) | |
1341 | { | |
1342 | assign(a,roty_quat(angle)); | |
1343 | } | |
92f5a8d4 | 1344 | |
1e59de90 TL |
1345 | template <class A,class Angle> |
1346 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1347 | typename enable_if_c< | |
1348 | is_quat<A>::value, | |
1349 | void>::type | |
1350 | rotate_y( A & a, Angle angle ) | |
1351 | { | |
1352 | a *= roty_quat(angle); | |
1353 | } | |
92f5a8d4 | 1354 | |
1e59de90 | 1355 | //////////////////////////////////////////////// |
92f5a8d4 | 1356 | |
1e59de90 TL |
1357 | namespace |
1358 | qvm_detail | |
1359 | { | |
1360 | template <class T> | |
1361 | struct | |
1362 | rotz_quat_ | |
1363 | { | |
1364 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
1365 | rotz_quat_() | |
92f5a8d4 | 1366 | { |
92f5a8d4 TL |
1367 | } |
1368 | ||
f51cf556 TL |
1369 | template <class R |
1370 | #if __cplusplus >= 201103L | |
1371 | , class = typename enable_if<is_quat<R> >::type | |
1372 | #endif | |
1373 | > | |
1e59de90 TL |
1374 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL |
1375 | operator R() const | |
92f5a8d4 | 1376 | { |
1e59de90 TL |
1377 | R r; |
1378 | assign(r,*this); | |
1379 | return r; | |
92f5a8d4 TL |
1380 | } |
1381 | ||
1e59de90 | 1382 | private: |
92f5a8d4 | 1383 | |
1e59de90 TL |
1384 | rotz_quat_( rotz_quat_ const & ); |
1385 | rotz_quat_ & operator=( rotz_quat_ const & ); | |
1386 | ~rotz_quat_(); | |
1387 | }; | |
92f5a8d4 | 1388 | |
1e59de90 TL |
1389 | template <int I> |
1390 | struct | |
1391 | rotz_q_get | |
1392 | { | |
1393 | template <class T> | |
1394 | static | |
1395 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1396 | T | |
1397 | get( T const & ) | |
92f5a8d4 | 1398 | { |
1e59de90 | 1399 | return scalar_traits<T>::value(0); |
92f5a8d4 | 1400 | } |
1e59de90 | 1401 | }; |
92f5a8d4 | 1402 | |
1e59de90 TL |
1403 | template <> |
1404 | struct | |
1405 | rotz_q_get<3> | |
1406 | { | |
1407 | template <class T> | |
1408 | static | |
1409 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1410 | T | |
1411 | get( T const & angle ) | |
92f5a8d4 | 1412 | { |
1e59de90 | 1413 | return sin(angle/2); |
92f5a8d4 | 1414 | } |
1e59de90 | 1415 | }; |
92f5a8d4 | 1416 | |
1e59de90 TL |
1417 | template <> |
1418 | struct | |
1419 | rotz_q_get<0> | |
1420 | { | |
1421 | template <class T> | |
1422 | static | |
1423 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1424 | T | |
1425 | get( T const & angle ) | |
92f5a8d4 | 1426 | { |
1e59de90 | 1427 | return cos(angle/2); |
92f5a8d4 | 1428 | } |
1e59de90 TL |
1429 | }; |
1430 | } | |
92f5a8d4 | 1431 | |
1e59de90 TL |
1432 | template <class Angle> |
1433 | struct | |
1434 | quat_traits< qvm_detail::rotz_quat_<Angle> > | |
1435 | { | |
1436 | typedef qvm_detail::rotz_quat_<Angle> this_quaternion; | |
1437 | typedef Angle scalar_type; | |
1438 | ||
1439 | template <int I> | |
1440 | static | |
1441 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_CRITICAL | |
1442 | scalar_type | |
1443 | read_element( this_quaternion const & x ) | |
1444 | { | |
1445 | BOOST_QVM_STATIC_ASSERT(I>=0); | |
1446 | BOOST_QVM_STATIC_ASSERT(I<4); | |
1447 | return qvm_detail::rotz_q_get<I>::get(reinterpret_cast<Angle const &>(x)); | |
1448 | } | |
1449 | }; | |
1450 | ||
1451 | template <class Angle> | |
1452 | struct | |
1453 | deduce_quat< qvm_detail::rotz_quat_<Angle> > | |
1454 | { | |
1455 | typedef quat<Angle> type; | |
1456 | }; | |
92f5a8d4 | 1457 | |
1e59de90 TL |
1458 | template <class Angle> |
1459 | struct | |
1460 | deduce_quat2< qvm_detail::rotz_quat_<Angle>, qvm_detail::rotz_quat_<Angle> > | |
1461 | { | |
1462 | typedef quat<Angle> type; | |
1463 | }; | |
92f5a8d4 | 1464 | |
1e59de90 TL |
1465 | template <class Angle> |
1466 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_TRIVIAL | |
1467 | qvm_detail::rotz_quat_<Angle> const & | |
1468 | rotz_quat( Angle const & angle ) | |
1469 | { | |
1470 | return reinterpret_cast<qvm_detail::rotz_quat_<Angle> const &>(angle); | |
1471 | } | |
1472 | ||
1473 | template <class A,class Angle> | |
1474 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1475 | typename enable_if_c< | |
1476 | is_quat<A>::value, | |
1477 | void>::type | |
1478 | set_rotz( A & a, Angle angle ) | |
1479 | { | |
1480 | assign(a,rotz_quat(angle)); | |
1481 | } | |
1482 | ||
1483 | template <class A,class Angle> | |
1484 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1485 | typename enable_if_c< | |
1486 | is_quat<A>::value, | |
1487 | void>::type | |
1488 | rotate_z( A & a, Angle angle ) | |
1489 | { | |
1490 | a *= rotz_quat(angle); | |
1491 | } | |
92f5a8d4 | 1492 | |
1e59de90 TL |
1493 | template <class A,class B> |
1494 | BOOST_QVM_CONSTEXPR BOOST_QVM_INLINE_OPERATIONS | |
1495 | typename enable_if_c< | |
1496 | is_quat<A>::value && is_vec<B>::value && vec_traits<B>::dim==3, | |
1497 | typename quat_traits<A>::scalar_type>::type | |
1498 | axis_angle( A const & a, B & b ) | |
1499 | { | |
1500 | typedef typename quat_traits<A>::scalar_type T; | |
1501 | T a0=quat_traits<A>::template read_element<0>(a); | |
1502 | T a1=quat_traits<A>::template read_element<1>(a); | |
1503 | T a2=quat_traits<A>::template read_element<2>(a); | |
1504 | T a3=quat_traits<A>::template read_element<3>(a); | |
1505 | if( a0>1 ) | |
1506 | { | |
1507 | T const m2=a0*a0+a1*a1+a2*a2+a3*a3; | |
1508 | if( m2==scalar_traits<T>::value(0) ) | |
1509 | BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error()); | |
1510 | T const s=sqrt(m2); | |
1511 | a0/=s; | |
1512 | a1/=s; | |
1513 | a2/=s; | |
1514 | a3/=s; | |
1515 | } | |
1516 | if( T s=sqrt(1-a0*a0) ) | |
1517 | { | |
f51cf556 TL |
1518 | write_vec_element<0>(b, a1/s); |
1519 | write_vec_element<1>(b, a2/s); | |
1520 | write_vec_element<2>(b, a3/s); | |
1e59de90 TL |
1521 | } |
1522 | else | |
1523 | { | |
1524 | typedef typename vec_traits<B>::scalar_type U; | |
f51cf556 TL |
1525 | write_vec_element<0>(b, scalar_traits<U>::value(1)); |
1526 | write_vec_element<1>(b, scalar_traits<U>::value(0)); | |
1527 | write_vec_element<2>(b, scalar_traits<U>::value(0)); | |
92f5a8d4 | 1528 | } |
1e59de90 | 1529 | return scalar_traits<T>::value(2) * qvm::acos(a0); |
92f5a8d4 TL |
1530 | } |
1531 | ||
1e59de90 TL |
1532 | //////////////////////////////////////////////// |
1533 | ||
1534 | namespace | |
1535 | sfinae | |
1536 | { | |
1537 | using ::boost::qvm::assign; | |
1538 | using ::boost::qvm::cmp; | |
1539 | using ::boost::qvm::convert_to; | |
1540 | using ::boost::qvm::conjugate; | |
1541 | using ::boost::qvm::set_identity; | |
1542 | using ::boost::qvm::set_zero; | |
1543 | using ::boost::qvm::scalar_cast; | |
1544 | using ::boost::qvm::operator/=; | |
1545 | using ::boost::qvm::operator/; | |
1546 | using ::boost::qvm::dot; | |
1547 | using ::boost::qvm::operator==; | |
1548 | using ::boost::qvm::inverse; | |
1549 | using ::boost::qvm::mag_sqr; | |
1550 | using ::boost::qvm::mag; | |
f51cf556 TL |
1551 | using ::boost::qvm::slerp360; |
1552 | using ::boost::qvm::slerp180; | |
1e59de90 TL |
1553 | using ::boost::qvm::slerp; |
1554 | using ::boost::qvm::operator-=; | |
1555 | using ::boost::qvm::operator-; | |
1556 | using ::boost::qvm::operator*=; | |
1557 | using ::boost::qvm::operator*; | |
1558 | using ::boost::qvm::operator!=; | |
1559 | using ::boost::qvm::normalized; | |
1560 | using ::boost::qvm::normalize; | |
1561 | using ::boost::qvm::operator+=; | |
1562 | using ::boost::qvm::operator+; | |
1563 | using ::boost::qvm::qref; | |
1564 | using ::boost::qvm::rot_quat; | |
1565 | using ::boost::qvm::set_rot; | |
1566 | using ::boost::qvm::rotate; | |
1567 | using ::boost::qvm::rotx_quat; | |
1568 | using ::boost::qvm::set_rotx; | |
1569 | using ::boost::qvm::rotate_x; | |
1570 | using ::boost::qvm::roty_quat; | |
1571 | using ::boost::qvm::set_roty; | |
1572 | using ::boost::qvm::rotate_y; | |
1573 | using ::boost::qvm::rotz_quat; | |
1574 | using ::boost::qvm::set_rotz; | |
1575 | using ::boost::qvm::rotate_z; | |
1576 | } | |
1577 | ||
1578 | } } | |
1579 | ||
92f5a8d4 | 1580 | #endif |