copy of docs from 2.4x for python modules that have been kept
[blender.git] / source / blender / python / generic / Mathutils.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., 59 Temple Place - Suite 330, Boston, MA        02111-1307, USA.
19  *
20  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
21  * All rights reserved.
22  *
23  * This is a new part of Blender.
24  *
25  * Contributor(s): Joseph Gilbert, Campbell Barton
26  *
27  * ***** END GPL LICENSE BLOCK *****
28  */
29
30 #include "Mathutils.h"
31
32 #include "BLI_math.h"
33 #include "PIL_time.h"
34 #include "BLI_rand.h"
35 #include "BKE_utildefines.h"
36
37 //-------------------------DOC STRINGS ---------------------------
38 static char M_Mathutils_doc[] = "The Blender Mathutils module\n\n";
39 static char M_Mathutils_Rand_doc[] = "() - return a random number";
40 static char M_Mathutils_AngleBetweenVecs_doc[] = "() - returns the angle between two vectors in degrees";
41 static char M_Mathutils_MidpointVecs_doc[] = "() - return the vector to the midpoint between two vectors";
42 static char M_Mathutils_ProjectVecs_doc[] =     "() - returns the projection vector from the projection of vecA onto vecB";
43 static char M_Mathutils_RotationMatrix_doc[] = "() - construct a rotation matrix from an angle and axis of rotation";
44 static char M_Mathutils_ScaleMatrix_doc[] =     "() - construct a scaling matrix from a scaling factor";
45 static char M_Mathutils_OrthoProjectionMatrix_doc[] = "() - construct a orthographic projection matrix from a selected plane";
46 static char M_Mathutils_ShearMatrix_doc[] = "() - construct a shearing matrix from a plane of shear and a shear factor";
47 static char M_Mathutils_TranslationMatrix_doc[] = "(vec) - create a translation matrix from a vector";
48 static char M_Mathutils_Slerp_doc[] = "() - returns the interpolation between two quaternions";
49 static char M_Mathutils_DifferenceQuats_doc[] = "() - return the angular displacment difference between two quats";
50 static char M_Mathutils_Intersect_doc[] = "(v1, v2, v3, ray, orig, clip=1) - returns the intersection between a ray and a triangle, if possible, returns None otherwise";
51 static char M_Mathutils_TriangleArea_doc[] = "(v1, v2, v3) - returns the area size of the 2D or 3D triangle defined";
52 static char M_Mathutils_TriangleNormal_doc[] = "(v1, v2, v3) - returns the normal of the 3D triangle defined";
53 static char M_Mathutils_QuadNormal_doc[] = "(v1, v2, v3, v4) - returns the normal of the 3D quad defined";
54 static char M_Mathutils_LineIntersect_doc[] = "(v1, v2, v3, v4) - returns a tuple with the points on each line respectively closest to the other";
55 //-----------------------METHOD DEFINITIONS ----------------------
56
57 static PyObject *M_Mathutils_Rand(PyObject * self, PyObject * args);
58 static PyObject *M_Mathutils_AngleBetweenVecs(PyObject * self, PyObject * args);
59 static PyObject *M_Mathutils_MidpointVecs(PyObject * self, PyObject * args);
60 static PyObject *M_Mathutils_ProjectVecs(PyObject * self, PyObject * args);
61 static PyObject *M_Mathutils_RotationMatrix(PyObject * self, PyObject * args);
62 static PyObject *M_Mathutils_TranslationMatrix(PyObject * self, VectorObject * value);
63 static PyObject *M_Mathutils_ScaleMatrix(PyObject * self, PyObject * args);
64 static PyObject *M_Mathutils_OrthoProjectionMatrix(PyObject * self, PyObject * args);
65 static PyObject *M_Mathutils_ShearMatrix(PyObject * self, PyObject * args);
66 static PyObject *M_Mathutils_DifferenceQuats(PyObject * self, PyObject * args);
67 static PyObject *M_Mathutils_Slerp(PyObject * self, PyObject * args);
68 static PyObject *M_Mathutils_Intersect( PyObject * self, PyObject * args );
69 static PyObject *M_Mathutils_TriangleArea( PyObject * self, PyObject * args );
70 static PyObject *M_Mathutils_TriangleNormal( PyObject * self, PyObject * args );
71 static PyObject *M_Mathutils_QuadNormal( PyObject * self, PyObject * args );
72 static PyObject *M_Mathutils_LineIntersect( PyObject * self, PyObject * args );
73
74 struct PyMethodDef M_Mathutils_methods[] = {
75         {"Rand", (PyCFunction) M_Mathutils_Rand, METH_VARARGS, M_Mathutils_Rand_doc},
76         {"AngleBetweenVecs", (PyCFunction) M_Mathutils_AngleBetweenVecs, METH_VARARGS, M_Mathutils_AngleBetweenVecs_doc},
77         {"MidpointVecs", (PyCFunction) M_Mathutils_MidpointVecs, METH_VARARGS, M_Mathutils_MidpointVecs_doc},
78         {"ProjectVecs", (PyCFunction) M_Mathutils_ProjectVecs, METH_VARARGS, M_Mathutils_ProjectVecs_doc},
79         {"RotationMatrix", (PyCFunction) M_Mathutils_RotationMatrix, METH_VARARGS, M_Mathutils_RotationMatrix_doc},
80         {"ScaleMatrix", (PyCFunction) M_Mathutils_ScaleMatrix, METH_VARARGS, M_Mathutils_ScaleMatrix_doc},
81         {"ShearMatrix", (PyCFunction) M_Mathutils_ShearMatrix, METH_VARARGS, M_Mathutils_ShearMatrix_doc},
82         {"TranslationMatrix", (PyCFunction) M_Mathutils_TranslationMatrix, METH_O, M_Mathutils_TranslationMatrix_doc},
83         {"OrthoProjectionMatrix", (PyCFunction) M_Mathutils_OrthoProjectionMatrix,  METH_VARARGS, M_Mathutils_OrthoProjectionMatrix_doc},
84         {"DifferenceQuats", (PyCFunction) M_Mathutils_DifferenceQuats, METH_VARARGS,M_Mathutils_DifferenceQuats_doc},
85         {"Slerp", (PyCFunction) M_Mathutils_Slerp, METH_VARARGS, M_Mathutils_Slerp_doc},
86         {"Intersect", ( PyCFunction ) M_Mathutils_Intersect, METH_VARARGS, M_Mathutils_Intersect_doc},
87         {"TriangleArea", ( PyCFunction ) M_Mathutils_TriangleArea, METH_VARARGS, M_Mathutils_TriangleArea_doc},
88         {"TriangleNormal", ( PyCFunction ) M_Mathutils_TriangleNormal, METH_VARARGS, M_Mathutils_TriangleNormal_doc},
89         {"QuadNormal", ( PyCFunction ) M_Mathutils_QuadNormal, METH_VARARGS, M_Mathutils_QuadNormal_doc},
90         {"LineIntersect", ( PyCFunction ) M_Mathutils_LineIntersect, METH_VARARGS, M_Mathutils_LineIntersect_doc},
91         {NULL, NULL, 0, NULL}
92 };
93
94 /*----------------------------MODULE INIT-------------------------*/
95 /* from can be Blender.Mathutils or GameLogic.Mathutils for the BGE */
96
97 static struct PyModuleDef M_Mathutils_module_def = {
98         PyModuleDef_HEAD_INIT,
99         "Mathutils",  /* m_name */
100         M_Mathutils_doc,  /* m_doc */
101         0,  /* m_size */
102         M_Mathutils_methods,  /* m_methods */
103         0,  /* m_reload */
104         0,  /* m_traverse */
105         0,  /* m_clear */
106         0,  /* m_free */
107 };
108
109 PyObject *Mathutils_Init(void)
110 {
111         PyObject *submodule;
112
113         //seed the generator for the rand function
114         BLI_srand((unsigned int) (PIL_check_seconds_timer() * 0x7FFFFFFF));
115         
116         if( PyType_Ready( &vector_Type ) < 0 )
117                 return NULL;
118         if( PyType_Ready( &matrix_Type ) < 0 )
119                 return NULL;    
120         if( PyType_Ready( &euler_Type ) < 0 )
121                 return NULL;
122         if( PyType_Ready( &quaternion_Type ) < 0 )
123                 return NULL;
124         
125         submodule = PyModule_Create(&M_Mathutils_module_def);
126         PyDict_SetItemString(PySys_GetObject("modules"), M_Mathutils_module_def.m_name, submodule);
127         
128         /* each type has its own new() function */
129         PyModule_AddObject( submodule, "Vector",                (PyObject *)&vector_Type );
130         PyModule_AddObject( submodule, "Matrix",                (PyObject *)&matrix_Type );
131         PyModule_AddObject( submodule, "Euler",                 (PyObject *)&euler_Type );
132         PyModule_AddObject( submodule, "Quaternion",    (PyObject *)&quaternion_Type );
133         
134         mathutils_matrix_vector_cb_index= Mathutils_RegisterCallback(&mathutils_matrix_vector_cb);
135
136         return (submodule);
137 }
138
139 //-----------------------------METHODS----------------------------
140 //-----------------quat_rotation (internal)-----------
141 //This function multiplies a vector/point * quat or vice versa
142 //to rotate the point/vector by the quaternion
143 //arguments should all be 3D
144 PyObject *quat_rotation(PyObject *arg1, PyObject *arg2)
145 {
146         float rot[3];
147         QuaternionObject *quat = NULL;
148         VectorObject *vec = NULL;
149
150         if(QuaternionObject_Check(arg1)){
151                 quat = (QuaternionObject*)arg1;
152                 if(!BaseMath_ReadCallback(quat))
153                         return NULL;
154
155                 if(VectorObject_Check(arg2)){
156                         vec = (VectorObject*)arg2;
157                         
158                         if(!BaseMath_ReadCallback(vec))
159                                 return NULL;
160                         
161                         rot[0] = quat->quat[0]*quat->quat[0]*vec->vec[0] + 2*quat->quat[2]*quat->quat[0]*vec->vec[2] - 
162                                 2*quat->quat[3]*quat->quat[0]*vec->vec[1] + quat->quat[1]*quat->quat[1]*vec->vec[0] + 
163                                 2*quat->quat[2]*quat->quat[1]*vec->vec[1] + 2*quat->quat[3]*quat->quat[1]*vec->vec[2] - 
164                                 quat->quat[3]*quat->quat[3]*vec->vec[0] - quat->quat[2]*quat->quat[2]*vec->vec[0];
165                         rot[1] = 2*quat->quat[1]*quat->quat[2]*vec->vec[0] + quat->quat[2]*quat->quat[2]*vec->vec[1] + 
166                                 2*quat->quat[3]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[3]*vec->vec[0] - 
167                                 quat->quat[3]*quat->quat[3]*vec->vec[1] + quat->quat[0]*quat->quat[0]*vec->vec[1] - 
168                                 2*quat->quat[1]*quat->quat[0]*vec->vec[2] - quat->quat[1]*quat->quat[1]*vec->vec[1];
169                         rot[2] = 2*quat->quat[1]*quat->quat[3]*vec->vec[0] + 2*quat->quat[2]*quat->quat[3]*vec->vec[1] + 
170                                 quat->quat[3]*quat->quat[3]*vec->vec[2] - 2*quat->quat[0]*quat->quat[2]*vec->vec[0] - 
171                                 quat->quat[2]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[1]*vec->vec[1] - 
172                                 quat->quat[1]*quat->quat[1]*vec->vec[2] + quat->quat[0]*quat->quat[0]*vec->vec[2];
173                         return newVectorObject(rot, 3, Py_NEW, NULL);
174                 }
175         }else if(VectorObject_Check(arg1)){
176                 vec = (VectorObject*)arg1;
177                 
178                 if(!BaseMath_ReadCallback(vec))
179                         return NULL;
180                 
181                 if(QuaternionObject_Check(arg2)){
182                         quat = (QuaternionObject*)arg2;
183                         if(!BaseMath_ReadCallback(quat))
184                                 return NULL;
185
186                         rot[0] = quat->quat[0]*quat->quat[0]*vec->vec[0] + 2*quat->quat[2]*quat->quat[0]*vec->vec[2] - 
187                                 2*quat->quat[3]*quat->quat[0]*vec->vec[1] + quat->quat[1]*quat->quat[1]*vec->vec[0] + 
188                                 2*quat->quat[2]*quat->quat[1]*vec->vec[1] + 2*quat->quat[3]*quat->quat[1]*vec->vec[2] - 
189                                 quat->quat[3]*quat->quat[3]*vec->vec[0] - quat->quat[2]*quat->quat[2]*vec->vec[0];
190                         rot[1] = 2*quat->quat[1]*quat->quat[2]*vec->vec[0] + quat->quat[2]*quat->quat[2]*vec->vec[1] + 
191                                 2*quat->quat[3]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[3]*vec->vec[0] - 
192                                 quat->quat[3]*quat->quat[3]*vec->vec[1] + quat->quat[0]*quat->quat[0]*vec->vec[1] - 
193                                 2*quat->quat[1]*quat->quat[0]*vec->vec[2] - quat->quat[1]*quat->quat[1]*vec->vec[1];
194                         rot[2] = 2*quat->quat[1]*quat->quat[3]*vec->vec[0] + 2*quat->quat[2]*quat->quat[3]*vec->vec[1] + 
195                                 quat->quat[3]*quat->quat[3]*vec->vec[2] - 2*quat->quat[0]*quat->quat[2]*vec->vec[0] - 
196                                 quat->quat[2]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[1]*vec->vec[1] - 
197                                 quat->quat[1]*quat->quat[1]*vec->vec[2] + quat->quat[0]*quat->quat[0]*vec->vec[2];
198                         return newVectorObject(rot, 3, Py_NEW, NULL);
199                 }
200         }
201
202         PyErr_SetString(PyExc_RuntimeError, "quat_rotation(internal): internal problem rotating vector/point\n");
203         return NULL;
204         
205 }
206
207 //----------------------------------Mathutils.Rand() --------------------
208 //returns a random number between a high and low value
209 static PyObject *M_Mathutils_Rand(PyObject * self, PyObject * args)
210 {
211         float high, low, range;
212         double drand;
213         //initializers
214         high = 1.0;
215         low = 0.0;
216
217         if(!PyArg_ParseTuple(args, "|ff", &low, &high)) {
218                 PyErr_SetString(PyExc_TypeError, "Mathutils.Rand(): expected nothing or optional (float, float)\n");
219                 return NULL;
220         }
221
222         if((high < low) || (high < 0 && low > 0)) {
223                 PyErr_SetString(PyExc_ValueError, "Mathutils.Rand(): high value should be larger than low value\n");
224                 return NULL;
225         }
226         //get the random number 0 - 1
227         drand = BLI_drand();
228
229         //set it to range
230         range = high - low;
231         drand = drand * range;
232         drand = drand + low;
233
234         return PyFloat_FromDouble(drand);
235 }
236 //----------------------------------VECTOR FUNCTIONS---------------------
237 //----------------------------------Mathutils.AngleBetweenVecs() ---------
238 //calculates the angle between 2 vectors
239 static PyObject *M_Mathutils_AngleBetweenVecs(PyObject * self, PyObject * args)
240 {
241         VectorObject *vec1 = NULL, *vec2 = NULL;
242         double dot = 0.0f, angleRads, test_v1 = 0.0f, test_v2 = 0.0f;
243         int x, size;
244
245         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2))
246                 goto AttributeError1; //not vectors
247         if(vec1->size != vec2->size)
248                 goto AttributeError1; //bad sizes
249
250         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2))
251                 return NULL;
252         
253         //since size is the same....
254         size = vec1->size;
255
256         for(x = 0; x < size; x++) {
257                 test_v1 += vec1->vec[x] * vec1->vec[x];
258                 test_v2 += vec2->vec[x] * vec2->vec[x];
259         }
260         if (!test_v1 || !test_v2){
261                 goto AttributeError2; //zero-length vector
262         }
263
264         //dot product
265         for(x = 0; x < size; x++) {
266                 dot += vec1->vec[x] * vec2->vec[x];
267         }
268         dot /= (sqrt(test_v1) * sqrt(test_v2));
269
270         angleRads = (double)saacos(dot);
271
272 #ifdef USE_MATHUTILS_DEG
273         return PyFloat_FromDouble(angleRads * (180/ Py_PI));
274 #else
275         return PyFloat_FromDouble(angleRads);
276 #endif
277 AttributeError1:
278         PyErr_SetString(PyExc_AttributeError, "Mathutils.AngleBetweenVecs(): expects (2) VECTOR objects of the same size\n");
279         return NULL;
280
281 AttributeError2:
282         PyErr_SetString(PyExc_AttributeError, "Mathutils.AngleBetweenVecs(): zero length vectors are not acceptable arguments\n");
283         return NULL;
284 }
285 //----------------------------------Mathutils.MidpointVecs() -------------
286 //calculates the midpoint between 2 vectors
287 static PyObject *M_Mathutils_MidpointVecs(PyObject * self, PyObject * args)
288 {
289         VectorObject *vec1 = NULL, *vec2 = NULL;
290         float vec[4];
291         int x;
292         
293         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2)) {
294                 PyErr_SetString(PyExc_TypeError, "Mathutils.MidpointVecs(): expects (2) vector objects of the same size\n");
295                 return NULL;
296         }
297         if(vec1->size != vec2->size) {
298                 PyErr_SetString(PyExc_AttributeError, "Mathutils.MidpointVecs(): expects (2) vector objects of the same size\n");
299                 return NULL;
300         }
301         
302         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2))
303                 return NULL;
304
305         for(x = 0; x < vec1->size; x++) {
306                 vec[x] = 0.5f * (vec1->vec[x] + vec2->vec[x]);
307         }
308         return newVectorObject(vec, vec1->size, Py_NEW, NULL);
309 }
310 //----------------------------------Mathutils.ProjectVecs() -------------
311 //projects vector 1 onto vector 2
312 static PyObject *M_Mathutils_ProjectVecs(PyObject * self, PyObject * args)
313 {
314         VectorObject *vec1 = NULL, *vec2 = NULL;
315         float vec[4]; 
316         double dot = 0.0f, dot2 = 0.0f;
317         int x, size;
318
319         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2)) {
320                 PyErr_SetString(PyExc_TypeError, "Mathutils.ProjectVecs(): expects (2) vector objects of the same size\n");
321                 return NULL;
322         }
323         if(vec1->size != vec2->size) {
324                 PyErr_SetString(PyExc_AttributeError, "Mathutils.ProjectVecs(): expects (2) vector objects of the same size\n");
325                 return NULL;
326         }
327
328         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2))
329                 return NULL;
330
331         
332         //since they are the same size...
333         size = vec1->size;
334
335         //get dot products
336         for(x = 0; x < size; x++) {
337                 dot += vec1->vec[x] * vec2->vec[x];
338                 dot2 += vec2->vec[x] * vec2->vec[x];
339         }
340         //projection
341         dot /= dot2;
342         for(x = 0; x < size; x++) {
343                 vec[x] = (float)(dot * vec2->vec[x]);
344         }
345         return newVectorObject(vec, size, Py_NEW, NULL);
346 }
347 //----------------------------------MATRIX FUNCTIONS--------------------
348 //----------------------------------Mathutils.RotationMatrix() ----------
349 //mat is a 1D array of floats - row[0][0],row[0][1], row[1][0], etc.
350 //creates a rotation matrix
351 static PyObject *M_Mathutils_RotationMatrix(PyObject * self, PyObject * args)
352 {
353         VectorObject *vec = NULL;
354         char *axis = NULL;
355         int matSize;
356         float angle = 0.0f;
357         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
358                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
359
360         if(!PyArg_ParseTuple(args, "fi|sO!", &angle, &matSize, &axis, &vector_Type, &vec)) {
361                 PyErr_SetString(PyExc_TypeError, "Mathutils.RotationMatrix(): expected float int and optional string and vector\n");
362                 return NULL;
363         }
364
365 #ifdef USE_MATHUTILS_DEG
366         /* Clamp to -360:360 */
367         while (angle<-360.0f)
368                 angle+=360.0;
369         while (angle>360.0f)
370                 angle-=360.0;
371 #else
372         while (angle<-(Py_PI*2))
373                 angle+=(Py_PI*2);
374         while (angle>(Py_PI*2))
375                 angle-=(Py_PI*2);
376 #endif
377         
378         if(matSize != 2 && matSize != 3 && matSize != 4) {
379                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
380                 return NULL;
381         }
382         if(matSize == 2 && (axis != NULL || vec != NULL)) {
383                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): cannot create a 2x2 rotation matrix around arbitrary axis\n");
384                 return NULL;
385         }
386         if((matSize == 3 || matSize == 4) && axis == NULL) {
387                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): please choose an axis of rotation for 3d and 4d matrices\n");
388                 return NULL;
389         }
390         if(axis) {
391                 if(((strcmp(axis, "r") == 0) || (strcmp(axis, "R") == 0)) && vec == NULL) {
392                         PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): please define the arbitrary axis of rotation\n");
393                         return NULL;
394                 }
395         }
396         if(vec) {
397                 if(vec->size != 3) {
398                         PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): the arbitrary axis must be a 3D vector\n");
399                         return NULL;
400                 }
401                 
402                 if(!BaseMath_ReadCallback(vec))
403                         return NULL;
404                 
405         }
406 #ifdef USE_MATHUTILS_DEG
407         //convert to radians
408         angle = angle * (float) (Py_PI / 180);
409 #endif
410
411         if(axis == NULL && matSize == 2) {
412                 //2D rotation matrix
413                 mat[0] = (float) cos (angle);
414                 mat[1] = (float) sin (angle);
415                 mat[2] = -((float) sin(angle));
416                 mat[3] = (float) cos(angle);
417         } else if((strcmp(axis, "x") == 0) || (strcmp(axis, "X") == 0)) {
418                 //rotation around X
419                 mat[0] = 1.0f;
420                 mat[4] = (float) cos(angle);
421                 mat[5] = (float) sin(angle);
422                 mat[7] = -((float) sin(angle));
423                 mat[8] = (float) cos(angle);
424         } else if((strcmp(axis, "y") == 0) || (strcmp(axis, "Y") == 0)) {
425                 //rotation around Y
426                 mat[0] = (float) cos(angle);
427                 mat[2] = -((float) sin(angle));
428                 mat[4] = 1.0f;
429                 mat[6] = (float) sin(angle);
430                 mat[8] = (float) cos(angle);
431         } else if((strcmp(axis, "z") == 0) || (strcmp(axis, "Z") == 0)) {
432                 //rotation around Z
433                 mat[0] = (float) cos(angle);
434                 mat[1] = (float) sin(angle);
435                 mat[3] = -((float) sin(angle));
436                 mat[4] = (float) cos(angle);
437                 mat[8] = 1.0f;
438         } else if((strcmp(axis, "r") == 0) || (strcmp(axis, "R") == 0)) {
439                 //arbitrary rotation
440                 axis_angle_to_mat3( (float (*)[3])mat,vec->vec, angle);
441
442         } else {
443                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): unrecognizable axis of rotation type - expected x,y,z or r\n");
444                 return NULL;
445         }
446         if(matSize == 4) {
447                 //resize matrix
448                 mat[10] = mat[8];
449                 mat[9] = mat[7];
450                 mat[8] = mat[6];
451                 mat[7] = 0.0f;
452                 mat[6] = mat[5];
453                 mat[5] = mat[4];
454                 mat[4] = mat[3];
455                 mat[3] = 0.0f;
456         }
457         //pass to matrix creation
458         return newMatrixObject(mat, matSize, matSize, Py_NEW, NULL);
459 }
460 //----------------------------------Mathutils.TranslationMatrix() -------
461 //creates a translation matrix
462 static PyObject *M_Mathutils_TranslationMatrix(PyObject * self, VectorObject * vec)
463 {
464         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
465                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
466         
467         if(!VectorObject_Check(vec)) {
468                 PyErr_SetString(PyExc_TypeError, "Mathutils.TranslationMatrix(): expected vector\n");
469                 return NULL;
470         }
471         if(vec->size != 3 && vec->size != 4) {
472                 PyErr_SetString(PyExc_TypeError, "Mathutils.TranslationMatrix(): vector must be 3D or 4D\n");
473                 return NULL;
474         }
475         
476         if(!BaseMath_ReadCallback(vec))
477                 return NULL;
478         
479         //create a identity matrix and add translation
480         unit_m4((float(*)[4]) mat);
481         mat[12] = vec->vec[0];
482         mat[13] = vec->vec[1];
483         mat[14] = vec->vec[2];
484
485         return newMatrixObject(mat, 4, 4, Py_NEW, NULL);
486 }
487 //----------------------------------Mathutils.ScaleMatrix() -------------
488 //mat is a 1D array of floats - row[0][0],row[0][1], row[1][0], etc.
489 //creates a scaling matrix
490 static PyObject *M_Mathutils_ScaleMatrix(PyObject * self, PyObject * args)
491 {
492         VectorObject *vec = NULL;
493         float norm = 0.0f, factor;
494         int matSize, x;
495         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
496                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
497
498         if(!PyArg_ParseTuple(args, "fi|O!", &factor, &matSize, &vector_Type, &vec)) {
499                 PyErr_SetString(PyExc_TypeError, "Mathutils.ScaleMatrix(): expected float int and optional vector\n");
500                 return NULL;
501         }
502         if(matSize != 2 && matSize != 3 && matSize != 4) {
503                 PyErr_SetString(PyExc_AttributeError, "Mathutils.ScaleMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
504                 return NULL;
505         }
506         if(vec) {
507                 if(vec->size > 2 && matSize == 2) {
508                         PyErr_SetString(PyExc_AttributeError, "Mathutils.ScaleMatrix(): please use 2D vectors when scaling in 2D\n");
509                         return NULL;
510                 }
511                 
512                 if(!BaseMath_ReadCallback(vec))
513                         return NULL;
514                 
515         }
516         if(vec == NULL) {       //scaling along axis
517                 if(matSize == 2) {
518                         mat[0] = factor;
519                         mat[3] = factor;
520                 } else {
521                         mat[0] = factor;
522                         mat[4] = factor;
523                         mat[8] = factor;
524                 }
525         } else { //scaling in arbitrary direction
526                 //normalize arbitrary axis
527                 for(x = 0; x < vec->size; x++) {
528                         norm += vec->vec[x] * vec->vec[x];
529                 }
530                 norm = (float) sqrt(norm);
531                 for(x = 0; x < vec->size; x++) {
532                         vec->vec[x] /= norm;
533                 }
534                 if(matSize == 2) {
535                         mat[0] = 1 +((factor - 1) *(vec->vec[0] * vec->vec[0]));
536                         mat[1] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
537                         mat[2] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
538                         mat[3] = 1 + ((factor - 1) *(vec->vec[1] * vec->vec[1]));
539                 } else {
540                         mat[0] = 1 + ((factor - 1) *(vec->vec[0] * vec->vec[0]));
541                         mat[1] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
542                         mat[2] =((factor - 1) *(vec->vec[0] * vec->vec[2]));
543                         mat[3] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
544                         mat[4] = 1 + ((factor - 1) *(vec->vec[1] * vec->vec[1]));
545                         mat[5] =((factor - 1) *(vec->vec[1] * vec->vec[2]));
546                         mat[6] =((factor - 1) *(vec->vec[0] * vec->vec[2]));
547                         mat[7] =((factor - 1) *(vec->vec[1] * vec->vec[2]));
548                         mat[8] = 1 + ((factor - 1) *(vec->vec[2] * vec->vec[2]));
549                 }
550         }
551         if(matSize == 4) {
552                 //resize matrix
553                 mat[10] = mat[8];
554                 mat[9] = mat[7];
555                 mat[8] = mat[6];
556                 mat[7] = 0.0f;
557                 mat[6] = mat[5];
558                 mat[5] = mat[4];
559                 mat[4] = mat[3];
560                 mat[3] = 0.0f;
561         }
562         //pass to matrix creation
563         return newMatrixObject(mat, matSize, matSize, Py_NEW, NULL);
564 }
565 //----------------------------------Mathutils.OrthoProjectionMatrix() ---
566 //mat is a 1D array of floats - row[0][0],row[0][1], row[1][0], etc.
567 //creates an ortho projection matrix
568 static PyObject *M_Mathutils_OrthoProjectionMatrix(PyObject * self, PyObject * args)
569 {
570         VectorObject *vec = NULL;
571         char *plane;
572         int matSize, x;
573         float norm = 0.0f;
574         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
575                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
576         
577         if(!PyArg_ParseTuple(args, "si|O!", &plane, &matSize, &vector_Type, &vec)) {
578                 PyErr_SetString(PyExc_TypeError, "Mathutils.OrthoProjectionMatrix(): expected string and int and optional vector\n");
579                 return NULL;
580         }
581         if(matSize != 2 && matSize != 3 && matSize != 4) {
582                 PyErr_SetString(PyExc_AttributeError,"Mathutils.OrthoProjectionMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
583                 return NULL;
584         }
585         if(vec) {
586                 if(vec->size > 2 && matSize == 2) {
587                         PyErr_SetString(PyExc_AttributeError, "Mathutils.OrthoProjectionMatrix(): please use 2D vectors when scaling in 2D\n");
588                         return NULL;
589                 }
590                 
591                 if(!BaseMath_ReadCallback(vec))
592                         return NULL;
593                 
594         }
595         if(vec == NULL) {       //ortho projection onto cardinal plane
596                 if(((strcmp(plane, "x") == 0)
597                       || (strcmp(plane, "X") == 0)) && matSize == 2) {
598                         mat[0] = 1.0f;
599                 } else if(((strcmp(plane, "y") == 0) 
600                         || (strcmp(plane, "Y") == 0))
601                            && matSize == 2) {
602                         mat[3] = 1.0f;
603                 } else if(((strcmp(plane, "xy") == 0)
604                              || (strcmp(plane, "XY") == 0))
605                            && matSize > 2) {
606                         mat[0] = 1.0f;
607                         mat[4] = 1.0f;
608                 } else if(((strcmp(plane, "xz") == 0)
609                              || (strcmp(plane, "XZ") == 0))
610                            && matSize > 2) {
611                         mat[0] = 1.0f;
612                         mat[8] = 1.0f;
613                 } else if(((strcmp(plane, "yz") == 0)
614                              || (strcmp(plane, "YZ") == 0))
615                            && matSize > 2) {
616                         mat[4] = 1.0f;
617                         mat[8] = 1.0f;
618                 } else {
619                         PyErr_SetString(PyExc_AttributeError, "Mathutils.OrthoProjectionMatrix(): unknown plane - expected: x, y, xy, xz, yz\n");
620                         return NULL;
621                 }
622         } else { //arbitrary plane
623                 //normalize arbitrary axis
624                 for(x = 0; x < vec->size; x++) {
625                         norm += vec->vec[x] * vec->vec[x];
626                 }
627                 norm = (float) sqrt(norm);
628                 for(x = 0; x < vec->size; x++) {
629                         vec->vec[x] /= norm;
630                 }
631                 if(((strcmp(plane, "r") == 0)
632                       || (strcmp(plane, "R") == 0)) && matSize == 2) {
633                         mat[0] = 1 - (vec->vec[0] * vec->vec[0]);
634                         mat[1] = -(vec->vec[0] * vec->vec[1]);
635                         mat[2] = -(vec->vec[0] * vec->vec[1]);
636                         mat[3] = 1 - (vec->vec[1] * vec->vec[1]);
637                 } else if(((strcmp(plane, "r") == 0)
638                              || (strcmp(plane, "R") == 0))
639                            && matSize > 2) {
640                         mat[0] = 1 - (vec->vec[0] * vec->vec[0]);
641                         mat[1] = -(vec->vec[0] * vec->vec[1]);
642                         mat[2] = -(vec->vec[0] * vec->vec[2]);
643                         mat[3] = -(vec->vec[0] * vec->vec[1]);
644                         mat[4] = 1 - (vec->vec[1] * vec->vec[1]);
645                         mat[5] = -(vec->vec[1] * vec->vec[2]);
646                         mat[6] = -(vec->vec[0] * vec->vec[2]);
647                         mat[7] = -(vec->vec[1] * vec->vec[2]);
648                         mat[8] = 1 - (vec->vec[2] * vec->vec[2]);
649                 } else {
650                         PyErr_SetString(PyExc_AttributeError, "Mathutils.OrthoProjectionMatrix(): unknown plane - expected: 'r' expected for axis designation\n");
651                         return NULL;
652                 }
653         }
654         if(matSize == 4) {
655                 //resize matrix
656                 mat[10] = mat[8];
657                 mat[9] = mat[7];
658                 mat[8] = mat[6];
659                 mat[7] = 0.0f;
660                 mat[6] = mat[5];
661                 mat[5] = mat[4];
662                 mat[4] = mat[3];
663                 mat[3] = 0.0f;
664         }
665         //pass to matrix creation
666         return newMatrixObject(mat, matSize, matSize, Py_NEW, NULL);
667 }
668 //----------------------------------Mathutils.ShearMatrix() -------------
669 //creates a shear matrix
670 static PyObject *M_Mathutils_ShearMatrix(PyObject * self, PyObject * args)
671 {
672         int matSize;
673         char *plane;
674         float factor;
675         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
676                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
677
678         if(!PyArg_ParseTuple(args, "sfi", &plane, &factor, &matSize)) {
679                 PyErr_SetString(PyExc_TypeError,"Mathutils.ShearMatrix(): expected string float and int\n");
680                 return NULL;
681         }
682         if(matSize != 2 && matSize != 3 && matSize != 4) {
683                 PyErr_SetString(PyExc_AttributeError,"Mathutils.ShearMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
684                 return NULL;
685         }
686
687         if(((strcmp(plane, "x") == 0) || (strcmp(plane, "X") == 0))
688             && matSize == 2) {
689                 mat[0] = 1.0f;
690                 mat[2] = factor;
691                 mat[3] = 1.0f;
692         } else if(((strcmp(plane, "y") == 0)
693                      || (strcmp(plane, "Y") == 0)) && matSize == 2) {
694                 mat[0] = 1.0f;
695                 mat[1] = factor;
696                 mat[3] = 1.0f;
697         } else if(((strcmp(plane, "xy") == 0)
698                      || (strcmp(plane, "XY") == 0)) && matSize > 2) {
699                 mat[0] = 1.0f;
700                 mat[4] = 1.0f;
701                 mat[6] = factor;
702                 mat[7] = factor;
703         } else if(((strcmp(plane, "xz") == 0)
704                      || (strcmp(plane, "XZ") == 0)) && matSize > 2) {
705                 mat[0] = 1.0f;
706                 mat[3] = factor;
707                 mat[4] = 1.0f;
708                 mat[5] = factor;
709                 mat[8] = 1.0f;
710         } else if(((strcmp(plane, "yz") == 0)
711                      || (strcmp(plane, "YZ") == 0)) && matSize > 2) {
712                 mat[0] = 1.0f;
713                 mat[1] = factor;
714                 mat[2] = factor;
715                 mat[4] = 1.0f;
716                 mat[8] = 1.0f;
717         } else {
718                 PyErr_SetString(PyExc_AttributeError, "Mathutils.ShearMatrix(): expected: x, y, xy, xz, yz or wrong matrix size for shearing plane\n");
719                 return NULL;
720         }
721         if(matSize == 4) {
722                 //resize matrix
723                 mat[10] = mat[8];
724                 mat[9] = mat[7];
725                 mat[8] = mat[6];
726                 mat[7] = 0.0f;
727                 mat[6] = mat[5];
728                 mat[5] = mat[4];
729                 mat[4] = mat[3];
730                 mat[3] = 0.0f;
731         }
732         //pass to matrix creation
733         return newMatrixObject(mat, matSize, matSize, Py_NEW, NULL);
734 }
735 //----------------------------------QUATERNION FUNCTIONS-----------------
736
737 //----------------------------------Mathutils.DifferenceQuats() ---------
738 //returns the difference between 2 quaternions
739 static PyObject *M_Mathutils_DifferenceQuats(PyObject * self, PyObject * args)
740 {
741         QuaternionObject *quatU = NULL, *quatV = NULL;
742         float quat[4], tempQuat[4];
743         double dot = 0.0f;
744         int x;
745
746         if(!PyArg_ParseTuple(args, "O!O!", &quaternion_Type, &quatU, &quaternion_Type, &quatV)) {
747                 PyErr_SetString(PyExc_TypeError, "Mathutils.DifferenceQuats(): expected Quaternion types");
748                 return NULL;
749         }
750
751         if(!BaseMath_ReadCallback(quatU) || !BaseMath_ReadCallback(quatV))
752                 return NULL;
753
754         tempQuat[0] = quatU->quat[0];
755         tempQuat[1] = -quatU->quat[1];
756         tempQuat[2] = -quatU->quat[2];
757         tempQuat[3] = -quatU->quat[3];
758
759         dot = sqrt(tempQuat[0] * tempQuat[0] + tempQuat[1] *  tempQuat[1] +
760                                tempQuat[2] * tempQuat[2] + tempQuat[3] * tempQuat[3]);
761
762         for(x = 0; x < 4; x++) {
763                 tempQuat[x] /= (float)(dot * dot);
764         }
765         mul_qt_qtqt(quat, tempQuat, quatV->quat);
766         return newQuaternionObject(quat, Py_NEW, NULL);
767 }
768 //----------------------------------Mathutils.Slerp() ------------------
769 //attemps to interpolate 2 quaternions and return the result
770 static PyObject *M_Mathutils_Slerp(PyObject * self, PyObject * args)
771 {
772         QuaternionObject *quatU = NULL, *quatV = NULL;
773         float quat[4], quat_u[4], quat_v[4], param;
774         double x, y, dot, sinT, angle, IsinT;
775         int z;
776
777         if(!PyArg_ParseTuple(args, "O!O!f", &quaternion_Type, &quatU, &quaternion_Type, &quatV, &param)) {
778                 PyErr_SetString(PyExc_TypeError, "Mathutils.Slerp(): expected Quaternion types and float");
779                 return NULL;
780         }
781
782         if(!BaseMath_ReadCallback(quatU) || !BaseMath_ReadCallback(quatV))
783                 return NULL;
784
785         if(param > 1.0f || param < 0.0f) {
786                 PyErr_SetString(PyExc_AttributeError, "Mathutils.Slerp(): interpolation factor must be between 0.0 and 1.0");
787                 return NULL;
788         }
789
790         //copy quats
791         for(z = 0; z < 4; z++){
792                 quat_u[z] = quatU->quat[z];
793                 quat_v[z] = quatV->quat[z];
794         }
795
796         //dot product
797         dot = quat_u[0] * quat_v[0] + quat_u[1] * quat_v[1] +
798                 quat_u[2] * quat_v[2] + quat_u[3] * quat_v[3];
799
800         //if negative negate a quat (shortest arc)
801         if(dot < 0.0f) {
802                 quat_v[0] = -quat_v[0];
803                 quat_v[1] = -quat_v[1];
804                 quat_v[2] = -quat_v[2];
805                 quat_v[3] = -quat_v[3];
806                 dot = -dot;
807         }
808         if(dot > .99999f) { //very close
809                 x = 1.0f - param;
810                 y = param;
811         } else {
812                 //calculate sin of angle
813                 sinT = sqrt(1.0f - (dot * dot));
814                 //calculate angle
815                 angle = atan2(sinT, dot);
816                 //caluculate inverse of sin(theta)
817                 IsinT = 1.0f / sinT;
818                 x = sin((1.0f - param) * angle) * IsinT;
819                 y = sin(param * angle) * IsinT;
820         }
821         //interpolate
822         quat[0] = (float)(quat_u[0] * x + quat_v[0] * y);
823         quat[1] = (float)(quat_u[1] * x + quat_v[1] * y);
824         quat[2] = (float)(quat_u[2] * x + quat_v[2] * y);
825         quat[3] = (float)(quat_u[3] * x + quat_v[3] * y);
826
827         return newQuaternionObject(quat, Py_NEW, NULL);
828 }
829 //----------------------------------EULER FUNCTIONS----------------------
830 //---------------------------------INTERSECTION FUNCTIONS--------------------
831 //----------------------------------Mathutils.Intersect() -------------------
832 static PyObject *M_Mathutils_Intersect( PyObject * self, PyObject * args )
833 {
834         VectorObject *ray, *ray_off, *vec1, *vec2, *vec3;
835         float dir[3], orig[3], v1[3], v2[3], v3[3], e1[3], e2[3], pvec[3], tvec[3], qvec[3];
836         float det, inv_det, u, v, t;
837         int clip = 1;
838
839         if(!PyArg_ParseTuple(args, "O!O!O!O!O!|i", &vector_Type, &vec1, &vector_Type, &vec2, &vector_Type, &vec3, &vector_Type, &ray, &vector_Type, &ray_off , &clip)) {
840                 PyErr_SetString( PyExc_TypeError, "expected 5 vector types\n" );
841                 return NULL;
842         }
843         if(vec1->size != 3 || vec2->size != 3 || vec3->size != 3 || ray->size != 3 || ray_off->size != 3) {
844                 PyErr_SetString( PyExc_TypeError, "only 3D vectors for all parameters\n");
845                 return NULL;
846         }
847
848         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2) || !BaseMath_ReadCallback(vec3) || !BaseMath_ReadCallback(ray) || !BaseMath_ReadCallback(ray_off))
849                 return NULL;
850         
851         VECCOPY(v1, vec1->vec);
852         VECCOPY(v2, vec2->vec);
853         VECCOPY(v3, vec3->vec);
854
855         VECCOPY(dir, ray->vec);
856         normalize_v3(dir);
857
858         VECCOPY(orig, ray_off->vec);
859
860         /* find vectors for two edges sharing v1 */
861         sub_v3_v3v3(e1, v2, v1);
862         sub_v3_v3v3(e2, v3, v1);
863
864         /* begin calculating determinant - also used to calculated U parameter */
865         cross_v3_v3v3(pvec, dir, e2);   
866
867         /* if determinant is near zero, ray lies in plane of triangle */
868         det = dot_v3v3(e1, pvec);
869
870         if (det > -0.000001 && det < 0.000001) {
871                 Py_RETURN_NONE;
872         }
873
874         inv_det = 1.0f / det;
875
876         /* calculate distance from v1 to ray origin */
877         sub_v3_v3v3(tvec, orig, v1);
878
879         /* calculate U parameter and test bounds */
880         u = dot_v3v3(tvec, pvec) * inv_det;
881         if (clip && (u < 0.0f || u > 1.0f)) {
882                 Py_RETURN_NONE;
883         }
884
885         /* prepare to test the V parameter */
886         cross_v3_v3v3(qvec, tvec, e1);
887
888         /* calculate V parameter and test bounds */
889         v = dot_v3v3(dir, qvec) * inv_det;
890
891         if (clip && (v < 0.0f || u + v > 1.0f)) {
892                 Py_RETURN_NONE;
893         }
894
895         /* calculate t, ray intersects triangle */
896         t = dot_v3v3(e2, qvec) * inv_det;
897
898         mul_v3_fl(dir, t);
899         add_v3_v3v3(pvec, orig, dir);
900
901         return newVectorObject(pvec, 3, Py_NEW, NULL);
902 }
903 //----------------------------------Mathutils.LineIntersect() -------------------
904 /* Line-Line intersection using algorithm from mathworld.wolfram.com */
905 static PyObject *M_Mathutils_LineIntersect( PyObject * self, PyObject * args )
906 {
907         PyObject * tuple;
908         VectorObject *vec1, *vec2, *vec3, *vec4;
909         float v1[3], v2[3], v3[3], v4[3], i1[3], i2[3];
910
911         if( !PyArg_ParseTuple( args, "O!O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2, &vector_Type, &vec3, &vector_Type, &vec4 ) ) {
912                 PyErr_SetString( PyExc_TypeError, "expected 4 vector types\n" );
913                 return NULL;
914         }
915         if( vec1->size != vec2->size || vec1->size != vec3->size || vec3->size != vec2->size) {
916                 PyErr_SetString( PyExc_TypeError,"vectors must be of the same size\n" );
917                 return NULL;
918         }
919         
920         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2) || !BaseMath_ReadCallback(vec3) || !BaseMath_ReadCallback(vec4))
921                 return NULL;
922         
923         if( vec1->size == 3 || vec1->size == 2) {
924                 int result;
925                 
926                 if (vec1->size == 3) {
927                         VECCOPY(v1, vec1->vec);
928                         VECCOPY(v2, vec2->vec);
929                         VECCOPY(v3, vec3->vec);
930                         VECCOPY(v4, vec4->vec);
931                 }
932                 else {
933                         v1[0] = vec1->vec[0];
934                         v1[1] = vec1->vec[1];
935                         v1[2] = 0.0f;
936
937                         v2[0] = vec2->vec[0];
938                         v2[1] = vec2->vec[1];
939                         v2[2] = 0.0f;
940
941                         v3[0] = vec3->vec[0];
942                         v3[1] = vec3->vec[1];
943                         v3[2] = 0.0f;
944
945                         v4[0] = vec4->vec[0];
946                         v4[1] = vec4->vec[1];
947                         v4[2] = 0.0f;
948                 }
949                 
950                 result = isect_line_line_v3(v1, v2, v3, v4, i1, i2);
951
952                 if (result == 0) {
953                         /* colinear */
954                         Py_RETURN_NONE;
955                 }
956                 else {
957                         tuple = PyTuple_New( 2 );
958                         PyTuple_SetItem( tuple, 0, newVectorObject(i1, vec1->size, Py_NEW, NULL) );
959                         PyTuple_SetItem( tuple, 1, newVectorObject(i2, vec1->size, Py_NEW, NULL) );
960                         return tuple;
961                 }
962         }
963         else {
964                 PyErr_SetString( PyExc_TypeError, "2D/3D vectors only\n" );
965                 return NULL;
966         }
967 }
968
969
970
971 //---------------------------------NORMALS FUNCTIONS--------------------
972 //----------------------------------Mathutils.QuadNormal() -------------------
973 static PyObject *M_Mathutils_QuadNormal( PyObject * self, PyObject * args )
974 {
975         VectorObject *vec1;
976         VectorObject *vec2;
977         VectorObject *vec3;
978         VectorObject *vec4;
979         float v1[3], v2[3], v3[3], v4[3], e1[3], e2[3], n1[3], n2[3];
980
981         if( !PyArg_ParseTuple( args, "O!O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2, &vector_Type, &vec3, &vector_Type, &vec4 ) ) {
982                 PyErr_SetString( PyExc_TypeError, "expected 4 vector types\n" );
983                 return NULL;
984         }
985         if( vec1->size != vec2->size || vec1->size != vec3->size || vec1->size != vec4->size) {
986                 PyErr_SetString( PyExc_TypeError,"vectors must be of the same size\n" );
987                 return NULL;
988         }
989         if( vec1->size != 3 ) {
990                 PyErr_SetString( PyExc_TypeError, "only 3D vectors\n" );
991                 return NULL;
992         }
993         
994         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2) || !BaseMath_ReadCallback(vec3) || !BaseMath_ReadCallback(vec4))
995                 return NULL;
996         
997         VECCOPY(v1, vec1->vec);
998         VECCOPY(v2, vec2->vec);
999         VECCOPY(v3, vec3->vec);
1000         VECCOPY(v4, vec4->vec);
1001
1002         /* find vectors for two edges sharing v2 */
1003         sub_v3_v3v3(e1, v1, v2);
1004         sub_v3_v3v3(e2, v3, v2);
1005
1006         cross_v3_v3v3(n1, e2, e1);
1007         normalize_v3(n1);
1008
1009         /* find vectors for two edges sharing v4 */
1010         sub_v3_v3v3(e1, v3, v4);
1011         sub_v3_v3v3(e2, v1, v4);
1012
1013         cross_v3_v3v3(n2, e2, e1);
1014         normalize_v3(n2);
1015
1016         /* adding and averaging the normals of both triangles */
1017         add_v3_v3v3(n1, n2, n1);
1018         normalize_v3(n1);
1019
1020         return newVectorObject(n1, 3, Py_NEW, NULL);
1021 }
1022
1023 //----------------------------Mathutils.TriangleNormal() -------------------
1024 static PyObject *M_Mathutils_TriangleNormal( PyObject * self, PyObject * args )
1025 {
1026         VectorObject *vec1, *vec2, *vec3;
1027         float v1[3], v2[3], v3[3], e1[3], e2[3], n[3];
1028
1029         if( !PyArg_ParseTuple( args, "O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2, &vector_Type, &vec3 ) ) {
1030                 PyErr_SetString( PyExc_TypeError, "expected 3 vector types\n" );
1031                 return NULL;
1032         }
1033         if( vec1->size != vec2->size || vec1->size != vec3->size ) {
1034                 PyErr_SetString( PyExc_TypeError, "vectors must be of the same size\n" );
1035                 return NULL;
1036         }
1037         if( vec1->size != 3 ) {
1038                 PyErr_SetString( PyExc_TypeError, "only 3D vectors\n" );
1039                 return NULL;
1040         }
1041         
1042         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2) || !BaseMath_ReadCallback(vec3))
1043                 return NULL;
1044
1045         VECCOPY(v1, vec1->vec);
1046         VECCOPY(v2, vec2->vec);
1047         VECCOPY(v3, vec3->vec);
1048
1049         /* find vectors for two edges sharing v2 */
1050         sub_v3_v3v3(e1, v1, v2);
1051         sub_v3_v3v3(e2, v3, v2);
1052
1053         cross_v3_v3v3(n, e2, e1);
1054         normalize_v3(n);
1055
1056         return newVectorObject(n, 3, Py_NEW, NULL);
1057 }
1058
1059 //--------------------------------- AREA FUNCTIONS--------------------
1060 //----------------------------------Mathutils.TriangleArea() -------------------
1061 static PyObject *M_Mathutils_TriangleArea( PyObject * self, PyObject * args )
1062 {
1063         VectorObject *vec1, *vec2, *vec3;
1064         float v1[3], v2[3], v3[3];
1065
1066         if( !PyArg_ParseTuple
1067             ( args, "O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2
1068                 , &vector_Type, &vec3 ) ) {
1069                 PyErr_SetString( PyExc_TypeError, "expected 3 vector types\n");
1070                 return NULL;
1071         }
1072         if( vec1->size != vec2->size || vec1->size != vec3->size ) {
1073                 PyErr_SetString( PyExc_TypeError, "vectors must be of the same size\n" );
1074                 return NULL;
1075         }
1076         
1077         if(!BaseMath_ReadCallback(vec1) || !BaseMath_ReadCallback(vec2) || !BaseMath_ReadCallback(vec3))
1078                 return NULL;
1079
1080         if (vec1->size == 3) {
1081                 VECCOPY(v1, vec1->vec);
1082                 VECCOPY(v2, vec2->vec);
1083                 VECCOPY(v3, vec3->vec);
1084
1085                 return PyFloat_FromDouble( area_tri_v3(v1, v2, v3) );
1086         }
1087         else if (vec1->size == 2) {
1088                 v1[0] = vec1->vec[0];
1089                 v1[1] = vec1->vec[1];
1090
1091                 v2[0] = vec2->vec[0];
1092                 v2[1] = vec2->vec[1];
1093
1094                 v3[0] = vec3->vec[0];
1095                 v3[1] = vec3->vec[1];
1096
1097                 return PyFloat_FromDouble( area_tri_v2(v1, v2, v3) );
1098         }
1099         else {
1100                 PyErr_SetString( PyExc_TypeError, "only 2D,3D vectors are supported\n" );
1101                 return NULL;
1102         }
1103 }
1104
1105 /* Utility functions */
1106
1107 /*---------------------- EXPP_FloatsAreEqual -------------------------
1108   Floating point comparisons 
1109   floatStep = number of representable floats allowable in between
1110    float A and float B to be considered equal. */
1111 int EXPP_FloatsAreEqual(float A, float B, int floatSteps)
1112 {
1113         int a, b, delta;
1114     assert(floatSteps > 0 && floatSteps < (4 * 1024 * 1024));
1115     a = *(int*)&A;
1116     if (a < 0)  
1117                 a = 0x80000000 - a;
1118     b = *(int*)&B;
1119     if (b < 0)  
1120                 b = 0x80000000 - b;
1121     delta = abs(a - b);
1122     if (delta <= floatSteps)    
1123                 return 1;
1124     return 0;
1125 }
1126 /*---------------------- EXPP_VectorsAreEqual -------------------------
1127   Builds on EXPP_FloatsAreEqual to test vectors */
1128 int EXPP_VectorsAreEqual(float *vecA, float *vecB, int size, int floatSteps)
1129 {
1130         int x;
1131         for (x=0; x< size; x++){
1132                 if (EXPP_FloatsAreEqual(vecA[x], vecB[x], floatSteps) == 0)
1133                         return 0;
1134         }
1135         return 1;
1136 }
1137
1138
1139 /* Mathutils Callbacks */
1140
1141 /* for mathutils internal use only, eventually should re-alloc but to start with we only have a few users */
1142 Mathutils_Callback *mathutils_callbacks[8] = {NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL};
1143
1144 int Mathutils_RegisterCallback(Mathutils_Callback *cb)
1145 {
1146         int i;
1147         
1148         /* find the first free slot */
1149         for(i= 0; mathutils_callbacks[i]; i++) {
1150                 if(mathutils_callbacks[i]==cb) /* alredy registered? */
1151                         return i;
1152         }
1153         
1154         mathutils_callbacks[i] = cb;
1155         return i;
1156 }
1157
1158 /* use macros to check for NULL */
1159 int _BaseMathObject_ReadCallback(BaseMathObject *self)
1160 {
1161         Mathutils_Callback *cb= mathutils_callbacks[self->cb_type];
1162         if(cb->get(self->cb_user, self->cb_subtype, self->data))
1163                 return 1;
1164
1165         PyErr_Format(PyExc_SystemError, "%s user has become invalid", Py_TYPE(self)->tp_name);
1166         return 0;
1167 }
1168
1169 int _BaseMathObject_WriteCallback(BaseMathObject *self)
1170 {
1171         Mathutils_Callback *cb= mathutils_callbacks[self->cb_type];
1172         if(cb->set(self->cb_user, self->cb_subtype, self->data))
1173                 return 1;
1174
1175         PyErr_Format(PyExc_SystemError, "%s user has become invalid", Py_TYPE(self)->tp_name);
1176         return 0;
1177 }
1178
1179 int _BaseMathObject_ReadIndexCallback(BaseMathObject *self, int index)
1180 {
1181         Mathutils_Callback *cb= mathutils_callbacks[self->cb_type];
1182         if(cb->get_index(self->cb_user, self->cb_subtype, self->data, index))
1183                 return 1;
1184
1185         PyErr_Format(PyExc_SystemError, "%s user has become invalid", Py_TYPE(self)->tp_name);
1186         return 0;
1187 }
1188
1189 int _BaseMathObject_WriteIndexCallback(BaseMathObject *self, int index)
1190 {
1191         Mathutils_Callback *cb= mathutils_callbacks[self->cb_type];
1192         if(cb->set_index(self->cb_user, self->cb_subtype, self->data, index))
1193                 return 1;
1194
1195         PyErr_Format(PyExc_SystemError, "%s user has become invalid", Py_TYPE(self)->tp_name);
1196         return 0;
1197 }
1198
1199 /* BaseMathObject generic functions for all mathutils types */
1200 PyObject *BaseMathObject_getOwner( BaseMathObject * self, void *type )
1201 {
1202         PyObject *ret= self->cb_user ? self->cb_user : Py_None;
1203         Py_INCREF(ret);
1204         return ret;
1205 }
1206
1207 PyObject *BaseMathObject_getWrapped( BaseMathObject *self, void *type )
1208 {
1209         return PyBool_FromLong((self->wrapped == Py_WRAP) ? 1:0);
1210 }
1211
1212 void BaseMathObject_dealloc(BaseMathObject * self)
1213 {
1214         /* only free non wrapped */
1215         if(self->wrapped != Py_WRAP)
1216                 PyMem_Free(self->data);
1217
1218         Py_XDECREF(self->cb_user);
1219         Py_TYPE(self)->tp_free(self); // PyObject_DEL(self); // breaks subtypes
1220 }
1221