mathutils rotate functions for Euler/Quaternion/Matrix/Vector types.
[blender.git] / source / blender / python / generic / mathutils_quat.c
1 /*
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
21  * All rights reserved.
22  *
23  *
24  * Contributor(s): Joseph Gilbert
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 #include "mathutils.h"
30
31 #include "BLI_math.h"
32 #include "BLI_utildefines.h"
33
34 #define QUAT_SIZE 4
35
36 #define QUAT_APPLY_TO_COPY(quat_meth_noargs, _self) \
37         QuaternionObject *ret= (QuaternionObject *)Quaternion_copy(_self); \
38         PyObject *ret_dummy= quat_meth_noargs(ret); \
39         if(ret_dummy) { \
40                 Py_DECREF(ret_dummy); \
41                 return (PyObject *)ret; \
42         } \
43         else { /* error */ \
44                 Py_DECREF(ret); \
45                 return NULL; \
46         } \
47
48 static PyObject *Quaternion_copy(QuaternionObject *self);
49
50 //-----------------------------METHODS------------------------------
51
52 /* note: BaseMath_ReadCallback must be called beforehand */
53 static PyObject *Quaternion_to_tuple_ext(QuaternionObject *self, int ndigits)
54 {
55         PyObject *ret;
56         int i;
57
58         ret= PyTuple_New(QUAT_SIZE);
59
60         if(ndigits >= 0) {
61                 for(i= 0; i < QUAT_SIZE; i++) {
62                         PyTuple_SET_ITEM(ret, i, PyFloat_FromDouble(double_round((double)self->quat[i], ndigits)));
63                 }
64         }
65         else {
66                 for(i= 0; i < QUAT_SIZE; i++) {
67                         PyTuple_SET_ITEM(ret, i, PyFloat_FromDouble(self->quat[i]));
68                 }
69         }
70
71         return ret;
72 }
73
74 static char Quaternion_to_euler_doc[] =
75 ".. method:: to_euler(order, euler_compat)\n"
76 "\n"
77 "   Return Euler representation of the quaternion.\n"
78 "\n"
79 "   :arg order: Optional rotation order argument in ['XYZ', 'XZY', 'YXZ', 'YZX', 'ZXY', 'ZYX'].\n"
80 "   :type order: string\n"
81 "   :arg euler_compat: Optional euler argument the new euler will be made compatible with (no axis flipping between them). Useful for converting a series of matrices to animation curves.\n"
82 "   :type euler_compat: :class:`Euler`\n"
83 "   :return: Euler representation of the quaternion.\n"
84 "   :rtype: :class:`Euler`\n"
85 ;
86 static PyObject *Quaternion_to_euler(QuaternionObject *self, PyObject *args)
87 {
88         float tquat[4];
89         float eul[3];
90         const char *order_str= NULL;
91         short order= EULER_ORDER_XYZ;
92         EulerObject *eul_compat = NULL;
93
94         if(!PyArg_ParseTuple(args, "|sO!:to_euler", &order_str, &euler_Type, &eul_compat))
95                 return NULL;
96
97         if(!BaseMath_ReadCallback(self))
98                 return NULL;
99
100         if(order_str) {
101                 order= euler_order_from_string(order_str, "Matrix.to_euler()");
102
103                 if(order == -1)
104                         return NULL;
105         }
106
107         normalize_qt_qt(tquat, self->quat);
108
109         if(eul_compat) {
110                 float mat[3][3];
111
112                 if(!BaseMath_ReadCallback(eul_compat))
113                         return NULL;
114
115                 quat_to_mat3(mat, tquat);
116
117                 if(order == EULER_ORDER_XYZ)    mat3_to_compatible_eul(eul, eul_compat->eul, mat);
118                 else                                                    mat3_to_compatible_eulO(eul, eul_compat->eul, order, mat);
119         }
120         else {
121                 if(order == EULER_ORDER_XYZ)    quat_to_eul(eul, tquat);
122                 else                                                    quat_to_eulO(eul, order, tquat);
123         }
124
125         return newEulerObject(eul, order, Py_NEW, NULL);
126 }
127 //----------------------------Quaternion.toMatrix()------------------
128 static char Quaternion_to_matrix_doc[] =
129 ".. method:: to_matrix()\n"
130 "\n"
131 "   Return a matrix representation of the quaternion.\n"
132 "\n"
133 "   :return: A 3x3 rotation matrix representation of the quaternion.\n"
134 "   :rtype: :class:`Matrix`\n"
135 ;
136 static PyObject *Quaternion_to_matrix(QuaternionObject *self)
137 {
138         float mat[9]; /* all values are set */
139
140         if(!BaseMath_ReadCallback(self))
141                 return NULL;
142
143         quat_to_mat3( (float (*)[3]) mat,self->quat);
144         return newMatrixObject(mat, 3, 3, Py_NEW, NULL);
145 }
146
147 //----------------------------Quaternion.cross(other)------------------
148 static char Quaternion_cross_doc[] =
149 ".. method:: cross(other)\n"
150 "\n"
151 "   Return the cross product of this quaternion and another.\n"
152 "\n"
153 "   :arg other: The other quaternion to perform the cross product with.\n"
154 "   :type other: :class:`Quaternion`\n"
155 "   :return: The cross product.\n"
156 "   :rtype: :class:`Quaternion`\n"
157 ;
158 static PyObject *Quaternion_cross(QuaternionObject *self, PyObject *value)
159 {
160         float quat[QUAT_SIZE], tquat[QUAT_SIZE];
161
162         if(!BaseMath_ReadCallback(self))
163                 return NULL;
164
165         if(mathutils_array_parse(tquat, QUAT_SIZE, QUAT_SIZE, value, "quaternion.cross(other), invalid 'other' arg") == -1)
166                 return NULL;
167
168         mul_qt_qtqt(quat, self->quat, tquat);
169         return newQuaternionObject(quat, Py_NEW, Py_TYPE(self));
170 }
171
172 //----------------------------Quaternion.dot(other)------------------
173 static char Quaternion_dot_doc[] =
174 ".. method:: dot(other)\n"
175 "\n"
176 "   Return the dot product of this quaternion and another.\n"
177 "\n"
178 "   :arg other: The other quaternion to perform the dot product with.\n"
179 "   :type other: :class:`Quaternion`\n"
180 "   :return: The dot product.\n"
181 "   :rtype: :class:`Quaternion`\n"
182 ;
183 static PyObject *Quaternion_dot(QuaternionObject *self, PyObject *value)
184 {
185         float tquat[QUAT_SIZE];
186
187         if(!BaseMath_ReadCallback(self))
188                 return NULL;
189
190         if(mathutils_array_parse(tquat, QUAT_SIZE, QUAT_SIZE, value, "quaternion.dot(other), invalid 'other' arg") == -1)
191                 return NULL;
192
193         return PyFloat_FromDouble(dot_qtqt(self->quat, tquat));
194 }
195
196 static char Quaternion_difference_doc[] =
197 ".. function:: difference(other)\n"
198 "\n"
199 "   Returns a quaternion representing the rotational difference.\n"
200 "\n"
201 "   :arg other: second quaternion.\n"
202 "   :type other: :class:`Quaternion`\n"
203 "   :return: the rotational difference between the two quat rotations.\n"
204 "   :rtype: :class:`Quaternion`\n"
205 ;
206 static PyObject *Quaternion_difference(QuaternionObject *self, PyObject *value)
207 {
208         float tquat[QUAT_SIZE], quat[QUAT_SIZE];
209
210         if(!BaseMath_ReadCallback(self))
211                 return NULL;
212
213         if(mathutils_array_parse(tquat, QUAT_SIZE, QUAT_SIZE, value, "quaternion.difference(other), invalid 'other' arg") == -1)
214                 return NULL;
215
216         rotation_between_quats_to_quat(quat, self->quat, tquat);
217
218         return newQuaternionObject(quat, Py_NEW, Py_TYPE(self));
219 }
220
221 static char Quaternion_slerp_doc[] =
222 ".. function:: slerp(other, factor)\n"
223 "\n"
224 "   Returns the interpolation of two quaternions.\n"
225 "\n"
226 "   :arg other: value to interpolate with.\n"
227 "   :type other: :class:`Quaternion`\n"
228 "   :arg factor: The interpolation value in [0.0, 1.0].\n"
229 "   :type factor: float\n"
230 "   :return: The interpolated rotation.\n"
231 "   :rtype: :class:`Quaternion`\n"
232 ;
233 static PyObject *Quaternion_slerp(QuaternionObject *self, PyObject *args)
234 {
235         PyObject *value;
236         float tquat[QUAT_SIZE], quat[QUAT_SIZE], fac;
237
238         if(!PyArg_ParseTuple(args, "Of:slerp", &value, &fac)) {
239                 PyErr_SetString(PyExc_TypeError, "quat.slerp(): expected Quaternion types and float");
240                 return NULL;
241         }
242
243         if(!BaseMath_ReadCallback(self))
244                 return NULL;
245
246         if(mathutils_array_parse(tquat, QUAT_SIZE, QUAT_SIZE, value, "quaternion.slerp(other), invalid 'other' arg") == -1)
247                 return NULL;
248
249         if(fac > 1.0f || fac < 0.0f) {
250                 PyErr_SetString(PyExc_AttributeError, "quat.slerp(): interpolation factor must be between 0.0 and 1.0");
251                 return NULL;
252         }
253
254         interp_qt_qtqt(quat, self->quat, tquat, fac);
255
256         return newQuaternionObject(quat, Py_NEW, Py_TYPE(self));
257 }
258
259 static char Quaternion_rotate_doc[] =
260 ".. method:: rotate(other)\n"
261 "\n"
262 "   Rotates the quaternion a by another mathutils value.\n"
263 "\n"
264 "   :arg other: rotation component of mathutils value\n"
265 "   :type other: :class:`Euler`, :class:`Quaternion` or :class:`Matrix`\n"
266 ;
267 static PyObject *Quaternion_rotate(QuaternionObject *self, PyObject *value)
268 {
269         float self_rmat[3][3], other_rmat[3][3], rmat[3][3];
270         float tquat[4], length;
271
272         if(!BaseMath_ReadCallback(self))
273                 return NULL;
274
275         if(mathutils_any_to_rotmat(other_rmat, value, "quaternion.rotate(value)") == -1)
276                 return NULL;
277
278         length= normalize_qt_qt(tquat, self->quat);
279         quat_to_mat3(self_rmat, tquat);
280         mul_m3_m3m3(rmat, self_rmat, other_rmat);
281
282         mat3_to_quat(self->quat, rmat);
283         mul_qt_fl(self->quat, length); /* maintain length after rotating */
284
285         (void)BaseMath_WriteCallback(self);
286         Py_RETURN_NONE;
287 }
288
289 //----------------------------Quaternion.normalize()----------------
290 //normalize the axis of rotation of [theta,vector]
291 static char Quaternion_normalize_doc[] =
292 ".. function:: normalize()\n"
293 "\n"
294 "   Normalize the quaternion.\n"
295 ;
296 static PyObject *Quaternion_normalize(QuaternionObject *self)
297 {
298         if(!BaseMath_ReadCallback(self))
299                 return NULL;
300
301         normalize_qt(self->quat);
302
303         (void)BaseMath_WriteCallback(self);
304         Py_RETURN_NONE;
305 }
306 static char Quaternion_normalized_doc[] =
307 ".. function:: normalized()\n"
308 "\n"
309 "   Return a new normalized quaternion.\n"
310 "\n"
311 "   :return: a normalized copy.\n"
312 "   :rtype: :class:`Quaternion`\n"
313 ;
314 static PyObject *Quaternion_normalized(QuaternionObject *self)
315 {
316         QUAT_APPLY_TO_COPY(Quaternion_normalize, self);
317 }
318
319 //----------------------------Quaternion.invert()------------------
320 static char Quaternion_invert_doc[] =
321 ".. function:: invert()\n"
322 "\n"
323 "   Set the quaternion to its inverse.\n"
324 ;
325 static PyObject *Quaternion_invert(QuaternionObject *self)
326 {
327         if(!BaseMath_ReadCallback(self))
328                 return NULL;
329
330         invert_qt(self->quat);
331
332         (void)BaseMath_WriteCallback(self);
333         Py_RETURN_NONE;
334 }
335 static char Quaternion_inverted_doc[] =
336 ".. function:: inverted()\n"
337 "\n"
338 "   Return a new, inverted quaternion.\n"
339 "\n"
340 "   :return: the inverted value.\n"
341 "   :rtype: :class:`Quaternion`\n"
342 ;
343 static PyObject *Quaternion_inverted(QuaternionObject *self)
344 {
345         QUAT_APPLY_TO_COPY(Quaternion_invert, self);
346 }
347
348 //----------------------------Quaternion.identity()-----------------
349 static char Quaternion_identity_doc[] =
350 ".. function:: identity()\n"
351 "\n"
352 "   Set the quaternion to an identity quaternion.\n"
353 "\n"
354 "   :return: an instance of itself.\n"
355 "   :rtype: :class:`Quaternion`\n"
356 ;
357 static PyObject *Quaternion_identity(QuaternionObject *self)
358 {
359         if(!BaseMath_ReadCallback(self))
360                 return NULL;
361
362         unit_qt(self->quat);
363
364         (void)BaseMath_WriteCallback(self);
365         Py_RETURN_NONE;
366 }
367 //----------------------------Quaternion.negate()-------------------
368 static char Quaternion_negate_doc[] =
369 ".. function:: negate()\n"
370 "\n"
371 "   Set the quaternion to its negative.\n"
372 "\n"
373 "   :return: an instance of itself.\n"
374 "   :rtype: :class:`Quaternion`\n"
375 ;
376 static PyObject *Quaternion_negate(QuaternionObject *self)
377 {
378         if(!BaseMath_ReadCallback(self))
379                 return NULL;
380
381         mul_qt_fl(self->quat, -1.0f);
382
383         (void)BaseMath_WriteCallback(self);
384         Py_RETURN_NONE;
385 }
386 //----------------------------Quaternion.conjugate()----------------
387 static char Quaternion_conjugate_doc[] =
388 ".. function:: conjugate()\n"
389 "\n"
390 "   Set the quaternion to its conjugate (negate x, y, z).\n"
391 ;
392 static PyObject *Quaternion_conjugate(QuaternionObject *self)
393 {
394         if(!BaseMath_ReadCallback(self))
395                 return NULL;
396
397         conjugate_qt(self->quat);
398
399         (void)BaseMath_WriteCallback(self);
400         Py_RETURN_NONE;
401 }
402 static char Quaternion_conjugated_doc[] =
403 ".. function:: conjugated()\n"
404 "\n"
405 "   Return a new conjugated quaternion.\n"
406 "\n"
407 "   :return: a new quaternion.\n"
408 "   :rtype: :class:`Quaternion`\n"
409 ;
410 static PyObject *Quaternion_conjugated(QuaternionObject *self)
411 {
412         QUAT_APPLY_TO_COPY(Quaternion_conjugate, self);
413 }
414
415 //----------------------------Quaternion.copy()----------------
416 static char Quaternion_copy_doc[] =
417 ".. function:: copy()\n"
418 "\n"
419 "   Returns a copy of this quaternion.\n"
420 "\n"
421 "   :return: A copy of the quaternion.\n"
422 "   :rtype: :class:`Quaternion`\n"
423 "\n"
424 "   .. note:: use this to get a copy of a wrapped quaternion with no reference to the original data.\n"
425 ;
426 static PyObject *Quaternion_copy(QuaternionObject *self)
427 {
428         if(!BaseMath_ReadCallback(self))
429                 return NULL;
430
431         return newQuaternionObject(self->quat, Py_NEW, Py_TYPE(self));
432 }
433
434 //----------------------------print object (internal)--------------
435 //print the object to screen
436 static PyObject *Quaternion_repr(QuaternionObject *self)
437 {
438         PyObject *ret, *tuple;
439
440         if(!BaseMath_ReadCallback(self))
441                 return NULL;
442
443         tuple= Quaternion_to_tuple_ext(self, -1);
444
445         ret= PyUnicode_FromFormat("Quaternion(%R)", tuple);
446
447         Py_DECREF(tuple);
448         return ret;
449 }
450
451 //------------------------tp_richcmpr
452 //returns -1 execption, 0 false, 1 true
453 static PyObject* Quaternion_richcmpr(PyObject *objectA, PyObject *objectB, int comparison_type)
454 {
455         QuaternionObject *quatA = NULL, *quatB = NULL;
456         int result = 0;
457
458         if(QuaternionObject_Check(objectA)) {
459                 quatA = (QuaternionObject*)objectA;
460                 if(!BaseMath_ReadCallback(quatA))
461                         return NULL;
462         }
463         if(QuaternionObject_Check(objectB)) {
464                 quatB = (QuaternionObject*)objectB;
465                 if(!BaseMath_ReadCallback(quatB))
466                         return NULL;
467         }
468
469         if (!quatA || !quatB){
470                 if (comparison_type == Py_NE){
471                         Py_RETURN_TRUE;
472                 }else{
473                         Py_RETURN_FALSE;
474                 }
475         }
476
477         switch (comparison_type){
478                 case Py_EQ:
479                         result = EXPP_VectorsAreEqual(quatA->quat, quatB->quat, QUAT_SIZE, 1);
480                         break;
481                 case Py_NE:
482                         result = EXPP_VectorsAreEqual(quatA->quat, quatB->quat, QUAT_SIZE, 1);
483                         if (result == 0){
484                                 result = 1;
485                         }else{
486                                 result = 0;
487                         }
488                         break;
489                 default:
490                         printf("The result of the comparison could not be evaluated");
491                         break;
492         }
493         if (result == 1){
494                 Py_RETURN_TRUE;
495         }else{
496                 Py_RETURN_FALSE;
497         }
498 }
499
500 //---------------------SEQUENCE PROTOCOLS------------------------
501 //----------------------------len(object)------------------------
502 //sequence length
503 static int Quaternion_len(QuaternionObject *UNUSED(self))
504 {
505         return QUAT_SIZE;
506 }
507 //----------------------------object[]---------------------------
508 //sequence accessor (get)
509 static PyObject *Quaternion_item(QuaternionObject *self, int i)
510 {
511         if(i<0) i= QUAT_SIZE-i;
512
513         if(i < 0 || i >= QUAT_SIZE) {
514                 PyErr_SetString(PyExc_IndexError, "quaternion[attribute]: array index out of range");
515                 return NULL;
516         }
517
518         if(!BaseMath_ReadIndexCallback(self, i))
519                 return NULL;
520
521         return PyFloat_FromDouble(self->quat[i]);
522
523 }
524 //----------------------------object[]-------------------------
525 //sequence accessor (set)
526 static int Quaternion_ass_item(QuaternionObject *self, int i, PyObject *ob)
527 {
528         float scalar= (float)PyFloat_AsDouble(ob);
529         if(scalar==-1.0f && PyErr_Occurred()) { /* parsed item not a number */
530                 PyErr_SetString(PyExc_TypeError, "quaternion[index] = x: index argument not a number");
531                 return -1;
532         }
533
534         if(i<0) i= QUAT_SIZE-i;
535
536         if(i < 0 || i >= QUAT_SIZE){
537                 PyErr_SetString(PyExc_IndexError, "quaternion[attribute] = x: array assignment index out of range");
538                 return -1;
539         }
540         self->quat[i] = scalar;
541
542         if(!BaseMath_WriteIndexCallback(self, i))
543                 return -1;
544
545         return 0;
546 }
547 //----------------------------object[z:y]------------------------
548 //sequence slice (get)
549 static PyObject *Quaternion_slice(QuaternionObject *self, int begin, int end)
550 {
551         PyObject *tuple;
552         int count;
553
554         if(!BaseMath_ReadCallback(self))
555                 return NULL;
556
557         CLAMP(begin, 0, QUAT_SIZE);
558         if (end<0) end= (QUAT_SIZE + 1) + end;
559         CLAMP(end, 0, QUAT_SIZE);
560         begin= MIN2(begin, end);
561
562         tuple= PyTuple_New(end - begin);
563         for(count= begin; count < end; count++) {
564                 PyTuple_SET_ITEM(tuple, count - begin, PyFloat_FromDouble(self->quat[count]));
565         }
566
567         return tuple;
568 }
569 //----------------------------object[z:y]------------------------
570 //sequence slice (set)
571 static int Quaternion_ass_slice(QuaternionObject *self, int begin, int end, PyObject *seq)
572 {
573         int i, size;
574         float quat[QUAT_SIZE];
575
576         if(!BaseMath_ReadCallback(self))
577                 return -1;
578
579         CLAMP(begin, 0, QUAT_SIZE);
580         if (end<0) end= (QUAT_SIZE + 1) + end;
581         CLAMP(end, 0, QUAT_SIZE);
582         begin = MIN2(begin,end);
583
584         if((size=mathutils_array_parse(quat, 0, QUAT_SIZE, seq, "mathutils.Quaternion[begin:end] = []")) == -1)
585                 return -1;
586
587         if(size != (end - begin)){
588                 PyErr_SetString(PyExc_TypeError, "quaternion[begin:end] = []: size mismatch in slice assignment");
589                 return -1;
590         }
591
592         /* parsed well - now set in vector */
593         for(i= 0; i < size; i++)
594                 self->quat[begin + i] = quat[i];
595
596         (void)BaseMath_WriteCallback(self);
597         return 0;
598 }
599
600
601 static PyObject *Quaternion_subscript(QuaternionObject *self, PyObject *item)
602 {
603         if (PyIndex_Check(item)) {
604                 Py_ssize_t i;
605                 i = PyNumber_AsSsize_t(item, PyExc_IndexError);
606                 if (i == -1 && PyErr_Occurred())
607                         return NULL;
608                 if (i < 0)
609                         i += QUAT_SIZE;
610                 return Quaternion_item(self, i);
611         } else if (PySlice_Check(item)) {
612                 Py_ssize_t start, stop, step, slicelength;
613
614                 if (PySlice_GetIndicesEx((void *)item, QUAT_SIZE, &start, &stop, &step, &slicelength) < 0)
615                         return NULL;
616
617                 if (slicelength <= 0) {
618                         return PyTuple_New(0);
619                 }
620                 else if (step == 1) {
621                         return Quaternion_slice(self, start, stop);
622                 }
623                 else {
624                         PyErr_SetString(PyExc_TypeError, "slice steps not supported with quaternions");
625                         return NULL;
626                 }
627         }
628         else {
629                 PyErr_Format(PyExc_TypeError, "quaternion indices must be integers, not %.200s", Py_TYPE(item)->tp_name);
630                 return NULL;
631         }
632 }
633
634
635 static int Quaternion_ass_subscript(QuaternionObject *self, PyObject *item, PyObject *value)
636 {
637         if (PyIndex_Check(item)) {
638                 Py_ssize_t i = PyNumber_AsSsize_t(item, PyExc_IndexError);
639                 if (i == -1 && PyErr_Occurred())
640                         return -1;
641                 if (i < 0)
642                         i += QUAT_SIZE;
643                 return Quaternion_ass_item(self, i, value);
644         }
645         else if (PySlice_Check(item)) {
646                 Py_ssize_t start, stop, step, slicelength;
647
648                 if (PySlice_GetIndicesEx((void *)item, QUAT_SIZE, &start, &stop, &step, &slicelength) < 0)
649                         return -1;
650
651                 if (step == 1)
652                         return Quaternion_ass_slice(self, start, stop, value);
653                 else {
654                         PyErr_SetString(PyExc_TypeError, "slice steps not supported with quaternion");
655                         return -1;
656                 }
657         }
658         else {
659                 PyErr_Format(PyExc_TypeError, "quaternion indices must be integers, not %.200s", Py_TYPE(item)->tp_name);
660                 return -1;
661         }
662 }
663
664 //------------------------NUMERIC PROTOCOLS----------------------
665 //------------------------obj + obj------------------------------
666 //addition
667 static PyObject *Quaternion_add(PyObject *q1, PyObject *q2)
668 {
669         float quat[QUAT_SIZE];
670         QuaternionObject *quat1 = NULL, *quat2 = NULL;
671
672         if(!QuaternionObject_Check(q1) || !QuaternionObject_Check(q2)) {
673                 PyErr_SetString(PyExc_AttributeError, "Quaternion addition: arguments not valid for this operation");
674                 return NULL;
675         }
676         quat1 = (QuaternionObject*)q1;
677         quat2 = (QuaternionObject*)q2;
678
679         if(!BaseMath_ReadCallback(quat1) || !BaseMath_ReadCallback(quat2))
680                 return NULL;
681
682         add_qt_qtqt(quat, quat1->quat, quat2->quat, 1.0f);
683         return newQuaternionObject(quat, Py_NEW, Py_TYPE(q1));
684 }
685 //------------------------obj - obj------------------------------
686 //subtraction
687 static PyObject *Quaternion_sub(PyObject *q1, PyObject *q2)
688 {
689         int x;
690         float quat[QUAT_SIZE];
691         QuaternionObject *quat1 = NULL, *quat2 = NULL;
692
693         if(!QuaternionObject_Check(q1) || !QuaternionObject_Check(q2)) {
694                 PyErr_SetString(PyExc_AttributeError, "Quaternion addition: arguments not valid for this operation");
695                 return NULL;
696         }
697
698         quat1 = (QuaternionObject*)q1;
699         quat2 = (QuaternionObject*)q2;
700
701         if(!BaseMath_ReadCallback(quat1) || !BaseMath_ReadCallback(quat2))
702                 return NULL;
703
704         for(x = 0; x < QUAT_SIZE; x++) {
705                 quat[x] = quat1->quat[x] - quat2->quat[x];
706         }
707
708         return newQuaternionObject(quat, Py_NEW, Py_TYPE(q1));
709 }
710
711 static PyObject *quat_mul_float(QuaternionObject *quat, const float scalar)
712 {
713         float tquat[4];
714         copy_qt_qt(tquat, quat->quat);
715         mul_qt_fl(tquat, scalar);
716         return newQuaternionObject(tquat, Py_NEW, Py_TYPE(quat));
717 }
718
719 //------------------------obj * obj------------------------------
720 //mulplication
721 static PyObject *Quaternion_mul(PyObject *q1, PyObject *q2)
722 {
723         float quat[QUAT_SIZE], scalar;
724         QuaternionObject *quat1 = NULL, *quat2 = NULL;
725
726         if(QuaternionObject_Check(q1)) {
727                 quat1 = (QuaternionObject*)q1;
728                 if(!BaseMath_ReadCallback(quat1))
729                         return NULL;
730         }
731         if(QuaternionObject_Check(q2)) {
732                 quat2 = (QuaternionObject*)q2;
733                 if(!BaseMath_ReadCallback(quat2))
734                         return NULL;
735         }
736
737         if(quat1 && quat2) { /* QUAT*QUAT (cross product) */
738                 mul_qt_qtqt(quat, quat1->quat, quat2->quat);
739                 return newQuaternionObject(quat, Py_NEW, Py_TYPE(q1));
740         }
741         /* the only case this can happen (for a supported type is "FLOAT*QUAT" ) */
742         else if(quat2) { /* FLOAT*QUAT */
743                 if(((scalar= PyFloat_AsDouble(q1)) == -1.0 && PyErr_Occurred())==0) {
744                         return quat_mul_float(quat2, scalar);
745                 }
746         }
747         else if (quat1) { /* QUAT*FLOAT */
748                 if((((scalar= PyFloat_AsDouble(q2)) == -1.0 && PyErr_Occurred())==0)) {
749                         return quat_mul_float(quat1, scalar);
750                 }
751         }
752         else {
753                 BLI_assert(!"internal error");
754         }
755
756         PyErr_Format(PyExc_TypeError, "Quaternion multiplication: not supported between '%.200s' and '%.200s' types", Py_TYPE(q1)->tp_name, Py_TYPE(q2)->tp_name);
757         return NULL;
758 }
759
760 //-----------------PROTOCOL DECLARATIONS--------------------------
761 static PySequenceMethods Quaternion_SeqMethods = {
762         (lenfunc) Quaternion_len,                               /* sq_length */
763         (binaryfunc) NULL,                                              /* sq_concat */
764         (ssizeargfunc) NULL,                                    /* sq_repeat */
765         (ssizeargfunc) Quaternion_item,                 /* sq_item */
766         (ssizessizeargfunc) NULL,                               /* sq_slice, deprecated */
767         (ssizeobjargproc) Quaternion_ass_item,  /* sq_ass_item */
768         (ssizessizeobjargproc) NULL,                    /* sq_ass_slice, deprecated */
769         (objobjproc) NULL,                                              /* sq_contains */
770         (binaryfunc) NULL,                                              /* sq_inplace_concat */
771         (ssizeargfunc) NULL,                                    /* sq_inplace_repeat */
772 };
773
774 static PyMappingMethods Quaternion_AsMapping = {
775         (lenfunc)Quaternion_len,
776         (binaryfunc)Quaternion_subscript,
777         (objobjargproc)Quaternion_ass_subscript
778 };
779
780 static PyNumberMethods Quaternion_NumMethods = {
781         (binaryfunc)    Quaternion_add, /*nb_add*/
782         (binaryfunc)    Quaternion_sub, /*nb_subtract*/
783         (binaryfunc)    Quaternion_mul, /*nb_multiply*/
784         0,                                                      /*nb_remainder*/
785         0,                                                      /*nb_divmod*/
786         0,                                                      /*nb_power*/
787         (unaryfunc)     0,      /*nb_negative*/
788         (unaryfunc)     0,      /*tp_positive*/
789         (unaryfunc)     0,      /*tp_absolute*/
790         (inquiry)       0,      /*tp_bool*/
791         (unaryfunc)     0,      /*nb_invert*/
792         0,                              /*nb_lshift*/
793         (binaryfunc)0,  /*nb_rshift*/
794         0,                              /*nb_and*/
795         0,                              /*nb_xor*/
796         0,                              /*nb_or*/
797         0,                              /*nb_int*/
798         0,                              /*nb_reserved*/
799         0,                              /*nb_float*/
800         0,                              /* nb_inplace_add */
801         0,                              /* nb_inplace_subtract */
802         0,                              /* nb_inplace_multiply */
803         0,                              /* nb_inplace_remainder */
804         0,                              /* nb_inplace_power */
805         0,                              /* nb_inplace_lshift */
806         0,                              /* nb_inplace_rshift */
807         0,                              /* nb_inplace_and */
808         0,                              /* nb_inplace_xor */
809         0,                              /* nb_inplace_or */
810         0,                              /* nb_floor_divide */
811         0,                              /* nb_true_divide */
812         0,                              /* nb_inplace_floor_divide */
813         0,                              /* nb_inplace_true_divide */
814         0,                              /* nb_index */
815 };
816
817 static PyObject *Quaternion_getAxis( QuaternionObject *self, void *type )
818 {
819         return Quaternion_item(self, GET_INT_FROM_POINTER(type));
820 }
821
822 static int Quaternion_setAxis( QuaternionObject *self, PyObject *value, void *type )
823 {
824         return Quaternion_ass_item(self, GET_INT_FROM_POINTER(type), value);
825 }
826
827 static PyObject *Quaternion_getMagnitude(QuaternionObject *self, void *UNUSED(closure))
828 {
829         if(!BaseMath_ReadCallback(self))
830                 return NULL;
831
832         return PyFloat_FromDouble(sqrt(dot_qtqt(self->quat, self->quat)));
833 }
834
835 static PyObject *Quaternion_getAngle(QuaternionObject *self, void *UNUSED(closure))
836 {
837         float tquat[4];
838
839         if(!BaseMath_ReadCallback(self))
840                 return NULL;
841
842         normalize_qt_qt(tquat, self->quat);
843         return PyFloat_FromDouble(2.0 * (saacos(tquat[0])));
844 }
845
846 static int Quaternion_setAngle(QuaternionObject *self, PyObject *value, void *UNUSED(closure))
847 {
848         float tquat[4];
849         float len;
850
851         float axis[3], angle_dummy;
852         double angle;
853
854         if(!BaseMath_ReadCallback(self))
855                 return -1;
856
857         len= normalize_qt_qt(tquat, self->quat);
858         quat_to_axis_angle(axis, &angle_dummy, tquat);
859
860         angle= PyFloat_AsDouble(value);
861
862         if(angle==-1.0f && PyErr_Occurred()) { /* parsed item not a number */
863                 PyErr_SetString(PyExc_TypeError, "quaternion.angle = value: float expected");
864                 return -1;
865         }
866
867         angle= fmod(angle + M_PI*2, M_PI*4) - M_PI*2;
868
869         /* If the axis of rotation is 0,0,0 set it to 1,0,0 - for zero-degree rotations */
870         if( EXPP_FloatsAreEqual(axis[0], 0.0f, 10) &&
871                 EXPP_FloatsAreEqual(axis[1], 0.0f, 10) &&
872                 EXPP_FloatsAreEqual(axis[2], 0.0f, 10)
873         ) {
874                 axis[0] = 1.0f;
875         }
876
877         axis_angle_to_quat(self->quat, axis, angle);
878         mul_qt_fl(self->quat, len);
879
880         if(!BaseMath_WriteCallback(self))
881                 return -1;
882
883         return 0;
884 }
885
886 static PyObject *Quaternion_getAxisVec(QuaternionObject *self, void *UNUSED(closure))
887 {
888         float tquat[4];
889
890         float axis[3];
891         float angle;
892
893         if(!BaseMath_ReadCallback(self))
894                 return NULL;
895
896         normalize_qt_qt(tquat, self->quat);
897         quat_to_axis_angle(axis, &angle, tquat);
898
899         /* If the axis of rotation is 0,0,0 set it to 1,0,0 - for zero-degree rotations */
900         if( EXPP_FloatsAreEqual(axis[0], 0.0f, 10) &&
901                 EXPP_FloatsAreEqual(axis[1], 0.0f, 10) &&
902                 EXPP_FloatsAreEqual(axis[2], 0.0f, 10)
903         ) {
904                 axis[0] = 1.0f;
905         }
906
907         return (PyObject *) newVectorObject(axis, 3, Py_NEW, NULL);
908 }
909
910 static int Quaternion_setAxisVec(QuaternionObject *self, PyObject *value, void *UNUSED(closure))
911 {
912         float tquat[4];
913         float len;
914
915         float axis[3];
916         float angle;
917
918         if(!BaseMath_ReadCallback(self))
919                 return -1;
920
921         len= normalize_qt_qt(tquat, self->quat);
922         quat_to_axis_angle(axis, &angle, tquat); /* axis value is unused */
923
924         if (mathutils_array_parse(axis, 3, 3, value, "quat.axis = other") == -1)
925                 return -1;
926
927         axis_angle_to_quat(self->quat, axis, angle);
928         mul_qt_fl(self->quat, len);
929
930         if(!BaseMath_WriteCallback(self))
931                 return -1;
932
933         return 0;
934 }
935
936 //----------------------------------mathutils.Quaternion() --------------
937 static PyObject *Quaternion_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
938 {
939         PyObject *seq= NULL;
940         double angle = 0.0f;
941         float quat[QUAT_SIZE]= {0.0f, 0.0f, 0.0f, 0.0f};
942
943         if(kwds && PyDict_Size(kwds)) {
944                 PyErr_SetString(PyExc_TypeError, "mathutils.Quaternion(): takes no keyword args");
945                 return NULL;
946         }
947
948         if(!PyArg_ParseTuple(args, "|Od:mathutils.Quaternion", &seq, &angle))
949                 return NULL;
950
951         switch(PyTuple_GET_SIZE(args)) {
952         case 0:
953                 break;
954         case 1:
955                 if (mathutils_array_parse(quat, QUAT_SIZE, QUAT_SIZE, seq, "mathutils.Quaternion()") == -1)
956                         return NULL;
957                 break;
958         case 2:
959                 if (mathutils_array_parse(quat, 3, 3, seq, "mathutils.Quaternion()") == -1)
960                         return NULL;
961                 angle= fmod(angle + M_PI*2, M_PI*4) - M_PI*2; /* clamp because of precision issues */
962                 axis_angle_to_quat(quat, quat, angle);
963                 break;
964         /* PyArg_ParseTuple assures no more then 2 */
965         }
966         return newQuaternionObject(quat, Py_NEW, type);
967 }
968
969
970 //-----------------------METHOD DEFINITIONS ----------------------
971 static struct PyMethodDef Quaternion_methods[] = {
972         /* in place only */
973         {"identity", (PyCFunction) Quaternion_identity, METH_NOARGS, Quaternion_identity_doc},
974         {"negate", (PyCFunction) Quaternion_negate, METH_NOARGS, Quaternion_negate_doc},
975
976         /* operate on original or copy */
977         {"conjugate", (PyCFunction) Quaternion_conjugate, METH_NOARGS, Quaternion_conjugate_doc},
978         {"conjugated", (PyCFunction) Quaternion_conjugated, METH_NOARGS, Quaternion_conjugated_doc},
979
980         {"invert", (PyCFunction) Quaternion_invert, METH_NOARGS, Quaternion_invert_doc},
981         {"inverted", (PyCFunction) Quaternion_inverted, METH_NOARGS, Quaternion_inverted_doc},
982
983         {"normalize", (PyCFunction) Quaternion_normalize, METH_NOARGS, Quaternion_normalize_doc},
984         {"normalized", (PyCFunction) Quaternion_normalized, METH_NOARGS, Quaternion_normalized_doc},
985
986         /* return converted representation */
987         {"to_euler", (PyCFunction) Quaternion_to_euler, METH_VARARGS, Quaternion_to_euler_doc},
988         {"to_matrix", (PyCFunction) Quaternion_to_matrix, METH_NOARGS, Quaternion_to_matrix_doc},
989
990         /* operation between 2 or more types  */
991         {"cross", (PyCFunction) Quaternion_cross, METH_O, Quaternion_cross_doc},
992         {"dot", (PyCFunction) Quaternion_dot, METH_O, Quaternion_dot_doc},
993         {"difference", (PyCFunction) Quaternion_difference, METH_O, Quaternion_difference_doc},
994         {"slerp", (PyCFunction) Quaternion_slerp, METH_VARARGS, Quaternion_slerp_doc},
995         {"rotate", (PyCFunction) Quaternion_rotate, METH_VARARGS, Quaternion_rotate_doc},
996
997         {"__copy__", (PyCFunction) Quaternion_copy, METH_NOARGS, Quaternion_copy_doc},
998         {"copy", (PyCFunction) Quaternion_copy, METH_NOARGS, Quaternion_copy_doc},
999         {NULL, NULL, 0, NULL}
1000 };
1001
1002 /*****************************************************************************/
1003 /* Python attributes get/set structure:                                      */
1004 /*****************************************************************************/
1005 static PyGetSetDef Quaternion_getseters[] = {
1006         {(char *)"w", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion W value.\n\n:type: float", (void *)0},
1007         {(char *)"x", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion X axis.\n\n:type: float", (void *)1},
1008         {(char *)"y", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion Y axis.\n\n:type: float", (void *)2},
1009         {(char *)"z", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion Z axis.\n\n:type: float", (void *)3},
1010         {(char *)"magnitude", (getter)Quaternion_getMagnitude, (setter)NULL, (char *)"Size of the quaternion (readonly).\n\n:type: float", NULL},
1011         {(char *)"angle", (getter)Quaternion_getAngle, (setter)Quaternion_setAngle, (char *)"angle of the quaternion.\n\n:type: float", NULL},
1012         {(char *)"axis",(getter)Quaternion_getAxisVec, (setter)Quaternion_setAxisVec, (char *)"quaternion axis as a vector.\n\n:type: :class:`Vector`", NULL},
1013         {(char *)"is_wrapped", (getter)BaseMathObject_getWrapped, (setter)NULL, (char *)BaseMathObject_Wrapped_doc, NULL},
1014         {(char *)"owner", (getter)BaseMathObject_getOwner, (setter)NULL, (char *)BaseMathObject_Owner_doc, NULL},
1015         {NULL,NULL,NULL,NULL,NULL}  /* Sentinel */
1016 };
1017
1018 //------------------PY_OBECT DEFINITION--------------------------
1019 static char quaternion_doc[] =
1020 "This object gives access to Quaternions in Blender."
1021 ;
1022 PyTypeObject quaternion_Type = {
1023         PyVarObject_HEAD_INIT(NULL, 0)
1024         "mathutils.Quaternion",                                         //tp_name
1025         sizeof(QuaternionObject),                       //tp_basicsize
1026         0,                                                              //tp_itemsize
1027         (destructor)BaseMathObject_dealloc,             //tp_dealloc
1028         0,                                                              //tp_print
1029         0,                                                              //tp_getattr
1030         0,                                                              //tp_setattr
1031         0,                                                              //tp_compare
1032         (reprfunc) Quaternion_repr,             //tp_repr
1033         &Quaternion_NumMethods,                 //tp_as_number
1034         &Quaternion_SeqMethods,                 //tp_as_sequence
1035         &Quaternion_AsMapping,                  //tp_as_mapping
1036         0,                                                              //tp_hash
1037         0,                                                              //tp_call
1038         0,                                                              //tp_str
1039         0,                                                              //tp_getattro
1040         0,                                                              //tp_setattro
1041         0,                                                              //tp_as_buffer
1042         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, //tp_flags
1043         quaternion_doc, //tp_doc
1044         0,                                                              //tp_traverse
1045         0,                                                              //tp_clear
1046         (richcmpfunc)Quaternion_richcmpr,       //tp_richcompare
1047         0,                                                              //tp_weaklistoffset
1048         0,                                                              //tp_iter
1049         0,                                                              //tp_iternext
1050         Quaternion_methods,                             //tp_methods
1051         0,                                                              //tp_members
1052         Quaternion_getseters,                   //tp_getset
1053         0,                                                              //tp_base
1054         0,                                                              //tp_dict
1055         0,                                                              //tp_descr_get
1056         0,                                                              //tp_descr_set
1057         0,                                                              //tp_dictoffset
1058         0,                                                              //tp_init
1059         0,                                                              //tp_alloc
1060         Quaternion_new,                                 //tp_new
1061         0,                                                              //tp_free
1062         0,                                                              //tp_is_gc
1063         0,                                                              //tp_bases
1064         0,                                                              //tp_mro
1065         0,                                                              //tp_cache
1066         0,                                                              //tp_subclasses
1067         0,                                                              //tp_weaklist
1068         0                                                               //tp_del
1069 };
1070 //------------------------newQuaternionObject (internal)-------------
1071 //creates a new quaternion object
1072 /*pass Py_WRAP - if vector is a WRAPPER for data allocated by BLENDER
1073  (i.e. it was allocated elsewhere by MEM_mallocN())
1074   pass Py_NEW - if vector is not a WRAPPER and managed by PYTHON
1075  (i.e. it must be created here with PyMEM_malloc())*/
1076 PyObject *newQuaternionObject(float *quat, int type, PyTypeObject *base_type)
1077 {
1078         QuaternionObject *self;
1079
1080         if(base_type)   self = (QuaternionObject *)base_type->tp_alloc(base_type, 0);
1081         else                    self = PyObject_NEW(QuaternionObject, &quaternion_Type);
1082
1083         /* init callbacks as NULL */
1084         self->cb_user= NULL;
1085         self->cb_type= self->cb_subtype= 0;
1086
1087         if(type == Py_WRAP){
1088                 self->quat = quat;
1089                 self->wrapped = Py_WRAP;
1090         }else if (type == Py_NEW){
1091                 self->quat = PyMem_Malloc(QUAT_SIZE * sizeof(float));
1092                 if(!quat) { //new empty
1093                         unit_qt(self->quat);
1094                 }else{
1095                         QUATCOPY(self->quat, quat);
1096                 }
1097                 self->wrapped = Py_NEW;
1098         }else{ //bad type
1099                 return NULL;
1100         }
1101         return (PyObject *) self;
1102 }
1103
1104 PyObject *newQuaternionObject_cb(PyObject *cb_user, int cb_type, int cb_subtype)
1105 {
1106         QuaternionObject *self= (QuaternionObject *)newQuaternionObject(NULL, Py_NEW, NULL);
1107         if(self) {
1108                 Py_INCREF(cb_user);
1109                 self->cb_user=                  cb_user;
1110                 self->cb_type=                  (unsigned char)cb_type;
1111                 self->cb_subtype=               (unsigned char)cb_subtype;
1112         }
1113
1114         return (PyObject *)self;
1115 }
1116