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