c7d4fd186a424f2f5fbe52cea8e84c16ca3dc5e1
[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 //----------------------------Quaternion.normalize()----------------
260 //normalize the axis of rotation of [theta,vector]
261 static char Quaternion_normalize_doc[] =
262 ".. function:: normalize()\n"
263 "\n"
264 "   Normalize the quaternion.\n"
265 ;
266 static PyObject *Quaternion_normalize(QuaternionObject *self)
267 {
268         if(!BaseMath_ReadCallback(self))
269                 return NULL;
270
271         normalize_qt(self->quat);
272
273         (void)BaseMath_WriteCallback(self);
274         Py_RETURN_NONE;
275 }
276 static char Quaternion_normalized_doc[] =
277 ".. function:: normalized()\n"
278 "\n"
279 "   Return a new normalized quaternion.\n"
280 "\n"
281 "   :return: a normalized copy.\n"
282 "   :rtype: :class:`Quaternion`\n"
283 ;
284 static PyObject *Quaternion_normalized(QuaternionObject *self)
285 {
286         QUAT_APPLY_TO_COPY(Quaternion_normalize, self);
287 }
288
289 //----------------------------Quaternion.invert()------------------
290 static char Quaternion_invert_doc[] =
291 ".. function:: invert()\n"
292 "\n"
293 "   Set the quaternion to its inverse.\n"
294 ;
295 static PyObject *Quaternion_invert(QuaternionObject *self)
296 {
297         if(!BaseMath_ReadCallback(self))
298                 return NULL;
299
300         invert_qt(self->quat);
301
302         (void)BaseMath_WriteCallback(self);
303         Py_RETURN_NONE;
304 }
305 static char Quaternion_inverted_doc[] =
306 ".. function:: inverted()\n"
307 "\n"
308 "   Return a new, inverted quaternion.\n"
309 "\n"
310 "   :return: the inverted value.\n"
311 "   :rtype: :class:`Quaternion`\n"
312 ;
313 static PyObject *Quaternion_inverted(QuaternionObject *self)
314 {
315         QUAT_APPLY_TO_COPY(Quaternion_invert, self);
316 }
317
318 //----------------------------Quaternion.identity()-----------------
319 static char Quaternion_identity_doc[] =
320 ".. function:: identity()\n"
321 "\n"
322 "   Set the quaternion to an identity quaternion.\n"
323 "\n"
324 "   :return: an instance of itself.\n"
325 "   :rtype: :class:`Quaternion`\n"
326 ;
327 static PyObject *Quaternion_identity(QuaternionObject *self)
328 {
329         if(!BaseMath_ReadCallback(self))
330                 return NULL;
331
332         unit_qt(self->quat);
333
334         (void)BaseMath_WriteCallback(self);
335         Py_RETURN_NONE;
336 }
337 //----------------------------Quaternion.negate()-------------------
338 static char Quaternion_negate_doc[] =
339 ".. function:: negate()\n"
340 "\n"
341 "   Set the quaternion to its negative.\n"
342 "\n"
343 "   :return: an instance of itself.\n"
344 "   :rtype: :class:`Quaternion`\n"
345 ;
346 static PyObject *Quaternion_negate(QuaternionObject *self)
347 {
348         if(!BaseMath_ReadCallback(self))
349                 return NULL;
350
351         mul_qt_fl(self->quat, -1.0f);
352
353         (void)BaseMath_WriteCallback(self);
354         Py_RETURN_NONE;
355 }
356 //----------------------------Quaternion.conjugate()----------------
357 static char Quaternion_conjugate_doc[] =
358 ".. function:: conjugate()\n"
359 "\n"
360 "   Set the quaternion to its conjugate (negate x, y, z).\n"
361 ;
362 static PyObject *Quaternion_conjugate(QuaternionObject *self)
363 {
364         if(!BaseMath_ReadCallback(self))
365                 return NULL;
366
367         conjugate_qt(self->quat);
368
369         (void)BaseMath_WriteCallback(self);
370         Py_RETURN_NONE;
371 }
372 static char Quaternion_conjugated_doc[] =
373 ".. function:: conjugated()\n"
374 "\n"
375 "   Return a new conjugated quaternion.\n"
376 "\n"
377 "   :return: a new quaternion.\n"
378 "   :rtype: :class:`Quaternion`\n"
379 ;
380 static PyObject *Quaternion_conjugated(QuaternionObject *self)
381 {
382         QUAT_APPLY_TO_COPY(Quaternion_conjugate, self);
383 }
384
385 //----------------------------Quaternion.copy()----------------
386 static char Quaternion_copy_doc[] =
387 ".. function:: copy()\n"
388 "\n"
389 "   Returns a copy of this quaternion.\n"
390 "\n"
391 "   :return: A copy of the quaternion.\n"
392 "   :rtype: :class:`Quaternion`\n"
393 "\n"
394 "   .. note:: use this to get a copy of a wrapped quaternion with no reference to the original data.\n"
395 ;
396 static PyObject *Quaternion_copy(QuaternionObject *self)
397 {
398         if(!BaseMath_ReadCallback(self))
399                 return NULL;
400
401         return newQuaternionObject(self->quat, Py_NEW, Py_TYPE(self));
402 }
403
404 //----------------------------print object (internal)--------------
405 //print the object to screen
406 static PyObject *Quaternion_repr(QuaternionObject *self)
407 {
408         PyObject *ret, *tuple;
409
410         if(!BaseMath_ReadCallback(self))
411                 return NULL;
412
413         tuple= Quaternion_to_tuple_ext(self, -1);
414
415         ret= PyUnicode_FromFormat("Quaternion(%R)", tuple);
416
417         Py_DECREF(tuple);
418         return ret;
419 }
420
421 //------------------------tp_richcmpr
422 //returns -1 execption, 0 false, 1 true
423 static PyObject* Quaternion_richcmpr(PyObject *objectA, PyObject *objectB, int comparison_type)
424 {
425         QuaternionObject *quatA = NULL, *quatB = NULL;
426         int result = 0;
427
428         if(QuaternionObject_Check(objectA)) {
429                 quatA = (QuaternionObject*)objectA;
430                 if(!BaseMath_ReadCallback(quatA))
431                         return NULL;
432         }
433         if(QuaternionObject_Check(objectB)) {
434                 quatB = (QuaternionObject*)objectB;
435                 if(!BaseMath_ReadCallback(quatB))
436                         return NULL;
437         }
438
439         if (!quatA || !quatB){
440                 if (comparison_type == Py_NE){
441                         Py_RETURN_TRUE;
442                 }else{
443                         Py_RETURN_FALSE;
444                 }
445         }
446
447         switch (comparison_type){
448                 case Py_EQ:
449                         result = EXPP_VectorsAreEqual(quatA->quat, quatB->quat, QUAT_SIZE, 1);
450                         break;
451                 case Py_NE:
452                         result = EXPP_VectorsAreEqual(quatA->quat, quatB->quat, QUAT_SIZE, 1);
453                         if (result == 0){
454                                 result = 1;
455                         }else{
456                                 result = 0;
457                         }
458                         break;
459                 default:
460                         printf("The result of the comparison could not be evaluated");
461                         break;
462         }
463         if (result == 1){
464                 Py_RETURN_TRUE;
465         }else{
466                 Py_RETURN_FALSE;
467         }
468 }
469
470 //---------------------SEQUENCE PROTOCOLS------------------------
471 //----------------------------len(object)------------------------
472 //sequence length
473 static int Quaternion_len(QuaternionObject *UNUSED(self))
474 {
475         return QUAT_SIZE;
476 }
477 //----------------------------object[]---------------------------
478 //sequence accessor (get)
479 static PyObject *Quaternion_item(QuaternionObject *self, int i)
480 {
481         if(i<0) i= QUAT_SIZE-i;
482
483         if(i < 0 || i >= QUAT_SIZE) {
484                 PyErr_SetString(PyExc_IndexError, "quaternion[attribute]: array index out of range");
485                 return NULL;
486         }
487
488         if(!BaseMath_ReadIndexCallback(self, i))
489                 return NULL;
490
491         return PyFloat_FromDouble(self->quat[i]);
492
493 }
494 //----------------------------object[]-------------------------
495 //sequence accessor (set)
496 static int Quaternion_ass_item(QuaternionObject *self, int i, PyObject *ob)
497 {
498         float scalar= (float)PyFloat_AsDouble(ob);
499         if(scalar==-1.0f && PyErr_Occurred()) { /* parsed item not a number */
500                 PyErr_SetString(PyExc_TypeError, "quaternion[index] = x: index argument not a number");
501                 return -1;
502         }
503
504         if(i<0) i= QUAT_SIZE-i;
505
506         if(i < 0 || i >= QUAT_SIZE){
507                 PyErr_SetString(PyExc_IndexError, "quaternion[attribute] = x: array assignment index out of range");
508                 return -1;
509         }
510         self->quat[i] = scalar;
511
512         if(!BaseMath_WriteIndexCallback(self, i))
513                 return -1;
514
515         return 0;
516 }
517 //----------------------------object[z:y]------------------------
518 //sequence slice (get)
519 static PyObject *Quaternion_slice(QuaternionObject *self, int begin, int end)
520 {
521         PyObject *tuple;
522         int count;
523
524         if(!BaseMath_ReadCallback(self))
525                 return NULL;
526
527         CLAMP(begin, 0, QUAT_SIZE);
528         if (end<0) end= (QUAT_SIZE + 1) + end;
529         CLAMP(end, 0, QUAT_SIZE);
530         begin= MIN2(begin, end);
531
532         tuple= PyTuple_New(end - begin);
533         for(count= begin; count < end; count++) {
534                 PyTuple_SET_ITEM(tuple, count - begin, PyFloat_FromDouble(self->quat[count]));
535         }
536
537         return tuple;
538 }
539 //----------------------------object[z:y]------------------------
540 //sequence slice (set)
541 static int Quaternion_ass_slice(QuaternionObject *self, int begin, int end, PyObject *seq)
542 {
543         int i, size;
544         float quat[QUAT_SIZE];
545
546         if(!BaseMath_ReadCallback(self))
547                 return -1;
548
549         CLAMP(begin, 0, QUAT_SIZE);
550         if (end<0) end= (QUAT_SIZE + 1) + end;
551         CLAMP(end, 0, QUAT_SIZE);
552         begin = MIN2(begin,end);
553
554         if((size=mathutils_array_parse(quat, 0, QUAT_SIZE, seq, "mathutils.Quaternion[begin:end] = []")) == -1)
555                 return -1;
556
557         if(size != (end - begin)){
558                 PyErr_SetString(PyExc_TypeError, "quaternion[begin:end] = []: size mismatch in slice assignment");
559                 return -1;
560         }
561
562         /* parsed well - now set in vector */
563         for(i= 0; i < size; i++)
564                 self->quat[begin + i] = quat[i];
565
566         (void)BaseMath_WriteCallback(self);
567         return 0;
568 }
569
570
571 static PyObject *Quaternion_subscript(QuaternionObject *self, PyObject *item)
572 {
573         if (PyIndex_Check(item)) {
574                 Py_ssize_t i;
575                 i = PyNumber_AsSsize_t(item, PyExc_IndexError);
576                 if (i == -1 && PyErr_Occurred())
577                         return NULL;
578                 if (i < 0)
579                         i += QUAT_SIZE;
580                 return Quaternion_item(self, i);
581         } else if (PySlice_Check(item)) {
582                 Py_ssize_t start, stop, step, slicelength;
583
584                 if (PySlice_GetIndicesEx((void *)item, QUAT_SIZE, &start, &stop, &step, &slicelength) < 0)
585                         return NULL;
586
587                 if (slicelength <= 0) {
588                         return PyTuple_New(0);
589                 }
590                 else if (step == 1) {
591                         return Quaternion_slice(self, start, stop);
592                 }
593                 else {
594                         PyErr_SetString(PyExc_TypeError, "slice steps not supported with quaternions");
595                         return NULL;
596                 }
597         }
598         else {
599                 PyErr_Format(PyExc_TypeError, "quaternion indices must be integers, not %.200s", Py_TYPE(item)->tp_name);
600                 return NULL;
601         }
602 }
603
604
605 static int Quaternion_ass_subscript(QuaternionObject *self, PyObject *item, PyObject *value)
606 {
607         if (PyIndex_Check(item)) {
608                 Py_ssize_t i = PyNumber_AsSsize_t(item, PyExc_IndexError);
609                 if (i == -1 && PyErr_Occurred())
610                         return -1;
611                 if (i < 0)
612                         i += QUAT_SIZE;
613                 return Quaternion_ass_item(self, i, value);
614         }
615         else if (PySlice_Check(item)) {
616                 Py_ssize_t start, stop, step, slicelength;
617
618                 if (PySlice_GetIndicesEx((void *)item, QUAT_SIZE, &start, &stop, &step, &slicelength) < 0)
619                         return -1;
620
621                 if (step == 1)
622                         return Quaternion_ass_slice(self, start, stop, value);
623                 else {
624                         PyErr_SetString(PyExc_TypeError, "slice steps not supported with quaternion");
625                         return -1;
626                 }
627         }
628         else {
629                 PyErr_Format(PyExc_TypeError, "quaternion indices must be integers, not %.200s", Py_TYPE(item)->tp_name);
630                 return -1;
631         }
632 }
633
634 //------------------------NUMERIC PROTOCOLS----------------------
635 //------------------------obj + obj------------------------------
636 //addition
637 static PyObject *Quaternion_add(PyObject *q1, PyObject *q2)
638 {
639         float quat[QUAT_SIZE];
640         QuaternionObject *quat1 = NULL, *quat2 = NULL;
641
642         if(!QuaternionObject_Check(q1) || !QuaternionObject_Check(q2)) {
643                 PyErr_SetString(PyExc_AttributeError, "Quaternion addition: arguments not valid for this operation");
644                 return NULL;
645         }
646         quat1 = (QuaternionObject*)q1;
647         quat2 = (QuaternionObject*)q2;
648
649         if(!BaseMath_ReadCallback(quat1) || !BaseMath_ReadCallback(quat2))
650                 return NULL;
651
652         add_qt_qtqt(quat, quat1->quat, quat2->quat, 1.0f);
653         return newQuaternionObject(quat, Py_NEW, Py_TYPE(q1));
654 }
655 //------------------------obj - obj------------------------------
656 //subtraction
657 static PyObject *Quaternion_sub(PyObject *q1, PyObject *q2)
658 {
659         int x;
660         float quat[QUAT_SIZE];
661         QuaternionObject *quat1 = NULL, *quat2 = NULL;
662
663         if(!QuaternionObject_Check(q1) || !QuaternionObject_Check(q2)) {
664                 PyErr_SetString(PyExc_AttributeError, "Quaternion addition: arguments not valid for this operation");
665                 return NULL;
666         }
667
668         quat1 = (QuaternionObject*)q1;
669         quat2 = (QuaternionObject*)q2;
670
671         if(!BaseMath_ReadCallback(quat1) || !BaseMath_ReadCallback(quat2))
672                 return NULL;
673
674         for(x = 0; x < QUAT_SIZE; x++) {
675                 quat[x] = quat1->quat[x] - quat2->quat[x];
676         }
677
678         return newQuaternionObject(quat, Py_NEW, Py_TYPE(q1));
679 }
680
681 static PyObject *quat_mul_float(QuaternionObject *quat, const float scalar)
682 {
683         float tquat[4];
684         copy_qt_qt(tquat, quat->quat);
685         mul_qt_fl(tquat, scalar);
686         return newQuaternionObject(tquat, Py_NEW, Py_TYPE(quat));
687 }
688
689 //------------------------obj * obj------------------------------
690 //mulplication
691 static PyObject *Quaternion_mul(PyObject *q1, PyObject *q2)
692 {
693         float quat[QUAT_SIZE], scalar;
694         QuaternionObject *quat1 = NULL, *quat2 = NULL;
695
696         if(QuaternionObject_Check(q1)) {
697                 quat1 = (QuaternionObject*)q1;
698                 if(!BaseMath_ReadCallback(quat1))
699                         return NULL;
700         }
701         if(QuaternionObject_Check(q2)) {
702                 quat2 = (QuaternionObject*)q2;
703                 if(!BaseMath_ReadCallback(quat2))
704                         return NULL;
705         }
706
707         if(quat1 && quat2) { /* QUAT*QUAT (cross product) */
708                 mul_qt_qtqt(quat, quat1->quat, quat2->quat);
709                 return newQuaternionObject(quat, Py_NEW, Py_TYPE(q1));
710         }
711         /* the only case this can happen (for a supported type is "FLOAT*QUAT" ) */
712         else if(quat2) { /* FLOAT*QUAT */
713                 if(((scalar= PyFloat_AsDouble(q1)) == -1.0 && PyErr_Occurred())==0) {
714                         return quat_mul_float(quat2, scalar);
715                 }
716         }
717         else if (quat1) { /* QUAT*FLOAT */
718                 if((((scalar= PyFloat_AsDouble(q2)) == -1.0 && PyErr_Occurred())==0)) {
719                         return quat_mul_float(quat1, scalar);
720                 }
721         }
722         else {
723                 BLI_assert(!"internal error");
724         }
725
726         PyErr_Format(PyExc_TypeError, "Quaternion multiplication: not supported between '%.200s' and '%.200s' types", Py_TYPE(q1)->tp_name, Py_TYPE(q2)->tp_name);
727         return NULL;
728 }
729
730 //-----------------PROTOCOL DECLARATIONS--------------------------
731 static PySequenceMethods Quaternion_SeqMethods = {
732         (lenfunc) Quaternion_len,                               /* sq_length */
733         (binaryfunc) NULL,                                              /* sq_concat */
734         (ssizeargfunc) NULL,                                    /* sq_repeat */
735         (ssizeargfunc) Quaternion_item,                 /* sq_item */
736         (ssizessizeargfunc) NULL,                               /* sq_slice, deprecated */
737         (ssizeobjargproc) Quaternion_ass_item,  /* sq_ass_item */
738         (ssizessizeobjargproc) NULL,                    /* sq_ass_slice, deprecated */
739         (objobjproc) NULL,                                              /* sq_contains */
740         (binaryfunc) NULL,                                              /* sq_inplace_concat */
741         (ssizeargfunc) NULL,                                    /* sq_inplace_repeat */
742 };
743
744 static PyMappingMethods Quaternion_AsMapping = {
745         (lenfunc)Quaternion_len,
746         (binaryfunc)Quaternion_subscript,
747         (objobjargproc)Quaternion_ass_subscript
748 };
749
750 static PyNumberMethods Quaternion_NumMethods = {
751         (binaryfunc)    Quaternion_add, /*nb_add*/
752         (binaryfunc)    Quaternion_sub, /*nb_subtract*/
753         (binaryfunc)    Quaternion_mul, /*nb_multiply*/
754         0,                                                      /*nb_remainder*/
755         0,                                                      /*nb_divmod*/
756         0,                                                      /*nb_power*/
757         (unaryfunc)     0,      /*nb_negative*/
758         (unaryfunc)     0,      /*tp_positive*/
759         (unaryfunc)     0,      /*tp_absolute*/
760         (inquiry)       0,      /*tp_bool*/
761         (unaryfunc)     0,      /*nb_invert*/
762         0,                              /*nb_lshift*/
763         (binaryfunc)0,  /*nb_rshift*/
764         0,                              /*nb_and*/
765         0,                              /*nb_xor*/
766         0,                              /*nb_or*/
767         0,                              /*nb_int*/
768         0,                              /*nb_reserved*/
769         0,                              /*nb_float*/
770         0,                              /* nb_inplace_add */
771         0,                              /* nb_inplace_subtract */
772         0,                              /* nb_inplace_multiply */
773         0,                              /* nb_inplace_remainder */
774         0,                              /* nb_inplace_power */
775         0,                              /* nb_inplace_lshift */
776         0,                              /* nb_inplace_rshift */
777         0,                              /* nb_inplace_and */
778         0,                              /* nb_inplace_xor */
779         0,                              /* nb_inplace_or */
780         0,                              /* nb_floor_divide */
781         0,                              /* nb_true_divide */
782         0,                              /* nb_inplace_floor_divide */
783         0,                              /* nb_inplace_true_divide */
784         0,                              /* nb_index */
785 };
786
787 static PyObject *Quaternion_getAxis( QuaternionObject *self, void *type )
788 {
789         return Quaternion_item(self, GET_INT_FROM_POINTER(type));
790 }
791
792 static int Quaternion_setAxis( QuaternionObject *self, PyObject *value, void *type )
793 {
794         return Quaternion_ass_item(self, GET_INT_FROM_POINTER(type), value);
795 }
796
797 static PyObject *Quaternion_getMagnitude(QuaternionObject *self, void *UNUSED(closure))
798 {
799         if(!BaseMath_ReadCallback(self))
800                 return NULL;
801
802         return PyFloat_FromDouble(sqrt(dot_qtqt(self->quat, self->quat)));
803 }
804
805 static PyObject *Quaternion_getAngle(QuaternionObject *self, void *UNUSED(closure))
806 {
807         float tquat[4];
808
809         if(!BaseMath_ReadCallback(self))
810                 return NULL;
811
812         normalize_qt_qt(tquat, self->quat);
813         return PyFloat_FromDouble(2.0 * (saacos(tquat[0])));
814 }
815
816 static int Quaternion_setAngle(QuaternionObject *self, PyObject *value, void *UNUSED(closure))
817 {
818         float tquat[4];
819         float len;
820
821         float axis[3], angle_dummy;
822         double angle;
823
824         if(!BaseMath_ReadCallback(self))
825                 return -1;
826
827         len= normalize_qt_qt(tquat, self->quat);
828         quat_to_axis_angle(axis, &angle_dummy, tquat);
829
830         angle= PyFloat_AsDouble(value);
831
832         if(angle==-1.0f && PyErr_Occurred()) { /* parsed item not a number */
833                 PyErr_SetString(PyExc_TypeError, "quaternion.angle = value: float expected");
834                 return -1;
835         }
836
837         angle= fmod(angle + M_PI*2, M_PI*4) - M_PI*2;
838
839         /* If the axis of rotation is 0,0,0 set it to 1,0,0 - for zero-degree rotations */
840         if( EXPP_FloatsAreEqual(axis[0], 0.0f, 10) &&
841                 EXPP_FloatsAreEqual(axis[1], 0.0f, 10) &&
842                 EXPP_FloatsAreEqual(axis[2], 0.0f, 10)
843         ) {
844                 axis[0] = 1.0f;
845         }
846
847         axis_angle_to_quat(self->quat, axis, angle);
848         mul_qt_fl(self->quat, len);
849
850         if(!BaseMath_WriteCallback(self))
851                 return -1;
852
853         return 0;
854 }
855
856 static PyObject *Quaternion_getAxisVec(QuaternionObject *self, void *UNUSED(closure))
857 {
858         float tquat[4];
859
860         float axis[3];
861         float angle;
862
863         if(!BaseMath_ReadCallback(self))
864                 return NULL;
865
866         normalize_qt_qt(tquat, self->quat);
867         quat_to_axis_angle(axis, &angle, tquat);
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         return (PyObject *) newVectorObject(axis, 3, Py_NEW, NULL);
878 }
879
880 static int Quaternion_setAxisVec(QuaternionObject *self, PyObject *value, void *UNUSED(closure))
881 {
882         float tquat[4];
883         float len;
884
885         float axis[3];
886         float angle;
887
888         if(!BaseMath_ReadCallback(self))
889                 return -1;
890
891         len= normalize_qt_qt(tquat, self->quat);
892         quat_to_axis_angle(axis, &angle, tquat); /* axis value is unused */
893
894         if (mathutils_array_parse(axis, 3, 3, value, "quat.axis = other") == -1)
895                 return -1;
896
897         axis_angle_to_quat(self->quat, axis, angle);
898         mul_qt_fl(self->quat, len);
899
900         if(!BaseMath_WriteCallback(self))
901                 return -1;
902
903         return 0;
904 }
905
906 //----------------------------------mathutils.Quaternion() --------------
907 static PyObject *Quaternion_new(PyTypeObject *type, PyObject *args, PyObject *kwds)
908 {
909         PyObject *seq= NULL;
910         double angle = 0.0f;
911         float quat[QUAT_SIZE]= {0.0f, 0.0f, 0.0f, 0.0f};
912
913         if(kwds && PyDict_Size(kwds)) {
914                 PyErr_SetString(PyExc_TypeError, "mathutils.Quaternion(): takes no keyword args");
915                 return NULL;
916         }
917
918         if(!PyArg_ParseTuple(args, "|Od:mathutils.Quaternion", &seq, &angle))
919                 return NULL;
920
921         switch(PyTuple_GET_SIZE(args)) {
922         case 0:
923                 break;
924         case 1:
925                 if (mathutils_array_parse(quat, QUAT_SIZE, QUAT_SIZE, seq, "mathutils.Quaternion()") == -1)
926                         return NULL;
927                 break;
928         case 2:
929                 if (mathutils_array_parse(quat, 3, 3, seq, "mathutils.Quaternion()") == -1)
930                         return NULL;
931                 angle= fmod(angle + M_PI*2, M_PI*4) - M_PI*2; /* clamp because of precision issues */
932                 axis_angle_to_quat(quat, quat, angle);
933                 break;
934         /* PyArg_ParseTuple assures no more then 2 */
935         }
936         return newQuaternionObject(quat, Py_NEW, type);
937 }
938
939
940 //-----------------------METHOD DEFINITIONS ----------------------
941 static struct PyMethodDef Quaternion_methods[] = {
942         /* in place only */
943         {"identity", (PyCFunction) Quaternion_identity, METH_NOARGS, Quaternion_identity_doc},
944         {"negate", (PyCFunction) Quaternion_negate, METH_NOARGS, Quaternion_negate_doc},
945
946         /* operate on original or copy */
947         {"conjugate", (PyCFunction) Quaternion_conjugate, METH_NOARGS, Quaternion_conjugate_doc},
948         {"conjugated", (PyCFunction) Quaternion_conjugated, METH_NOARGS, Quaternion_conjugated_doc},
949
950         {"invert", (PyCFunction) Quaternion_invert, METH_NOARGS, Quaternion_invert_doc},
951         {"inverted", (PyCFunction) Quaternion_inverted, METH_NOARGS, Quaternion_inverted_doc},
952
953         {"normalize", (PyCFunction) Quaternion_normalize, METH_NOARGS, Quaternion_normalize_doc},
954         {"normalized", (PyCFunction) Quaternion_normalized, METH_NOARGS, Quaternion_normalized_doc},
955
956         /* return converted representation */
957         {"to_euler", (PyCFunction) Quaternion_to_euler, METH_VARARGS, Quaternion_to_euler_doc},
958         {"to_matrix", (PyCFunction) Quaternion_to_matrix, METH_NOARGS, Quaternion_to_matrix_doc},
959
960         /* operation between 2 or more types  */
961         {"cross", (PyCFunction) Quaternion_cross, METH_O, Quaternion_cross_doc},
962         {"dot", (PyCFunction) Quaternion_dot, METH_O, Quaternion_dot_doc},
963         {"difference", (PyCFunction) Quaternion_difference, METH_O, Quaternion_difference_doc},
964         {"slerp", (PyCFunction) Quaternion_slerp, METH_VARARGS, Quaternion_slerp_doc},
965
966         {"__copy__", (PyCFunction) Quaternion_copy, METH_NOARGS, Quaternion_copy_doc},
967         {"copy", (PyCFunction) Quaternion_copy, METH_NOARGS, Quaternion_copy_doc},
968         {NULL, NULL, 0, NULL}
969 };
970
971 /*****************************************************************************/
972 /* Python attributes get/set structure:                                      */
973 /*****************************************************************************/
974 static PyGetSetDef Quaternion_getseters[] = {
975         {(char *)"w", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion W value.\n\n:type: float", (void *)0},
976         {(char *)"x", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion X axis.\n\n:type: float", (void *)1},
977         {(char *)"y", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion Y axis.\n\n:type: float", (void *)2},
978         {(char *)"z", (getter)Quaternion_getAxis, (setter)Quaternion_setAxis, (char *)"Quaternion Z axis.\n\n:type: float", (void *)3},
979         {(char *)"magnitude", (getter)Quaternion_getMagnitude, (setter)NULL, (char *)"Size of the quaternion (readonly).\n\n:type: float", NULL},
980         {(char *)"angle", (getter)Quaternion_getAngle, (setter)Quaternion_setAngle, (char *)"angle of the quaternion.\n\n:type: float", NULL},
981         {(char *)"axis",(getter)Quaternion_getAxisVec, (setter)Quaternion_setAxisVec, (char *)"quaternion axis as a vector.\n\n:type: :class:`Vector`", NULL},
982         {(char *)"is_wrapped", (getter)BaseMathObject_getWrapped, (setter)NULL, (char *)BaseMathObject_Wrapped_doc, NULL},
983         {(char *)"owner", (getter)BaseMathObject_getOwner, (setter)NULL, (char *)BaseMathObject_Owner_doc, NULL},
984         {NULL,NULL,NULL,NULL,NULL}  /* Sentinel */
985 };
986
987 //------------------PY_OBECT DEFINITION--------------------------
988 static char quaternion_doc[] =
989 "This object gives access to Quaternions in Blender."
990 ;
991 PyTypeObject quaternion_Type = {
992         PyVarObject_HEAD_INIT(NULL, 0)
993         "mathutils.Quaternion",                                         //tp_name
994         sizeof(QuaternionObject),                       //tp_basicsize
995         0,                                                              //tp_itemsize
996         (destructor)BaseMathObject_dealloc,             //tp_dealloc
997         0,                                                              //tp_print
998         0,                                                              //tp_getattr
999         0,                                                              //tp_setattr
1000         0,                                                              //tp_compare
1001         (reprfunc) Quaternion_repr,             //tp_repr
1002         &Quaternion_NumMethods,                 //tp_as_number
1003         &Quaternion_SeqMethods,                 //tp_as_sequence
1004         &Quaternion_AsMapping,                  //tp_as_mapping
1005         0,                                                              //tp_hash
1006         0,                                                              //tp_call
1007         0,                                                              //tp_str
1008         0,                                                              //tp_getattro
1009         0,                                                              //tp_setattro
1010         0,                                                              //tp_as_buffer
1011         Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, //tp_flags
1012         quaternion_doc, //tp_doc
1013         0,                                                              //tp_traverse
1014         0,                                                              //tp_clear
1015         (richcmpfunc)Quaternion_richcmpr,       //tp_richcompare
1016         0,                                                              //tp_weaklistoffset
1017         0,                                                              //tp_iter
1018         0,                                                              //tp_iternext
1019         Quaternion_methods,                             //tp_methods
1020         0,                                                              //tp_members
1021         Quaternion_getseters,                   //tp_getset
1022         0,                                                              //tp_base
1023         0,                                                              //tp_dict
1024         0,                                                              //tp_descr_get
1025         0,                                                              //tp_descr_set
1026         0,                                                              //tp_dictoffset
1027         0,                                                              //tp_init
1028         0,                                                              //tp_alloc
1029         Quaternion_new,                                 //tp_new
1030         0,                                                              //tp_free
1031         0,                                                              //tp_is_gc
1032         0,                                                              //tp_bases
1033         0,                                                              //tp_mro
1034         0,                                                              //tp_cache
1035         0,                                                              //tp_subclasses
1036         0,                                                              //tp_weaklist
1037         0                                                               //tp_del
1038 };
1039 //------------------------newQuaternionObject (internal)-------------
1040 //creates a new quaternion object
1041 /*pass Py_WRAP - if vector is a WRAPPER for data allocated by BLENDER
1042  (i.e. it was allocated elsewhere by MEM_mallocN())
1043   pass Py_NEW - if vector is not a WRAPPER and managed by PYTHON
1044  (i.e. it must be created here with PyMEM_malloc())*/
1045 PyObject *newQuaternionObject(float *quat, int type, PyTypeObject *base_type)
1046 {
1047         QuaternionObject *self;
1048
1049         if(base_type)   self = (QuaternionObject *)base_type->tp_alloc(base_type, 0);
1050         else                    self = PyObject_NEW(QuaternionObject, &quaternion_Type);
1051
1052         /* init callbacks as NULL */
1053         self->cb_user= NULL;
1054         self->cb_type= self->cb_subtype= 0;
1055
1056         if(type == Py_WRAP){
1057                 self->quat = quat;
1058                 self->wrapped = Py_WRAP;
1059         }else if (type == Py_NEW){
1060                 self->quat = PyMem_Malloc(QUAT_SIZE * sizeof(float));
1061                 if(!quat) { //new empty
1062                         unit_qt(self->quat);
1063                 }else{
1064                         QUATCOPY(self->quat, quat);
1065                 }
1066                 self->wrapped = Py_NEW;
1067         }else{ //bad type
1068                 return NULL;
1069         }
1070         return (PyObject *) self;
1071 }
1072
1073 PyObject *newQuaternionObject_cb(PyObject *cb_user, int cb_type, int cb_subtype)
1074 {
1075         QuaternionObject *self= (QuaternionObject *)newQuaternionObject(NULL, Py_NEW, NULL);
1076         if(self) {
1077                 Py_INCREF(cb_user);
1078                 self->cb_user=                  cb_user;
1079                 self->cb_type=                  (unsigned char)cb_type;
1080                 self->cb_subtype=               (unsigned char)cb_subtype;
1081         }
1082
1083         return (PyObject *)self;
1084 }
1085