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