Debian Codegraph @

src/python/PyImath/PyImathQuat.cpp

raw
1 //
2 // SPDX-License-Identifier: BSD-3-Clause
3 // Copyright Contributors to the OpenEXR Project.
4 //
5
6 // clang-format off
7
8 #include <Python.h>
9 #define BOOST_BIND_GLOBAL_PLACEHOLDERS
10 #include <boost/python.hpp>
11 #include <boost/python/make_constructor.hpp>
12 #include <boost/format.hpp>
13 #include <ImathVec.h>
14 #include <ImathMatrixAlgo.h>
15 #include <ImathEuler.h>
16 #include "PyImathQuat.h"
17 #include "PyImathExport.h"
18 #include "PyImathDecorators.h"
19 #include "PyImath.h"
20 #include "PyImathMathExc.h"
21 #include "PyImathOperators.h"
22 #include "PyImathQuatOperators.h"
23
24 // XXX incomplete array wrapping, docstrings missing
25
26 namespace PyImath {
27 template <> const char *PyImath::QuatfArray::name() { return "QuatfArray"; }
28 template <> const char *PyImath::QuatdArray::name() { return "QuatdArray"; }
29 }
30
31 namespace PyImath {
32 using namespace boost::python;
33 using namespace IMATH_NAMESPACE;
34
35 template <class T> struct QuatName { static const char *value; };
36 template<> const char *QuatName<float>::value = "Quatf";
37 template<> const char *QuatName<double>::value = "Quatd";
38
39 template <class T>
40 static std::string Quat_str(const Quat<T> &v)
41 {
42 std::stringstream stream;
43 stream << QuatName<T>::value << "(" << v[0] << ", " << v[1] << ", "
44 << v[2] << ", " << v[3] << ")";
45 return stream.str();
46 }
47
48 // Non-specialized repr is same as str
49 template <class T>
50 static std::string Quat_repr(const Quat<T> &v)
51 {
52 return Quat_str(v);
53 }
54
55 // Specialization for float to full precision
56 template <>
57 std::string Quat_repr(const Quat<float> &v)
58 {
59 return (boost::format("%s(%.9g, %.9g, %.9g, %.9g)")
60 % QuatName<float>::value
61 % v[0] % v[1] % v[2] % v[3]).str();
62 }
63
64 // Specialization for double to full precision
65 template <>
66 std::string Quat_repr(const Quat<double> &v)
67 {
68 return (boost::format("%s(%.17g, %.17g, %.17g, %.17g)")
69 % QuatName<double>::value
70 % v[0] % v[1] % v[2] % v[3]).str();
71 }
72
73
74 template <class T>
75 static Quat<T> &
76 invert(Quat<T> &quat)
77 {
78 MATH_EXC_ON;
79 return quat.invert();
80 }
81
82 template <class T>
83 static Quat<T>
84 inverse(Quat<T> &quat)
85 {
86 MATH_EXC_ON;
87 return quat.inverse();
88 }
89
90 template <class T>
91 static Quat<T> &
92 normalize(Quat<T> &quat)
93 {
94 MATH_EXC_ON;
95 return quat.normalize();
96 }
97
98 template <class T>
99 static Quat<T>
100 normalized(Quat<T> &quat)
101 {
102 MATH_EXC_ON;
103 return quat.normalized();
104 }
105
106 template <class T>
107 static T
108 length (Quat<T> &quat)
109 {
110 MATH_EXC_ON;
111 return quat.length();
112 }
113
114 template <class T>
115 static Vec3<T>
116 rotateVector(const Quat<T> &quat, const Vec3<T> &original)
117 {
118 MATH_EXC_ON;
119 return quat.rotateVector(original);
120 }
121
122 template <class T>
123 static Quat<T> &
124 setAxisAngle(Quat<T> &quat, const Vec3<T> &axis, T radians)
125 {
126 MATH_EXC_ON;
127 return quat.setAxisAngle(axis, radians);
128 }
129
130 template <class T>
131 static Quat<T> &
132 setRotation(Quat<T> &quat, const Vec3<T> &from, const Vec3<T> &to)
133 {
134 MATH_EXC_ON;
135 return quat.setRotation(from, to);
136 }
137
138 template <class T>
139 static T
140 angle (Quat<T> &quat)
141 {
142 MATH_EXC_ON;
143 return quat.angle();
144 }
145
146 template <class T>
147 static Vec3<T>
148 axis (Quat<T> &quat)
149 {
150 MATH_EXC_ON;
151 return quat.axis();
152 }
153
154 template <class T>
155 static Matrix33<T>
156 toMatrix33 (Quat<T> &quat)
157 {
158 MATH_EXC_ON;
159 return quat.toMatrix33();
160 }
161
162 template <class T>
163 static Matrix44<T>
164 toMatrix44 (Quat<T> &quat)
165 {
166 MATH_EXC_ON;
167 return quat.toMatrix44();
168 }
169
170 template <class T>
171 static Quat<T>
172 log(Quat<T> &quat)
173 {
174 MATH_EXC_ON;
175 return quat.log();
176 }
177
178 template <class T>
179 static Quat<T>
180 exp(Quat<T> &quat)
181 {
182 MATH_EXC_ON;
183 return quat.exp();
184 }
185
186 template <class T>
187 static void
188 setR(Quat<T> &quat, const double &r)
189 {
190 quat.r = r;
191 }
192
193 template <class T>
194 static void
195 setV(Quat<T> &quat, const Vec3<T> &v)
196 {
197 quat.v = v;
198 }
199
200 template <class T>
201 static void
202 extract(Quat<T> &quat, const Matrix44<T> &mat)
203 {
204 MATH_EXC_ON;
205 Quat<T> q = IMATH_NAMESPACE::extractQuat(mat);
206 quat.r = q.r;
207 quat.v = q.v;
208 }
209
210 template <class T>
211 static T scalar(Quat<T> &quat)
212 {
213 return quat.r;
214 }
215
216 template <class T>
217 static Vec3<T> vector(Quat<T> &quat)
218 {
219 return quat.v;
220 }
221
222 template <class T>
223 static Quat<T>
224 slerp(const Quat<T> &quat, const Quat<T> &other, T t)
225 {
226 MATH_EXC_ON;
227 return IMATH_NAMESPACE::slerp (quat, other, t);
228 }
229
230 template <class T>
231 static Quat<T>
232 slerpShortestArc(const Quat<T>& quat, const Quat<T>& other, T t)
233 {
234 MATH_EXC_ON;
235 return IMATH_NAMESPACE::slerpShortestArc (quat, other, t);
236 }
237
238 template <class T>
239 static const Quat<T> &
240 imul (Quat<T> &quat, const Quat<T> &other)
241 {
242 MATH_EXC_ON;
243 return quat *= other;
244 }
245
246 template <class T>
247 static const Quat<T> &
248 imulT (Quat<T> &quat, T t)
249 {
250 MATH_EXC_ON;
251 return quat *= t;
252 }
253
254 template <class T>
255 static const Quat<T> &
256 idiv (Quat<T> &quat, const Quat<T> &other)
257 {
258 MATH_EXC_ON;
259 return quat /= other;
260 }
261
262 template <class T>
263 static const Quat<T> &
264 idivT (Quat<T> &quat, T t)
265 {
266 MATH_EXC_ON;
267 return quat /= t;
268 }
269
270 template <class T>
271 static const Quat<T> &
272 iadd (Quat<T> &quat, const Quat<T> &other)
273 {
274 MATH_EXC_ON;
275 return quat += other;
276 }
277
278 template <class T>
279 static const Quat<T> &
280 isub (Quat<T> &quat, const Quat<T> &other)
281 {
282 MATH_EXC_ON;
283 return quat -= other;
284 }
285
286 template <class T>
287 static Matrix33<T>
288 rmulM33(Quat<T> &quat, Matrix33<T> &m)
289 {
290 MATH_EXC_ON;
291 return m * quat;
292 }
293
294 template <class T>
295 static Matrix33<T>
296 mulM33(Quat<T> &quat, Matrix33<T> &m)
297 {
298 MATH_EXC_ON;
299 return quat * m;
300 }
301
302 template <class T>
303 static Quat<T>
304 mul(Quat<T> &quat, Quat<T> &other)
305 {
306 MATH_EXC_ON;
307 return quat * other;
308 }
309
310 template <class T>
311 static Quat<T>
312 div(Quat<T> &quat, Quat<T> &other)
313 {
314 MATH_EXC_ON;
315 return quat / other;
316 }
317
318 template <class T>
319 static Quat<T>
320 divT(Quat<T> &quat, T t)
321 {
322 MATH_EXC_ON;
323 return quat / t;
324 }
325
326 template <class T>
327 static Quat<T>
328 mulT(Quat<T> &quat, T t)
329 {
330 MATH_EXC_ON;
331 return quat * t;
332 }
333
334 template <class T>
335 static Quat<T>
336 add(Quat<T> &quat, Quat<T> &other)
337 {
338 MATH_EXC_ON;
339 return quat + other;
340 }
341
342 template <class T>
343 static Quat<T>
344 sub(Quat<T> &quat, Quat<T> &other)
345 {
346 MATH_EXC_ON;
347 return quat - other;
348 }
349
350 template <class T>
351 static Quat<T>
352 neg(Quat<T> &quat)
353 {
354 MATH_EXC_ON;
355 return -quat;
356 }
357
358 template <class T>
359 static Quat<T>
360 conj(Quat<T> &quat)
361 {
362 MATH_EXC_ON;
363 return ~quat;
364 }
365
366 template <class T>
367 static T
368 dot(Quat<T> &quat, Quat<T> &other)
369 {
370 MATH_EXC_ON;
371 return quat ^ other;
372 }
373
374 template <class T>
375 static Vec3<T>
376 rmulVec3(Quat<T> &quat, const Vec3<T> &v)
377 {
378 MATH_EXC_ON;
379 return v * quat.toMatrix44();
380 }
381
382 template <class T>
383 static FixedArray< Vec3<T> >
384 rmulVec3Array(Quat<T> &quat, const FixedArray< Vec3<T> > &a)
385 {
386 MATH_EXC_ON;
387 Matrix44<T> m = quat.toMatrix44();
388 size_t len = a.len();
389 FixedArray< Vec3<T> > r(len);
390 for (size_t i = 0; i < len; i++)
391 r[i] = a[i] * m;
392 return r;
393 }
394
395 template <class T>
396 static Quat<T> *
397 quatConstructor1(const Euler<T> &euler)
398 {
399 MATH_EXC_ON;
400 return new Quat<T>(euler.toQuat());
401 }
402
403 template <class T>
404 static Quat<T> *
405 quatConstructor2(const Matrix33<T> &mat)
406 {
407 MATH_EXC_ON;
408 return new Quat<T>(Euler<T>(mat).toQuat());
409 }
410
411 template <class T>
412 static Quat<T> *
413 quatConstructor3(const Matrix44<T> &mat)
414 {
415 MATH_EXC_ON;
416 return new Quat<T>(Euler<T>(mat).toQuat());
417 }
418
419 template <class T>
420 class_<Quat<T> >
421 register_Quat()
422 {
423 class_<Quat<T> > quat_class(QuatName<T>::value, QuatName<T>::value,init<Quat<T> >("copy construction"));
424 quat_class
425 .def(init<>("imath Quat initialization") )
426 .def(init<Quat<float> >("imath Quat copy initialization") )
427 .def(init<Quat<double> >("imath Quat copy initialization") )
428 .def(init<T,T,T,T>("make Quat from components") )
429 .def(init<T, Vec3<T> >("make Quat from components") )
430 .def("__init__", make_constructor(quatConstructor1<T>))
431 .def("__init__", make_constructor(quatConstructor2<T>))
432 .def("__init__", make_constructor(quatConstructor3<T>))
433 .def("identity",&Quat<T>::identity)
434 .def("invert",&invert<T>,return_internal_reference<>(),
435 "q.invert() -- inverts quaternion q\n"
436 "(modifying q); returns q")
437
438 .def("inverse",&inverse<T>,
439 "q.inverse() -- returns the inverse of\n"
440 "quaternion q; q is not modified\n")
441
442 .def("normalize",&normalize<T>,return_internal_reference<>(),
443 "q.normalize() -- normalizes quaternion q\n"
444 "(modifying q); returns q")
445
446 .def("normalized",&normalized<T>,
447 "q.normalized() -- returns a normalized version\n"
448 "of quaternion q; q is not modified\n")
449
450 .def("length",&length<T>)
451
452 .def("rotateVector",&rotateVector<T>,
453 "q.rotateVector(orig) -- Given a vector orig,\n"
454 " calculate orig' = q x orig x q*\n\n"
455 " Assumes unit quaternions")
456
457 .def("setAxisAngle",&setAxisAngle<T>,return_internal_reference<>(),
458 "q.setAxisAngle(x,r) -- sets the value of\n"
459 "quaternion q so that q represents a rotation\n"
460 "of r radians around axis x")
461
462 .def("setRotation",&setRotation<T>,return_internal_reference<>(),
463 "q.setRotation(v,w) -- sets the value of\n"
464 "quaternion q so that rotating vector v by\n"
465 "q produces vector w")
466
467 .def("angle",&angle<T>,
468 "q.angle() -- returns the rotation angle\n"
469 "(in radians) represented by quaternion q")
470
471 .def("axis",&axis<T>,
472 "q.axis() -- returns the rotation axis\n"
473 "represented by quaternion q")
474
475 .def("toMatrix33",&toMatrix33<T>,
476 "q.toMatrix33() -- returns a 3x3 matrix that\n"
477 "represents the same rotation as quaternion q")
478
479 .def("toMatrix44",&toMatrix44<T>,
480 "q.toMatrix44() -- returns a 4x4 matrix that\n"
481 "represents the same rotation as quaternion q")
482
483 .def("log",&log<T>)
484 .def("exp",&exp<T>)
485 .def_readwrite("v",&Quat<T>::v)
486 .def_readwrite("r",&Quat<T>::r)
487 .def("v", &vector<T>,
488 "q.v() -- returns the v (vector) component\n"
489 "of quaternion q")
490
491 .def("r", &scalar<T>,
492 "q.r() -- returns the r (scalar) component\n"
493 "of quaternion q")
494
495 .def("setR", &setR<T>,
496 "q.setR(s) -- sets the r (scalar) component\n"
497 "of quaternion q to s")
498
499 .def("setV", &setV<T>,
500 "q.setV(w) -- sets the v (vector) component\n"
501 "of quaternion q to w")
502
503 .def("extract", &extract<T>,
504 "q.extract(m) -- extracts the rotation component\n"
505 "from 4x4 matrix m and stores the result in q")
506
507 .def("slerp", &slerp<T>,
508 "q.slerp(p,t) -- performs sperical linear\n"
509 "interpolation between quaternions q and p:\n"
510 "q.slerp(p,0) returns q; q.slerp(p,1) returns p.\n"
511 "q and p must be normalized\n")
512
513 .def("slerpShortestArc", &slerpShortestArc<T>,
514 "q.slerpShortestArc(p,t) -- performs spherical linear\n"
515 "interpolation along the shortest arc between\n"
516 "quaternions q and either p or -p, whichever is\n"
517 "closer. q and p must be normalized\n")
518
519 .def("__str__",Quat_str<T>)
520 .def("__repr__",Quat_repr<T>)
521 .def ("__imul__", &imul<T>, return_internal_reference<>())
522 .def ("__imul__", &imulT<T>, return_internal_reference<>())
523 .def ("__idiv__", idiv<T>, return_internal_reference<>())
524 .def ("__idiv__", &idivT<T>, return_internal_reference<>())
525 .def ("__itruediv__", idiv<T>, return_internal_reference<>())
526 .def ("__itruediv__", &idivT<T>, return_internal_reference<>())
527 .def ("__iadd__", &iadd<T>, return_internal_reference<>())
528 .def ("__isub__", &isub<T>, return_internal_reference<>())
529 .def(self == self) // NOSONAR - suppress SonarCloud bug report.
530 .def(self != self) // NOSONAR - suppress SonarCloud bug report.
531 .def ("__rmul__", &rmulM33<T>)
532 .def ("__mul__", &mulM33<T>)
533 .def ("__mul__", &mul<T>)
534 .def ("__div__", &div<T>)
535 .def ("__div__", &divT<T>)
536 .def ("__truediv__", &div<T>)
537 .def ("__truediv__", &divT<T>)
538 .def ("__mul__", &mulT<T>)
539 .def ("__rmul__", &mulT<T>)
540 .def ("__add__", &add<T>)
541 .def ("__sub__", &sub<T>)
542 .def ("__neg__", &neg<T>)
543 .def ("__invert__", &conj<T>)
544 .def ("__xor__", &dot<T>)
545 .def ("__rmul__", &rmulVec3<T>)
546 .def ("__rmul__", &rmulVec3Array<T>)
547 ;
548
549 decoratecopy(quat_class);
550
551 return quat_class;
552 }
553
554 // XXX fixme - template this
555 // really this should get generated automatically...
556
557 template <class T,int index>
558 static FixedArray<T>
559 QuatArray_get(FixedArray<IMATH_NAMESPACE::Quat<T> > &qa)
560 {
561 return FixedArray<T>(&(qa.unchecked_index(0).r) + index,
562 qa.len(), 4*qa.stride(), qa.handle(), qa.writable());
563 }
564
565 template <class T>
566 struct QuatArray_SetRotationTask : public Task
567 {
568 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &from;
569 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &to;
570 FixedArray<IMATH_NAMESPACE::Quat<T> > &result;
571
572 QuatArray_SetRotationTask (const FixedArray<IMATH_NAMESPACE::Vec3<T> > &fromIn,
573 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &toIn,
574 FixedArray<IMATH_NAMESPACE::Quat<T> > &resultIn)
575 : from (fromIn), to (toIn), result (resultIn) {}
576
577 void execute (size_t start, size_t end)
578 {
579 for (size_t i = start; i < end; ++i)
580 result[i].setRotation (from[i], to[i]);
581 }
582 };
583
584 template <class T> static void
585 QuatArray_setRotation (FixedArray<IMATH_NAMESPACE::Quat<T> > &va,
586 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &from,
587 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &to)
588 {
589 MATH_EXC_ON;
590 size_t len = va.match_dimension(from);
591 va.match_dimension(to);
592
593 // Validate that 'va' is writable before entering the thread-task.
594 if (!va.writable())
595 throw std::invalid_argument ("Input fixed array is read-only.");
596
597 QuatArray_SetRotationTask<T> task (from, to, va);
598 dispatchTask (task, len);
599 }
600
601 template <class T>
602 struct QuatArray_OrientToVectors : public Task
603 {
604 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &forward;
605 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &up;
606 FixedArray<IMATH_NAMESPACE::Quat<T> > &result;
607 bool alignForward;
608
609 QuatArray_OrientToVectors (const FixedArray<IMATH_NAMESPACE::Vec3<T> > &forwardIn,
610 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &upIn,
611 FixedArray<IMATH_NAMESPACE::Quat<T> > &resultIn,
612 bool alignForwardIn)
613 : forward (forwardIn), up (upIn), result (resultIn),
614 alignForward (alignForwardIn) {}
615
616 void execute (size_t start, size_t end)
617 {
618 Vec3<T> f(0), u(0);
619 Euler<T> eu(0,0,0);
620 const Vec3<T> fRef(1,0,0);
621
622 for (size_t i = start; i < end; ++i)
623 {
624 if (alignForward)
625 {
626 f = forward[i].normalized();
627 u = up[i] - f.dot(up[i])*f;
628 u.normalize();
629 }
630 else
631 {
632 u = up[i].normalized();
633 f = forward[i] - u.dot(forward[i])*u;
634 f.normalize();
635 }
636
637 extractEulerXYZ (rotationMatrixWithUpDir (fRef, f, u), eu);
638 result[i] = eu.toQuat();
639 }
640 }
641 };
642
643 template <class T> static void
644 QuatArray_orientToVectors (FixedArray<IMATH_NAMESPACE::Quat<T> > &va,
645 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &forward,
646 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &up,
647 bool alignForward)
648 {
649 MATH_EXC_ON;
650 size_t len = va.match_dimension(forward);
651 va.match_dimension(up);
652
653 // Validate that 'va' is writable before entering the thread-task.
654 if (!va.writable())
655 throw std::invalid_argument ("Input fixed array is read-only.");
656
657 QuatArray_OrientToVectors<T> task (forward, up, va, alignForward);
658 dispatchTask (task, len);
659 }
660
661 template <class T>
662 struct QuatArray_Axis : public Task
663 {
664 const FixedArray<IMATH_NAMESPACE::Quat<T> > &va;
665 FixedArray<IMATH_NAMESPACE::Vec3<T> > &result;
666
667 QuatArray_Axis (const FixedArray<IMATH_NAMESPACE::Quat<T> > &vaIn,
668 FixedArray<IMATH_NAMESPACE::Vec3<T> > &resultIn)
669 : va (vaIn), result (resultIn) {}
670
671 void execute (size_t start, size_t end)
672 {
673 for (size_t i = start; i < end; ++i)
674 result[i] = va[i].axis();
675 }
676 };
677
678 template <class T> static FixedArray<IMATH_NAMESPACE::Vec3<T> >
679 QuatArray_axis(const FixedArray<IMATH_NAMESPACE::Quat<T> > &va)
680 {
681 MATH_EXC_ON;
682 size_t len = va.len();
683 FixedArray<IMATH_NAMESPACE::Vec3<T> > retval (Py_ssize_t(len), UNINITIALIZED);
684
685 QuatArray_Axis<T> task (va, retval);
686 dispatchTask (task, len);
687 return retval;
688 }
689
690 template <class T>
691 struct QuatArray_Angle : public Task
692 {
693 const FixedArray<IMATH_NAMESPACE::Quat<T> > &va;
694 FixedArray<T> &result;
695
696 QuatArray_Angle (const FixedArray<IMATH_NAMESPACE::Quat<T> > &vaIn,
697 FixedArray<T> &resultIn)
698 : va (vaIn), result (resultIn) {}
699
700 void execute (size_t start, size_t end)
701 {
702 for (size_t i = start; i < end; ++i)
703 result[i] = va[i].angle();
704 }
705 };
706
707
708 template <class T> static FixedArray<T>
709 QuatArray_angle(const FixedArray<IMATH_NAMESPACE::Quat<T> > &va)
710 {
711 MATH_EXC_ON;
712 size_t len = va.len();
713 FixedArray<T> retval (Py_ssize_t(len), UNINITIALIZED);
714
715 QuatArray_Angle<T> task (va, retval);
716 dispatchTask (task, len);
717 return retval;
718 }
719
720 template <class T>
721 struct QuatArray_RmulVec3 : public Task
722 {
723 const FixedArray<IMATH_NAMESPACE::Quat<T> > &a;
724 const Vec3<T> &v;
725 FixedArray<Vec3<T> > &r;
726
727 QuatArray_RmulVec3 (const FixedArray<IMATH_NAMESPACE::Quat<T> > &aIn,
728 const Vec3<T> &vIn, FixedArray<Vec3<T> > &rIn)
729 : a (aIn), v (vIn), r (rIn) {}
730
731 void execute(size_t start, size_t end)
732 {
733 for (size_t i = start; i < end; ++i)
734 {
735 Matrix44<T> m = a[i].toMatrix44();
736 r[i] = v * m;
737 }
738 }
739 };
740
741 template <class T>
742 static FixedArray< Vec3<T> >
743 QuatArray_rmulVec3 (const FixedArray< IMATH_NAMESPACE::Quat<T> > &a, const Vec3<T> &v)
744 {
745 MATH_EXC_ON;
746 size_t len = a.len();
747 FixedArray< Vec3<T> > r (Py_ssize_t(len), UNINITIALIZED);
748
749 QuatArray_RmulVec3<T> task (a, v, r);
750 dispatchTask (task, len);
751 return r;
752 }
753
754 template <class T>
755 struct QuatArray_RmulVec3Array : public Task
756 {
757 const FixedArray<IMATH_NAMESPACE::Quat<T> > &a;
758 const FixedArray<Vec3<T> > &b;
759 FixedArray<Vec3<T> > &r;
760
761 QuatArray_RmulVec3Array (const FixedArray<IMATH_NAMESPACE::Quat<T> > &aIn,
762 const FixedArray<Vec3<T> > &bIn,
763 FixedArray<Vec3<T> > &rIn)
764 : a (aIn), b (bIn), r (rIn) {}
765
766 void execute(size_t start, size_t end)
767 {
768 for (size_t i = start; i < end; ++i)
769 {
770 Matrix44<T> m = a[i].toMatrix44();
771 r[i] = b[i] * m;
772 }
773 }
774 };
775
776 template <class T>
777 static FixedArray< Vec3<T> >
778 QuatArray_rmulVec3Array (const FixedArray< IMATH_NAMESPACE::Quat<T> > &a,
779 const FixedArray< Vec3<T> > &b)
780 {
781 MATH_EXC_ON;
782 size_t len = a.match_dimension(b);
783 FixedArray< Vec3<T> > r (Py_ssize_t(len), UNINITIALIZED);
784
785 QuatArray_RmulVec3Array<T> task (a, b, r);
786 dispatchTask (task, len);
787 return r;
788 }
789
790 template <class T>
791 struct QuatArray_SetAxisAngle : public Task
792 {
793 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &axis;
794 const FixedArray<T> &angles;
795 FixedArray<IMATH_NAMESPACE::Quat<T> > &quats;
796
797 QuatArray_SetAxisAngle (const FixedArray<IMATH_NAMESPACE::Vec3<T> > &axisIn,
798 const FixedArray<T> &anglesIn,
799 FixedArray<IMATH_NAMESPACE::Quat<T> > &quatsIn)
800 : axis (axisIn), angles (anglesIn), quats (quatsIn) {}
801
802 void execute(size_t start, size_t end)
803 {
804 for (size_t i = start; i < end; ++i)
805 {
806 quats[i].setAxisAngle (axis[i], angles[i]);
807 }
808 }
809 };
810
811 template <class T>
812 static FixedArray< IMATH_NAMESPACE::Quat<T> > &
813 QuatArray_setAxisAngle (FixedArray< IMATH_NAMESPACE::Quat<T> > &quats,
814 const FixedArray< IMATH_NAMESPACE::Vec3<T> > &axis,
815 const FixedArray<T> &angles)
816 {
817 MATH_EXC_ON;
818 size_t len = quats.match_dimension(axis);
819 quats.match_dimension(angles);
820
821 // Validate that 'va' is writable before entering the thread-task.
822 if (!quats.writable())
823 throw std::invalid_argument ("Input fixed array is read-only.");
824
825 QuatArray_SetAxisAngle<T> task (axis, angles, quats);
826 dispatchTask (task, len);
827 return quats;
828 }
829
830 template <class T>
831 struct QuatArray_RotateVector : public Task
832 {
833 FixedArray<IMATH_NAMESPACE::Vec3<T> > &result;
834 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &vectors;
835 const FixedArray<IMATH_NAMESPACE::Quat<T> > &quats;
836
837 QuatArray_RotateVector (FixedArray<IMATH_NAMESPACE::Vec3<T> > &resultIn,
838 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &vectorsIn,
839 const FixedArray<IMATH_NAMESPACE::Quat<T> > &quatsIn)
840 : result (resultIn), vectors (vectorsIn), quats (quatsIn) {}
841
842 void execute(size_t start, size_t end)
843 {
844 for (size_t i = start; i < end; ++i)
845 {
846 result[i] = quats[i].rotateVector (vectors[i]);
847 }
848 }
849 };
850
851 template <class T>
852 static FixedArray<IMATH_NAMESPACE::Vec3<T> >
853 QuatArray_rotateVector (const FixedArray< IMATH_NAMESPACE::Quat<T> > &quats,
854 const FixedArray< IMATH_NAMESPACE::Vec3<T> > &vectors)
855 {
856 MATH_EXC_ON;
857 size_t len = quats.match_dimension(vectors);
858 FixedArray< IMATH_NAMESPACE::Vec3<T> > result (len);
859
860 QuatArray_RotateVector<T> task (result, vectors, quats);
861 dispatchTask (task, len);
862 return result;
863 }
864
865 template <class T>
866 struct QuatArray_Inverse : public Task
867 {
868 const FixedArray<IMATH_NAMESPACE::Quat<T> > &quats;
869 FixedArray<IMATH_NAMESPACE::Quat<T> > &result;
870
871 QuatArray_Inverse (FixedArray<IMATH_NAMESPACE::Quat<T> > &resultIn,
872 const FixedArray<IMATH_NAMESPACE::Quat<T> > &quatsIn)
873 : quats (quatsIn), result (resultIn) {}
874
875 void execute (size_t start, size_t end)
876 {
877 for (size_t i = start; i < end; ++i)
878 result[i] = quats[i].inverse();
879 }
880 };
881
882 template <class T>
883 static FixedArray<IMATH_NAMESPACE::Quat<T> >
884 QuatArray_inverse(const FixedArray<IMATH_NAMESPACE::Quat<T> > &quats)
885 {
886 MATH_EXC_ON;
887 size_t len = quats.len();
888 FixedArray<IMATH_NAMESPACE::Quat<T> > result (len);
889
890 QuatArray_Inverse<T> task (result, quats);
891 dispatchTask (task, len);
892
893 return result;
894 }
895
896 template <class T>
897 struct QuatArray_SetEulerXYZ : public Task
898 {
899 const FixedArray<IMATH_NAMESPACE::Vec3<T> > &rot;
900 FixedArray<IMATH_NAMESPACE::Quat<T> > &quats;
901
902 QuatArray_SetEulerXYZ (const FixedArray<IMATH_NAMESPACE::Vec3<T> > &rotIn,
903 FixedArray<IMATH_NAMESPACE::Quat<T> > &quatsIn)
904 : rot (rotIn), quats (quatsIn) {}
905
906 void execute (size_t start, size_t end)
907 {
908 for (size_t i = start; i < end; ++i)
909 {
910 Eulerf e(rot[i]);
911 quats[i] = e.toQuat();
912 }
913 }
914 };
915
916 template <class T>
917 static void
918 QuatArray_setEulerXYZ (FixedArray< IMATH_NAMESPACE::Quat<T> > &quats,
919 const FixedArray< IMATH_NAMESPACE::Vec3<T> > &rot)
920 {
921 MATH_EXC_ON;
922 size_t len = quats.match_dimension(rot);
923
924 // Validate that 'va' is writable before entering the thread-task.
925 if (!quats.writable())
926 throw std::invalid_argument ("Input fixed array is read-only.");
927
928 QuatArray_SetEulerXYZ<T> task (rot, quats);
929 dispatchTask (task, len);
930 }
931
932 template <class T>
933 struct QuatArray_Mul : public Task
934 {
935 const FixedArray<IMATH_NAMESPACE::Quat<T> > &q1;
936 const FixedArray<IMATH_NAMESPACE::Quat<T> > &q2;
937 FixedArray<IMATH_NAMESPACE::Quat<T> > &result;
938
939 QuatArray_Mul (const FixedArray<IMATH_NAMESPACE::Quat<T> > &q1In,
940 const FixedArray<IMATH_NAMESPACE::Quat<T> > &q2In,
941 FixedArray<IMATH_NAMESPACE::Quat<T> > &resultIn)
942 : q1 (q1In), q2 (q2In), result (resultIn) {}
943
944 void execute (size_t start, size_t end)
945 {
946 for (size_t i = start; i < end; ++i)
947 {
948 result[i] = q1[i] * q2[i];
949 }
950 }
951 };
952
953 template <class T>
954 static FixedArray< IMATH_NAMESPACE::Quat<T> >
955 QuatArray_mul(const FixedArray< IMATH_NAMESPACE::Quat<T> > &q1,
956 const FixedArray< IMATH_NAMESPACE::Quat<T> > &q2)
957 {
958 MATH_EXC_ON;
959 size_t len = q1.match_dimension(q2);
960 FixedArray< IMATH_NAMESPACE::Quat<T> > result (Py_ssize_t(len), UNINITIALIZED);
961
962 QuatArray_Mul<T> task (q1, q2, result);
963 dispatchTask (task, len);
964 return result;
965 }
966
967 template <class T>
968 struct QuatArray_QuatConstructor1 : public Task
969 {
970 const FixedArray<IMATH_NAMESPACE::Euler<T> > &euler;
971 FixedArray<IMATH_NAMESPACE::Quat<T> > &result;
972
973 QuatArray_QuatConstructor1 (const FixedArray<IMATH_NAMESPACE::Euler<T> > &eulerIn,
974 FixedArray<IMATH_NAMESPACE::Quat<T> > &resultIn)
975 : euler (eulerIn), result (resultIn) {}
976
977 void execute (size_t start, size_t end)
978 {
979 for (size_t i = start; i < end; ++i)
980 {
981 result[i] = euler[i].toQuat();
982 }
983 }
984 };
985
986 template <class T>
987 static FixedArray<IMATH_NAMESPACE::Quat<T> > *
988 QuatArray_quatConstructor1(const FixedArray<IMATH_NAMESPACE::Euler<T> > &e)
989 {
990 MATH_EXC_ON;
991 size_t len = e.len();
992 FixedArray<IMATH_NAMESPACE::Quat<T> >* result =
993 new FixedArray<IMATH_NAMESPACE::Quat<T> > (Py_ssize_t(len), UNINITIALIZED);
994
995 QuatArray_QuatConstructor1<T> task (e, *result);
996 dispatchTask (task, len);
997 return result;
998 }
999
1000 template <class T>
1001 struct QuatArray_ExtractTask : public Task
1002 {
1003 const FixedArray<Matrix44<double> > &m;
1004 FixedArray<IMATH_NAMESPACE::Quat<T> > &result;
1005
1006 QuatArray_ExtractTask (const FixedArray<Matrix44<double> > &mIn,
1007 FixedArray<IMATH_NAMESPACE::Quat<T> > &resultIn)
1008 : m (mIn), result (resultIn) {}
1009
1010 void execute (size_t start, size_t end)
1011 {
1012 for (size_t i = start; i < end; ++i)
1013 {
1014 result[i] = IMATH_NAMESPACE::extractQuat (m[i]);
1015 }
1016 }
1017 };
1018
1019 template <class T>
1020 static void
1021 QuatArray_extract(FixedArray<IMATH_NAMESPACE::Quat<T> > &q,
1022 const FixedArray<Matrix44<double> > &m)
1023 {
1024 MATH_EXC_ON;
1025 const size_t len = q.match_dimension(m);
1026
1027 QuatArray_ExtractTask<T> task (m, q);
1028 dispatchTask (task, len);
1029 }
1030
1031 template <class T>
1032 class_<FixedArray<IMATH_NAMESPACE::Quat<T> > >
1033 register_QuatArray()
1034 {
1035 using boost::mpl::true_;
1036 using boost::mpl::false_;
1037
1038 typedef IMATH_NAMESPACE::Quat<T> QuatT;
1039
1040 class_<FixedArray<QuatT> > quatArray_class = FixedArray<QuatT>::register_("Fixed length array of IMATH_NAMESPACE::Quat");
1041 quatArray_class
1042 .add_property("r",&QuatArray_get<T,0>)
1043 .add_property("x",&QuatArray_get<T,1>)
1044 .add_property("y",&QuatArray_get<T,2>)
1045 .add_property("z",&QuatArray_get<T,3>)
1046 .def("setRotation", &QuatArray_setRotation<T>,
1047 "set rotation angles for each quat",
1048 (args("from", "to")))
1049 .def("orientToVectors", &QuatArray_orientToVectors<T>,
1050 "Sets the orientations to match the given forward and up vectors, "
1051 "matching the forward vector exactly if 'alignForward' is True, matching "
1052 "the up vector exactly if 'alignForward' is False. If the vectors are "
1053 "already orthogonal, both vectors will be matched exactly.",
1054 (args("forward", "up", "alignForward")))
1055 .def("extract", &QuatArray_extract<T>,
1056 "Extract the rotation component of an M44d and return it as a quaternion.",
1057 (args("lxform")))
1058 .def("axis", &QuatArray_axis<T>,
1059 "get rotation axis for each quat")
1060 .def("angle", &QuatArray_angle<T>,
1061 "get rotation angle about the axis returned by axis() for each quat")
1062 .def("setAxisAngle", &QuatArray_setAxisAngle<T>,
1063 "set the quaternion arrays from a given axis and angle",
1064 (args("axis", "angle")), return_value_policy<copy_non_const_reference>())
1065 .def("setEulerXYZ", &QuatArray_setEulerXYZ<T>,
1066 "set the quaternion arrays from a given euler XYZ angle vector",
1067 (args("euler")))
1068 .def("rotateVector", &QuatArray_rotateVector<T>,
1069 "Rotate the supplied vectors by the quaternions. Assumes quaternions are normalized.",
1070 (args("vector")))
1071 .def("inverse", &QuatArray_inverse<T>,
1072 "Return 1/Q for each quaternion.",
1073 (args("QuatArray")))
1074 .def("__rmul__", &QuatArray_rmulVec3<T>)
1075 .def("__rmul__", &QuatArray_rmulVec3Array<T>)
1076 .def("__init__", make_constructor(QuatArray_quatConstructor1<T>))
1077 ;
1078
1079 generate_member_bindings<op_quatDot<QuatT>, true_>
1080 (quatArray_class, "dot",
1081 "Return the element-by-element Euclidean inner product",
1082 args("qB"));
1083 generate_member_bindings<op_quatDot<QuatT>, true_>
1084 (quatArray_class, "euclideanInnerProduct",
1085 "Return the element-by-element Euclidean inner product",
1086 args("qB"));
1087 generate_member_bindings<op_quatNormalize<QuatT> >
1088 (quatArray_class, "normalize",
1089 "Normalize each quaternion in the array");
1090 generate_member_bindings<op_quatNormalized<QuatT> >
1091 (quatArray_class, "normalized",
1092 "Return a new quaternion array with unit quaternions.");
1093
1094 generate_member_bindings<op_neg<QuatT> >
1095 (quatArray_class, "__neg__" , "-self");
1096 generate_member_bindings<op_mul<QuatT,QuatT>, true_ >
1097 (quatArray_class, "__mul__", "self * qB", args("qB"));
1098 generate_member_bindings<op_mul<QuatT,T>, false_ >
1099 (quatArray_class, "__mul__", "self * x", args("x"));
1100 generate_member_bindings<op_mul<QuatT,T>, false_ >
1101 (quatArray_class, "__rmul__", "self * x", args("x"));
1102 generate_member_bindings<op_quatDot<QuatT>, true_ >
1103 (quatArray_class, "__xor__", "self.dot(qB)", args("qB"));
1104
1105 generate_member_bindings<op_quatSlerp<QuatT>, true_, false_ >
1106 (quatArray_class,
1107 "slerp",
1108 "Return the element-by-element shortest arc spherical linear interpolation between self and B.",
1109 args("qB", "t"));
1110
1111 add_comparison_functions(quatArray_class);
1112 decoratecopy(quatArray_class);
1113
1114 return quatArray_class;
1115 }
1116
1117 template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Quat<float> > register_Quat<float>();
1118 template PYIMATH_EXPORT class_<IMATH_NAMESPACE::Quat<double> > register_Quat<double>();
1119
1120 template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Quat<float> > > register_QuatArray<float>();
1121 template PYIMATH_EXPORT class_<FixedArray<IMATH_NAMESPACE::Quat<double> > > register_QuatArray<double>();
1122 template<> PYIMATH_EXPORT IMATH_NAMESPACE::Quat<float> FixedArrayDefaultValue<IMATH_NAMESPACE::Quat<float> >::value() { return IMATH_NAMESPACE::Quat<float>(); }
1123 template<> PYIMATH_EXPORT IMATH_NAMESPACE::Quat<double> FixedArrayDefaultValue<IMATH_NAMESPACE::Quat<double> >::value() { return IMATH_NAMESPACE::Quat<double>(); }
1124 }