a6974ef5086c1dad208d2a7851c67aebc8ee9ace
[blender.git] / source / blender / python / api2_2x / quat.c
1 /*
2  * $Id$
3  *
4  * ***** BEGIN GPL/BL DUAL 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. The Blender
10  * Foundation also sells licenses for use in proprietary software under
11  * the Blender License.  See http://www.blender.org/BL/ for information
12  * about this.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software Foundation,
21  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22  *
23  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
24  * All rights reserved.
25  *
26  * 
27  * Contributor(s): Joseph Gilbert
28  *
29  * ***** END GPL/BL DUAL LICENSE BLOCK *****
30  */
31
32 #include "quat.h"
33
34 //doc strings
35 char Quaternion_Identity_doc[] =
36         "() - set the quaternion to it's identity (1, vector)";
37 char Quaternion_Negate_doc[] =
38         "() - set all values in the quaternion to their negative";
39 char Quaternion_Conjugate_doc[] = "() - set the quaternion to it's conjugate";
40 char Quaternion_Inverse_doc[] = "() - set the quaternion to it's inverse";
41 char Quaternion_Normalize_doc[] =
42         "() - normalize the vector portion of the quaternion";
43 char Quaternion_ToEuler_doc[] =
44         "() - return a euler rotation representing the quaternion";
45 char Quaternion_ToMatrix_doc[] =
46         "() - return a rotation matrix representing the quaternion";
47
48 //methods table
49 struct PyMethodDef Quaternion_methods[] = {
50         {"identity", ( PyCFunction ) Quaternion_Identity, METH_NOARGS,
51          Quaternion_Identity_doc},
52         {"negate", ( PyCFunction ) Quaternion_Negate, METH_NOARGS,
53          Quaternion_Negate_doc},
54         {"conjugate", ( PyCFunction ) Quaternion_Conjugate, METH_NOARGS,
55          Quaternion_Conjugate_doc},
56         {"inverse", ( PyCFunction ) Quaternion_Inverse, METH_NOARGS,
57          Quaternion_Inverse_doc},
58         {"normalize", ( PyCFunction ) Quaternion_Normalize, METH_NOARGS,
59          Quaternion_Normalize_doc},
60         {"toEuler", ( PyCFunction ) Quaternion_ToEuler, METH_NOARGS,
61          Quaternion_ToEuler_doc},
62         {"toMatrix", ( PyCFunction ) Quaternion_ToMatrix, METH_NOARGS,
63          Quaternion_ToMatrix_doc},
64         {NULL, NULL, 0, NULL}
65 };
66
67 /*****************************/
68 //    Quaternion Python Object   
69 /*****************************/
70
71 PyObject *Quaternion_ToEuler( QuaternionObject * self )
72 {
73         float *eul;
74         int x;
75
76         eul = PyMem_Malloc( 3 * sizeof( float ) );
77         QuatToEul( self->quat, eul );
78
79         for( x = 0; x < 3; x++ ) {
80                 eul[x] *= ( float ) ( 180 / Py_PI );
81         }
82         return ( PyObject * ) newEulerObject( eul );
83 }
84
85 PyObject *Quaternion_ToMatrix( QuaternionObject * self )
86 {
87         float *mat;
88
89         mat = PyMem_Malloc( 3 * 3 * sizeof( float ) );
90         QuatToMat3( self->quat, ( float ( * )[3] ) mat );
91
92         return ( PyObject * ) newMatrixObject( mat, 3, 3 );
93 }
94
95 //normalize the axis of rotation of [theta,vector]
96 PyObject *Quaternion_Normalize( QuaternionObject * self )
97 {
98         NormalQuat( self->quat );
99         return EXPP_incr_ret( Py_None );
100 }
101
102 PyObject *Quaternion_Inverse( QuaternionObject * self )
103 {
104         float mag = 0.0f;
105         int x;
106
107         for( x = 1; x < 4; x++ ) {
108                 self->quat[x] = -self->quat[x];
109         }
110         for( x = 0; x < 4; x++ ) {
111                 mag += ( self->quat[x] * self->quat[x] );
112         }
113         mag = ( float ) sqrt( mag );
114         for( x = 0; x < 4; x++ ) {
115                 self->quat[x] /= ( mag * mag );
116         }
117
118         return EXPP_incr_ret( Py_None );
119 }
120
121 PyObject *Quaternion_Identity( QuaternionObject * self )
122 {
123         self->quat[0] = 1.0;
124         self->quat[1] = 0.0;
125         self->quat[2] = 0.0;
126         self->quat[3] = 0.0;
127
128         return EXPP_incr_ret( Py_None );
129 }
130
131 PyObject *Quaternion_Negate( QuaternionObject * self )
132 {
133         int x;
134
135         for( x = 0; x < 4; x++ ) {
136                 self->quat[x] = -self->quat[x];
137         }
138         return EXPP_incr_ret( Py_None );
139 }
140
141 PyObject *Quaternion_Conjugate( QuaternionObject * self )
142 {
143         int x;
144
145         for( x = 1; x < 4; x++ ) {
146                 self->quat[x] = -self->quat[x];
147         }
148         return EXPP_incr_ret( Py_None );
149 }
150
151 static void Quaternion_dealloc( QuaternionObject * self )
152 {
153         PyMem_Free( self->quat );
154         PyObject_DEL( self );
155 }
156
157 static PyObject *Quaternion_getattr( QuaternionObject * self, char *name )
158 {
159         double mag = 0.0f;
160         float *vec = NULL;
161         int x;
162         PyObject *retval;
163
164         if( ELEM4( name[0], 'w', 'x', 'y', 'z' ) && name[1] == 0 ) {
165                 return PyFloat_FromDouble( self->quat[name[0] - 'w'] );
166         }
167         if( strcmp( name, "magnitude" ) == 0 ) {
168                 for( x = 0; x < 4; x++ ) {
169                         mag += self->quat[x] * self->quat[x];
170                 }
171                 mag = ( float ) sqrt( mag );
172                 return PyFloat_FromDouble( mag );
173         }
174         if( strcmp( name, "angle" ) == 0 ) {
175
176                 mag = self->quat[0];
177                 mag = 2 * ( acos( mag ) );
178                 mag *= ( 180 / Py_PI );
179                 return PyFloat_FromDouble( mag );
180         }
181         if( strcmp( name, "axis" ) == 0 ) {
182
183                 mag = ( double ) ( self->quat[0] * ( Py_PI / 180 ) );
184                 mag = 2 * ( acos( mag ) );
185                 mag = sin( mag / 2 );
186                 vec = PyMem_Malloc( 3 * sizeof( float ) );
187                 for( x = 0; x < 3; x++ ) {
188                         vec[x] = ( self->quat[x + 1] / ( ( float ) ( mag ) ) );
189                 }
190                 Normalise( vec );
191                 retval = ( PyObject * ) newVectorObject( vec, 3 );
192                 PyMem_Free( vec );
193                 return retval;
194         }
195         return Py_FindMethod( Quaternion_methods, ( PyObject * ) self, name );
196 }
197
198 static int Quaternion_setattr( QuaternionObject * self, char *name,
199                                PyObject * v )
200 {
201         float val;
202
203         if( !PyFloat_Check( v ) && !PyInt_Check( v ) ) {
204                 return EXPP_ReturnIntError( PyExc_TypeError,
205                                             "int or float expected\n" );
206         } else {
207                 if( !PyArg_Parse( v, "f", &val ) )
208                         return EXPP_ReturnIntError( PyExc_TypeError,
209                                                     "unable to parse float argument\n" );
210         }
211         if( ELEM4( name[0], 'w', 'x', 'y', 'z' ) && name[1] == 0 ) {
212                 self->quat[name[0] - 'w'] = val;
213         } else
214                 return -1;
215
216         return 0;
217 }
218
219 /* Quaternions Sequence methods */
220 static PyObject *Quaternion_item( QuaternionObject * self, int i )
221 {
222         if( i < 0 || i >= 4 )
223                 return EXPP_ReturnPyObjError( PyExc_IndexError,
224                                               "array index out of range\n" );
225
226         return Py_BuildValue( "f", self->quat[i] );
227 }
228
229 static PyObject *Quaternion_slice( QuaternionObject * self, int begin,
230                                    int end )
231 {
232         PyObject *list;
233         int count;
234
235         if( begin < 0 )
236                 begin = 0;
237         if( end > 4 )
238                 end = 4;
239         if( begin > end )
240                 begin = end;
241
242         list = PyList_New( end - begin );
243
244         for( count = begin; count < end; count++ ) {
245                 PyList_SetItem( list, count - begin,
246                                 PyFloat_FromDouble( self->quat[count] ) );
247         }
248         return list;
249 }
250
251 static int Quaternion_ass_item( QuaternionObject * self, int i, PyObject * ob )
252 {
253         if( i < 0 || i >= 4 )
254                 return EXPP_ReturnIntError( PyExc_IndexError,
255                                             "array assignment index out of range\n" );
256         if( !PyNumber_Check( ob ) )
257                 return EXPP_ReturnIntError( PyExc_IndexError,
258                                             "Quaternion member must be a number\n" );
259
260         if( !PyFloat_Check( ob ) && !PyInt_Check( ob ) ) {
261                 return EXPP_ReturnIntError( PyExc_TypeError,
262                                             "int or float expected\n" );
263         } else {
264                 self->quat[i] = ( float ) PyFloat_AsDouble( ob );
265         }
266         return 0;
267 }
268
269 static int Quaternion_ass_slice( QuaternionObject * self, int begin, int end,
270                                  PyObject * seq )
271 {
272         int count, z;
273
274         if( begin < 0 )
275                 begin = 0;
276         if( end > 4 )
277                 end = 4;
278         if( begin > end )
279                 begin = end;
280
281         if( !PySequence_Check( seq ) )
282                 return EXPP_ReturnIntError( PyExc_TypeError,
283                                             "illegal argument type for built-in operation\n" );
284         if( PySequence_Length( seq ) != ( end - begin ) )
285                 return EXPP_ReturnIntError( PyExc_TypeError,
286                                             "size mismatch in slice assignment\n" );
287
288         z = 0;
289         for( count = begin; count < end; count++ ) {
290                 PyObject *ob = PySequence_GetItem( seq, z );
291                 z++;
292
293                 if( !PyFloat_Check( ob ) && !PyInt_Check( ob ) ) {
294                         Py_DECREF( ob );
295                         return -1;
296                 } else {
297                         if( !PyArg_Parse( ob, "f", &self->quat[count] ) ) {
298                                 Py_DECREF( ob );
299                                 return -1;
300                         }
301                 }
302         }
303         return 0;
304 }
305
306 static PyObject *Quaternion_repr( QuaternionObject * self )
307 {
308         int i, maxindex = 4 - 1;
309         char ftoa[24];
310         PyObject *str1, *str2;
311
312         str1 = PyString_FromString( "[" );
313
314         for( i = 0; i < maxindex; i++ ) {
315                 sprintf( ftoa, "%.4f, ", self->quat[i] );
316                 str2 = PyString_FromString( ftoa );
317                 if( !str1 || !str2 )
318                         goto error;
319                 PyString_ConcatAndDel( &str1, str2 );
320         }
321
322         sprintf( ftoa, "%.4f]", self->quat[maxindex] );
323         str2 = PyString_FromString( ftoa );
324         if( !str1 || !str2 )
325                 goto error;
326         PyString_ConcatAndDel( &str1, str2 );
327
328         if( str1 )
329                 return str1;
330
331       error:
332         Py_XDECREF( str1 );
333         Py_XDECREF( str2 );
334         return EXPP_ReturnPyObjError( PyExc_MemoryError,
335                                       "couldn't create PyString!\n" );
336 }
337
338
339 PyObject *Quaternion_add( PyObject * q1, PyObject * q2 )
340 {
341         float *quat = NULL;
342         PyObject *retval;
343         int x;
344
345         if( ( !QuaternionObject_Check( q1 ) )
346             || ( !QuaternionObject_Check( q2 ) ) )
347                 return EXPP_ReturnPyObjError( PyExc_TypeError,
348                                               "unsupported type for this operation\n" );
349         if( ( ( QuaternionObject * ) q1 )->flag > 0
350             || ( ( QuaternionObject * ) q2 )->flag > 0 )
351                 return EXPP_ReturnPyObjError( PyExc_ArithmeticError,
352                                               "cannot add a scalar and a quat\n" );
353
354         quat = PyMem_Malloc( 4 * sizeof( float ) );
355         for( x = 0; x < 4; x++ ) {
356                 quat[x] =
357                         ( ( ( QuaternionObject * ) q1 )->quat[x] ) +
358                         ( ( ( QuaternionObject * ) q2 )->quat[x] );
359         }
360
361         retval =  ( PyObject * ) newQuaternionObject( quat );
362         PyMem_Free( quat );
363         return retval;
364 }
365
366 PyObject *Quaternion_sub( PyObject * q1, PyObject * q2 )
367 {
368         float *quat = NULL;
369         PyObject *retval;
370         int x;
371
372         if( ( !QuaternionObject_Check( q1 ) )
373             || ( !QuaternionObject_Check( q2 ) ) )
374                 return EXPP_ReturnPyObjError( PyExc_TypeError,
375                                               "unsupported type for this operation\n" );
376         if( ( ( QuaternionObject * ) q1 )->flag > 0
377             || ( ( QuaternionObject * ) q2 )->flag > 0 )
378                 return EXPP_ReturnPyObjError( PyExc_ArithmeticError,
379                                               "cannot subtract a scalar and a quat\n" );
380
381         quat = PyMem_Malloc( 4 * sizeof( float ) );
382         for( x = 0; x < 4; x++ ) {
383                 quat[x] =
384                         ( ( ( QuaternionObject * ) q1 )->quat[x] ) -
385                         ( ( ( QuaternionObject * ) q2 )->quat[x] );
386         }
387         retval = ( PyObject * ) newQuaternionObject( quat );
388
389         PyMem_Free( quat );
390         return retval;
391 }
392
393 PyObject *Quaternion_mul( PyObject * q1, PyObject * q2 )
394 {
395         float *quat = NULL;
396         PyObject *retval;
397         int x;
398
399         if( ( !QuaternionObject_Check( q1 ) )
400             || ( !QuaternionObject_Check( q2 ) ) )
401                 return EXPP_ReturnPyObjError( PyExc_TypeError,
402                                               "unsupported type for this operation\n" );
403         if( ( ( QuaternionObject * ) q1 )->flag == 0
404             && ( ( QuaternionObject * ) q2 )->flag == 0 )
405                 return EXPP_ReturnPyObjError( PyExc_ArithmeticError,
406                                               "please use the dot or cross product to multiply quaternions\n" );
407
408         quat = PyMem_Malloc( 4 * sizeof( float ) );
409         //scalar mult by quat
410         for( x = 0; x < 4; x++ ) {
411                 quat[x] =
412                         ( ( QuaternionObject * ) q1 )->quat[x] *
413                         ( ( QuaternionObject * ) q2 )->quat[x];
414         }
415         retval =  ( PyObject * ) newQuaternionObject( quat );
416
417         PyMem_Free( quat );
418         return retval;
419 }
420
421 //coercion of unknown types to type QuaternionObject for numeric protocols
422 int Quaternion_coerce( PyObject ** q1, PyObject ** q2 )
423 {
424         long *tempI = NULL;
425         double *tempF = NULL;
426         float *quat = NULL;
427         int x;
428
429         if( QuaternionObject_Check( *q1 ) ) {
430                 if( QuaternionObject_Check( *q2 ) ) {   //two Quaternions
431                         Py_INCREF( *q1 );
432                         Py_INCREF( *q2 );
433                         return 0;
434                 } else {
435                         if( PyNumber_Check( *q2 ) ) {
436                                 if( PyInt_Check( *q2 ) ) {      //cast scalar to Quaternion
437                                         tempI = PyMem_Malloc( 1 *
438                                                               sizeof( long ) );
439                                         *tempI = PyInt_AsLong( *q2 );
440                                         quat = PyMem_Malloc( 4 *
441                                                              sizeof( float ) );
442                                         for( x = 0; x < 4; x++ ) {
443                                                 quat[x] = ( float ) *tempI;
444                                         }
445                                         PyMem_Free( tempI );
446                                         *q2 = newQuaternionObject( quat );
447                                         PyMem_Free( quat );
448                                         ( ( QuaternionObject * ) * q2 )->flag = 1;      //int coercion
449                                         Py_INCREF( *q1 );  /* fixme:  is this needed? */
450                                         return 0;
451                                 } else if( PyFloat_Check( *q2 ) ) {     //cast scalar to Quaternion
452                                         tempF = PyMem_Malloc( 1 *
453                                                               sizeof
454                                                               ( double ) );
455                                         *tempF = PyFloat_AsDouble( *q2 );
456                                         quat = PyMem_Malloc( 4 *
457                                                              sizeof( float ) );
458                                         for( x = 0; x < 4; x++ ) {
459                                                 quat[x] = ( float ) *tempF;
460                                         }
461                                         PyMem_Free( tempF );
462                                         *q2 = newQuaternionObject( quat );
463                                         PyMem_Free( quat );
464                                         ( ( QuaternionObject * ) * q2 )->flag = 2;      //float coercion
465                                         Py_INCREF( *q1 );  /* fixme:  is this needed? */
466                                         return 0;
467                                 }
468                         }
469                         //unknown type or numeric cast failure
470                         printf( "attempting quaternion operation with unsupported type...\n" );
471                         Py_INCREF( *q1 );  /* fixme:  is this needed? */
472                         return 0;       //operation will type check
473                 }
474         } else {
475                 printf( "numeric protocol failure...\n" );
476                 return -1;      //this should not occur - fail
477         }
478         return -1;
479 }
480
481 static PySequenceMethods Quaternion_SeqMethods = {
482         ( inquiry ) 0,          /* sq_length */
483         ( binaryfunc ) 0,       /* sq_concat */
484         ( intargfunc ) 0,       /* sq_repeat */
485         ( intargfunc ) Quaternion_item, /* sq_item */
486         ( intintargfunc ) Quaternion_slice,     /* sq_slice */
487         ( intobjargproc ) Quaternion_ass_item,  /* sq_ass_item */
488         ( intintobjargproc ) Quaternion_ass_slice,      /* sq_ass_slice */
489 };
490
491 static PyNumberMethods Quaternion_NumMethods = {
492         ( binaryfunc ) Quaternion_add,  /* __add__ */
493         ( binaryfunc ) Quaternion_sub,  /* __sub__ */
494         ( binaryfunc ) Quaternion_mul,  /* __mul__ */
495         ( binaryfunc ) 0,       /* __div__ */
496         ( binaryfunc ) 0,       /* __mod__ */
497         ( binaryfunc ) 0,       /* __divmod__ */
498         ( ternaryfunc ) 0,      /* __pow__ */
499         ( unaryfunc ) 0,        /* __neg__ */
500         ( unaryfunc ) 0,        /* __pos__ */
501         ( unaryfunc ) 0,        /* __abs__ */
502         ( inquiry ) 0,          /* __nonzero__ */
503         ( unaryfunc ) 0,        /* __invert__ */
504         ( binaryfunc ) 0,       /* __lshift__ */
505         ( binaryfunc ) 0,       /* __rshift__ */
506         ( binaryfunc ) 0,       /* __and__ */
507         ( binaryfunc ) 0,       /* __xor__ */
508         ( binaryfunc ) 0,       /* __or__ */
509         ( coercion ) Quaternion_coerce, /* __coerce__ */
510         ( unaryfunc ) 0,        /* __int__ */
511         ( unaryfunc ) 0,        /* __long__ */
512         ( unaryfunc ) 0,        /* __float__ */
513         ( unaryfunc ) 0,        /* __oct__ */
514         ( unaryfunc ) 0,        /* __hex__ */
515
516 };
517
518 PyTypeObject quaternion_Type = {
519         PyObject_HEAD_INIT( NULL ) 
520         0,      /*ob_size */
521         "quaternion",           /*tp_name */
522         sizeof( QuaternionObject ),     /*tp_basicsize */
523         0,                      /*tp_itemsize */
524         ( destructor ) Quaternion_dealloc,      /*tp_dealloc */
525         ( printfunc ) 0,        /*tp_print */
526         ( getattrfunc ) Quaternion_getattr,     /*tp_getattr */
527         ( setattrfunc ) Quaternion_setattr,     /*tp_setattr */
528         0,                      /*tp_compare */
529         ( reprfunc ) Quaternion_repr,   /*tp_repr */
530         &Quaternion_NumMethods, /*tp_as_number */
531         &Quaternion_SeqMethods, /*tp_as_sequence */
532 };
533
534 /* 
535  newQuaternionObject
536  
537  if the quat arg is not null, this method allocates memory and copies *quat into it.
538  we will free the memory in the dealloc routine.
539 */
540
541 PyObject *newQuaternionObject( float *quat )
542 {
543         QuaternionObject *self;
544         int x;
545
546         quaternion_Type.ob_type = &PyType_Type;
547
548         self = PyObject_NEW( QuaternionObject, &quaternion_Type );
549
550         self->quat = PyMem_Malloc( 4 * sizeof( float ) );
551
552         if( !quat ) {
553                 for( x = 0; x < 4; x++ ) {
554                         self->quat[x] = 0.0f;
555                 }
556                 self->quat[3] = 1.0f;
557         } else {
558                 for( x = 0; x < 4; x++ ) {
559                         self->quat[x] = quat[x];
560                 }
561         }
562         self->flag = 0;
563
564         return ( PyObject * ) self;
565 }