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