* Added Main.add_object(type, name)
[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_Vector_doc[] = "() - create a new vector object from a list of floats";
40 static char M_Mathutils_Matrix_doc[] = "() - create a new matrix object from a list of floats";
41 static char M_Mathutils_Quaternion_doc[] = "() - create a quaternion from a list or an axis of rotation and an angle";
42 static char M_Mathutils_Euler_doc[] = "() - create and return a new euler object";
43 static char M_Mathutils_Rand_doc[] = "() - return a random number";
44 static char M_Mathutils_CrossVecs_doc[] = "() - returns a vector perpedicular to the 2 vectors crossed";
45 static char M_Mathutils_CopyVec_doc[] = "() - create a copy of vector";
46 static char M_Mathutils_DotVecs_doc[] = "() - return the dot product of two vectors";
47 static char M_Mathutils_AngleBetweenVecs_doc[] = "() - returns the angle between two vectors in degrees";
48 static char M_Mathutils_MidpointVecs_doc[] = "() - return the vector to the midpoint between two vectors";
49 static char M_Mathutils_MatMultVec_doc[] = "() - multiplies a matrix by a column vector";
50 static char M_Mathutils_VecMultMat_doc[] = "() - multiplies a row vector by a matrix";
51 static char M_Mathutils_ProjectVecs_doc[] =     "() - returns the projection vector from the projection of vecA onto vecB";
52 static char M_Mathutils_RotationMatrix_doc[] = "() - construct a rotation matrix from an angle and axis of rotation";
53 static char M_Mathutils_ScaleMatrix_doc[] =     "() - construct a scaling matrix from a scaling factor";
54 static char M_Mathutils_OrthoProjectionMatrix_doc[] = "() - construct a orthographic projection matrix from a selected plane";
55 static char M_Mathutils_ShearMatrix_doc[] = "() - construct a shearing matrix from a plane of shear and a shear factor";
56 static char M_Mathutils_CopyMat_doc[] = "() - create a copy of a matrix";
57 static char M_Mathutils_TranslationMatrix_doc[] = "(vec) - create a translation matrix from a vector";
58 static char M_Mathutils_CopyQuat_doc[] = "() - copy quatB to quatA";
59 static char M_Mathutils_CopyEuler_doc[] = "() - copy eulB to eultA";
60 static char M_Mathutils_CrossQuats_doc[] = "() - return the mutliplication of two quaternions";
61 static char M_Mathutils_DotQuats_doc[] = "() - return the dot product of two quaternions";
62 static char M_Mathutils_Slerp_doc[] = "() - returns the interpolation between two quaternions";
63 static char M_Mathutils_DifferenceQuats_doc[] = "() - return the angular displacment difference between two quats";
64 static char M_Mathutils_RotateEuler_doc[] = "() - rotate euler by an axis and angle";
65 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";
66 static char M_Mathutils_TriangleArea_doc[] = "(v1, v2, v3) - returns the area size of the 2D or 3D triangle defined";
67 static char M_Mathutils_TriangleNormal_doc[] = "(v1, v2, v3) - returns the normal of the 3D triangle defined";
68 static char M_Mathutils_QuadNormal_doc[] = "(v1, v2, v3, v4) - returns the normal of the 3D quad defined";
69 static char M_Mathutils_LineIntersect_doc[] = "(v1, v2, v3, v4) - returns a tuple with the points on each line respectively closest to the other";
70 //-----------------------METHOD DEFINITIONS ----------------------
71 struct PyMethodDef M_Mathutils_methods[] = {
72         {"Rand", (PyCFunction) M_Mathutils_Rand, METH_VARARGS, M_Mathutils_Rand_doc},
73         {"Vector", (PyCFunction) M_Mathutils_Vector, METH_VARARGS, M_Mathutils_Vector_doc},
74         {"CrossVecs", (PyCFunction) M_Mathutils_CrossVecs, METH_VARARGS, M_Mathutils_CrossVecs_doc},
75         {"DotVecs", (PyCFunction) M_Mathutils_DotVecs, METH_VARARGS, M_Mathutils_DotVecs_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         {"VecMultMat", (PyCFunction) M_Mathutils_VecMultMat, METH_VARARGS, M_Mathutils_VecMultMat_doc},
79         {"ProjectVecs", (PyCFunction) M_Mathutils_ProjectVecs, METH_VARARGS, M_Mathutils_ProjectVecs_doc},
80         {"CopyVec", (PyCFunction) M_Mathutils_CopyVec, METH_VARARGS, M_Mathutils_CopyVec_doc},
81         {"Matrix", (PyCFunction) M_Mathutils_Matrix, METH_VARARGS, M_Mathutils_Matrix_doc},
82         {"RotationMatrix", (PyCFunction) M_Mathutils_RotationMatrix, METH_VARARGS, M_Mathutils_RotationMatrix_doc},
83         {"ScaleMatrix", (PyCFunction) M_Mathutils_ScaleMatrix, METH_VARARGS, M_Mathutils_ScaleMatrix_doc},
84         {"ShearMatrix", (PyCFunction) M_Mathutils_ShearMatrix, METH_VARARGS, M_Mathutils_ShearMatrix_doc},
85         {"TranslationMatrix", (PyCFunction) M_Mathutils_TranslationMatrix, METH_O, M_Mathutils_TranslationMatrix_doc},
86         {"CopyMat", (PyCFunction) M_Mathutils_CopyMat, METH_VARARGS, M_Mathutils_CopyMat_doc},
87         {"OrthoProjectionMatrix", (PyCFunction) M_Mathutils_OrthoProjectionMatrix,  METH_VARARGS, M_Mathutils_OrthoProjectionMatrix_doc},
88         {"MatMultVec", (PyCFunction) M_Mathutils_MatMultVec, METH_VARARGS, M_Mathutils_MatMultVec_doc},
89         {"Quaternion", (PyCFunction) M_Mathutils_Quaternion, METH_VARARGS, M_Mathutils_Quaternion_doc},
90         {"CopyQuat", (PyCFunction) M_Mathutils_CopyQuat, METH_VARARGS, M_Mathutils_CopyQuat_doc},
91         {"CrossQuats", (PyCFunction) M_Mathutils_CrossQuats, METH_VARARGS, M_Mathutils_CrossQuats_doc},
92         {"DotQuats", (PyCFunction) M_Mathutils_DotQuats, METH_VARARGS, M_Mathutils_DotQuats_doc},
93         {"DifferenceQuats", (PyCFunction) M_Mathutils_DifferenceQuats, METH_VARARGS,M_Mathutils_DifferenceQuats_doc},
94         {"Slerp", (PyCFunction) M_Mathutils_Slerp, METH_VARARGS, M_Mathutils_Slerp_doc},
95         {"Euler", (PyCFunction) M_Mathutils_Euler, METH_VARARGS, M_Mathutils_Euler_doc},
96         {"CopyEuler", (PyCFunction) M_Mathutils_CopyEuler, METH_VARARGS, M_Mathutils_CopyEuler_doc},
97         {"RotateEuler", (PyCFunction) M_Mathutils_RotateEuler, METH_VARARGS, M_Mathutils_RotateEuler_doc},
98         {"Intersect", ( PyCFunction ) M_Mathutils_Intersect, METH_VARARGS, M_Mathutils_Intersect_doc},
99         {"TriangleArea", ( PyCFunction ) M_Mathutils_TriangleArea, METH_VARARGS, M_Mathutils_TriangleArea_doc},
100         {"TriangleNormal", ( PyCFunction ) M_Mathutils_TriangleNormal, METH_VARARGS, M_Mathutils_TriangleNormal_doc},
101         {"QuadNormal", ( PyCFunction ) M_Mathutils_QuadNormal, METH_VARARGS, M_Mathutils_QuadNormal_doc},
102         {"LineIntersect", ( PyCFunction ) M_Mathutils_LineIntersect, METH_VARARGS, M_Mathutils_LineIntersect_doc},
103         {NULL, NULL, 0, NULL}
104 };
105 /*----------------------------MODULE INIT-------------------------*/
106 /* from can be Blender.Mathutils or GameLogic.Mathutils for the BGE */
107
108 #if (PY_VERSION_HEX >= 0x03000000)
109 static struct PyModuleDef M_Mathutils_module_def = {
110         {}, /* m_base */
111         "Mathutils",  /* m_name */
112         M_Mathutils_doc,  /* m_doc */
113         0,  /* m_size */
114         M_Mathutils_methods,  /* m_methods */
115         0,  /* m_reload */
116         0,  /* m_traverse */
117         0,  /* m_clear */
118         0,  /* m_free */
119 };
120 #endif
121
122 PyObject *Mathutils_Init(const char *from)
123 {
124         PyObject *submodule;
125
126         //seed the generator for the rand function
127         BLI_srand((unsigned int) (PIL_check_seconds_timer() * 0x7FFFFFFF));
128         
129         if( PyType_Ready( &vector_Type ) < 0 )
130                 return NULL;
131         if( PyType_Ready( &matrix_Type ) < 0 )
132                 return NULL;    
133         if( PyType_Ready( &euler_Type ) < 0 )
134                 return NULL;
135         if( PyType_Ready( &quaternion_Type ) < 0 )
136                 return NULL;
137         
138 #if (PY_VERSION_HEX >= 0x03000000)
139         submodule = PyModule_Create(&M_Mathutils_module_def);
140         PyDict_SetItemString(PySys_GetObject("modules"), M_Mathutils_module_def.m_name, submodule);
141 #else
142         submodule = Py_InitModule3(from, M_Mathutils_methods, M_Mathutils_doc);
143 #endif
144         
145         return (submodule);
146 }
147
148 //-----------------------------METHODS----------------------------
149 //----------------column_vector_multiplication (internal)---------
150 //COLUMN VECTOR Multiplication (Matrix X Vector)
151 // [1][2][3]   [a]
152 // [4][5][6] * [b]
153 // [7][8][9]   [c]
154 //vector/matrix multiplication IS NOT COMMUTATIVE!!!!
155 PyObject *column_vector_multiplication(MatrixObject * mat, VectorObject* vec)
156 {
157         float vecNew[4], vecCopy[4];
158         double dot = 0.0f;
159         int x, y, z = 0;
160
161         if(mat->rowSize != vec->size){
162                 if(mat->rowSize == 4 && vec->size != 3){
163                         PyErr_SetString(PyExc_AttributeError, "matrix * vector: matrix row size and vector size must be the same");
164                         return NULL;
165                 }else{
166                         vecCopy[3] = 1.0f;
167                 }
168         }
169
170         for(x = 0; x < vec->size; x++){
171                 vecCopy[x] = vec->vec[x];
172                 }
173
174         for(x = 0; x < mat->rowSize; x++) {
175                 for(y = 0; y < mat->colSize; y++) {
176                         dot += mat->matrix[x][y] * vecCopy[y];
177                 }
178                 vecNew[z++] = (float)dot;
179                 dot = 0.0f;
180         }
181         return newVectorObject(vecNew, vec->size, Py_NEW);
182 }
183
184 //-----------------row_vector_multiplication (internal)-----------
185 //ROW VECTOR Multiplication - Vector X Matrix
186 //[x][y][z] *  [1][2][3]
187 //             [4][5][6]
188 //             [7][8][9]
189 //vector/matrix multiplication IS NOT COMMUTATIVE!!!!
190 PyObject *row_vector_multiplication(VectorObject* vec, MatrixObject * mat)
191 {
192         float vecNew[4], vecCopy[4];
193         double dot = 0.0f;
194         int x, y, z = 0, vec_size = vec->size;
195
196         if(mat->colSize != vec_size){
197                 if(mat->rowSize == 4 && vec_size != 3){
198                         PyErr_SetString(PyExc_AttributeError, "vector * matrix: matrix column size and the vector size must be the same");
199                         return NULL;
200                 }else{
201                         vecCopy[3] = 1.0f;
202                 }
203         }
204         
205         for(x = 0; x < vec_size; x++){
206                 vecCopy[x] = vec->vec[x];
207         }
208
209         //muliplication
210         for(x = 0; x < mat->colSize; x++) {
211                 for(y = 0; y < mat->rowSize; y++) {
212                         dot += mat->matrix[y][x] * vecCopy[y];
213                 }
214                 vecNew[z++] = (float)dot;
215                 dot = 0.0f;
216         }
217         return newVectorObject(vecNew, vec_size, Py_NEW);
218 }
219
220 //-----------------quat_rotation (internal)-----------
221 //This function multiplies a vector/point * quat or vice versa
222 //to rotate the point/vector by the quaternion
223 //arguments should all be 3D
224 PyObject *quat_rotation(PyObject *arg1, PyObject *arg2)
225 {
226         float rot[3];
227         QuaternionObject *quat = NULL;
228         VectorObject *vec = NULL;
229
230         if(QuaternionObject_Check(arg1)){
231                 quat = (QuaternionObject*)arg1;
232                 if(VectorObject_Check(arg2)){
233                         vec = (VectorObject*)arg2;
234                         rot[0] = quat->quat[0]*quat->quat[0]*vec->vec[0] + 2*quat->quat[2]*quat->quat[0]*vec->vec[2] - 
235                                 2*quat->quat[3]*quat->quat[0]*vec->vec[1] + quat->quat[1]*quat->quat[1]*vec->vec[0] + 
236                                 2*quat->quat[2]*quat->quat[1]*vec->vec[1] + 2*quat->quat[3]*quat->quat[1]*vec->vec[2] - 
237                                 quat->quat[3]*quat->quat[3]*vec->vec[0] - quat->quat[2]*quat->quat[2]*vec->vec[0];
238                         rot[1] = 2*quat->quat[1]*quat->quat[2]*vec->vec[0] + quat->quat[2]*quat->quat[2]*vec->vec[1] + 
239                                 2*quat->quat[3]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[3]*vec->vec[0] - 
240                                 quat->quat[3]*quat->quat[3]*vec->vec[1] + quat->quat[0]*quat->quat[0]*vec->vec[1] - 
241                                 2*quat->quat[1]*quat->quat[0]*vec->vec[2] - quat->quat[1]*quat->quat[1]*vec->vec[1];
242                         rot[2] = 2*quat->quat[1]*quat->quat[3]*vec->vec[0] + 2*quat->quat[2]*quat->quat[3]*vec->vec[1] + 
243                                 quat->quat[3]*quat->quat[3]*vec->vec[2] - 2*quat->quat[0]*quat->quat[2]*vec->vec[0] - 
244                                 quat->quat[2]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[1]*vec->vec[1] - 
245                                 quat->quat[1]*quat->quat[1]*vec->vec[2] + quat->quat[0]*quat->quat[0]*vec->vec[2];
246                         return newVectorObject(rot, 3, Py_NEW);
247                 }
248         }else if(VectorObject_Check(arg1)){
249                 vec = (VectorObject*)arg1;
250                 if(QuaternionObject_Check(arg2)){
251                         quat = (QuaternionObject*)arg2;
252                         rot[0] = quat->quat[0]*quat->quat[0]*vec->vec[0] + 2*quat->quat[2]*quat->quat[0]*vec->vec[2] - 
253                                 2*quat->quat[3]*quat->quat[0]*vec->vec[1] + quat->quat[1]*quat->quat[1]*vec->vec[0] + 
254                                 2*quat->quat[2]*quat->quat[1]*vec->vec[1] + 2*quat->quat[3]*quat->quat[1]*vec->vec[2] - 
255                                 quat->quat[3]*quat->quat[3]*vec->vec[0] - quat->quat[2]*quat->quat[2]*vec->vec[0];
256                         rot[1] = 2*quat->quat[1]*quat->quat[2]*vec->vec[0] + quat->quat[2]*quat->quat[2]*vec->vec[1] + 
257                                 2*quat->quat[3]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[3]*vec->vec[0] - 
258                                 quat->quat[3]*quat->quat[3]*vec->vec[1] + quat->quat[0]*quat->quat[0]*vec->vec[1] - 
259                                 2*quat->quat[1]*quat->quat[0]*vec->vec[2] - quat->quat[1]*quat->quat[1]*vec->vec[1];
260                         rot[2] = 2*quat->quat[1]*quat->quat[3]*vec->vec[0] + 2*quat->quat[2]*quat->quat[3]*vec->vec[1] + 
261                                 quat->quat[3]*quat->quat[3]*vec->vec[2] - 2*quat->quat[0]*quat->quat[2]*vec->vec[0] - 
262                                 quat->quat[2]*quat->quat[2]*vec->vec[2] + 2*quat->quat[0]*quat->quat[1]*vec->vec[1] - 
263                                 quat->quat[1]*quat->quat[1]*vec->vec[2] + quat->quat[0]*quat->quat[0]*vec->vec[2];
264                         return newVectorObject(rot, 3, Py_NEW);
265                 }
266         }
267
268         PyErr_SetString(PyExc_RuntimeError, "quat_rotation(internal): internal problem rotating vector/point\n");
269         return NULL;
270         
271 }
272
273 //----------------------------------Mathutils.Rand() --------------------
274 //returns a random number between a high and low value
275 PyObject *M_Mathutils_Rand(PyObject * self, PyObject * args)
276 {
277         float high, low, range;
278         double drand;
279         //initializers
280         high = 1.0;
281         low = 0.0;
282
283         if(!PyArg_ParseTuple(args, "|ff", &low, &high)) {
284                 PyErr_SetString(PyExc_TypeError, "Mathutils.Rand(): expected nothing or optional (float, float)\n");
285                 return NULL;
286         }
287
288         if((high < low) || (high < 0 && low > 0)) {
289                 PyErr_SetString(PyExc_ValueError, "Mathutils.Rand(): high value should be larger than low value\n");
290                 return NULL;
291         }
292         //get the random number 0 - 1
293         drand = BLI_drand();
294
295         //set it to range
296         range = high - low;
297         drand = drand * range;
298         drand = drand + low;
299
300         return PyFloat_FromDouble(drand);
301 }
302 //----------------------------------VECTOR FUNCTIONS---------------------
303 //----------------------------------Mathutils.Vector() ------------------
304 // Supports 2D, 3D, and 4D vector objects both int and float values
305 // accepted. Mixed float and int values accepted. Ints are parsed to float 
306 PyObject *M_Mathutils_Vector(PyObject * self, PyObject * args)
307 {
308         PyObject *listObject = NULL;
309         int size, i;
310         float vec[4], f;
311         PyObject *v;
312
313         size = PySequence_Length(args);
314         if (size == 1) {
315                 listObject = PySequence_GetItem(args, 0);
316                 if (PySequence_Check(listObject)) {
317                         size = PySequence_Length(listObject);
318                 } else { // Single argument was not a sequence
319                         Py_XDECREF(listObject);
320                         PyErr_SetString(PyExc_TypeError, "Mathutils.Vector(): 2-4 floats or ints expected (optionally in a sequence)\n");
321                         return NULL;
322                 }
323         } else if (size == 0) {
324                 //returns a new empty 3d vector
325                 return newVectorObject(NULL, 3, Py_NEW); 
326         } else {
327                 Py_INCREF(args);
328                 listObject = args;
329         }
330
331         if (size<2 || size>4) { // Invalid vector size
332                 Py_XDECREF(listObject);
333                 PyErr_SetString(PyExc_AttributeError, "Mathutils.Vector(): 2-4 floats or ints expected (optionally in a sequence)\n");
334                 return NULL;
335         }
336
337         for (i=0; i<size; i++) {
338                 v=PySequence_GetItem(listObject, i);
339                 if (v==NULL) { // Failed to read sequence
340                         Py_XDECREF(listObject);
341                         PyErr_SetString(PyExc_RuntimeError, "Mathutils.Vector(): 2-4 floats or ints expected (optionally in a sequence)\n");
342                         return NULL;
343                 }
344
345                 f= PyFloat_AsDouble(v);
346                 if(f==-1 && PyErr_Occurred()) { // parsed item not a number
347                         Py_DECREF(v);
348                         Py_XDECREF(listObject);
349                         PyErr_SetString(PyExc_TypeError, "Mathutils.Vector(): 2-4 floats or ints expected (optionally in a sequence)\n");
350                         return NULL;
351                 }
352
353                 vec[i]= f;
354                 Py_DECREF(v);
355         }
356         Py_DECREF(listObject);
357         return newVectorObject(vec, size, Py_NEW);
358 }
359 //----------------------------------Mathutils.CrossVecs() ---------------
360 //finds perpendicular vector - only 3D is supported
361 PyObject *M_Mathutils_CrossVecs(PyObject * self, PyObject * args)
362 {
363         PyObject *vecCross = NULL;
364         VectorObject *vec1 = NULL, *vec2 = NULL;
365
366         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2)) {
367                 PyErr_SetString(PyExc_TypeError, "Mathutils.CrossVecs(): expects (2) 3D vector objects\n");
368                 return NULL;
369         }
370         
371         if(vec1->size != 3 || vec2->size != 3) {
372                 PyErr_SetString(PyExc_AttributeError, "Mathutils.CrossVecs(): expects (2) 3D vector objects\n");
373                 return NULL;
374         }
375         vecCross = newVectorObject(NULL, 3, Py_NEW);
376         Crossf(((VectorObject*)vecCross)->vec, vec1->vec, vec2->vec);
377         return vecCross;
378 }
379 //----------------------------------Mathutils.DotVec() -------------------
380 //calculates the dot product of two vectors
381 PyObject *M_Mathutils_DotVecs(PyObject * self, PyObject * args)
382 {
383         VectorObject *vec1 = NULL, *vec2 = NULL;
384         double dot = 0.0f;
385         int x;
386
387         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2)) {
388                 PyErr_SetString(PyExc_TypeError, "Mathutils.DotVecs(): expects (2) vector objects of the same size\n");
389                 return NULL;
390         }
391         
392         if(vec1->size != vec2->size) {
393                 PyErr_SetString(PyExc_AttributeError, "Mathutils.DotVecs(): expects (2) vector objects of the same size\n");
394                 return NULL;
395         }
396
397         for(x = 0; x < vec1->size; x++) {
398                 dot += vec1->vec[x] * vec2->vec[x];
399         }
400         return PyFloat_FromDouble(dot);
401 }
402 //----------------------------------Mathutils.AngleBetweenVecs() ---------
403 //calculates the angle between 2 vectors
404 PyObject *M_Mathutils_AngleBetweenVecs(PyObject * self, PyObject * args)
405 {
406         VectorObject *vec1 = NULL, *vec2 = NULL;
407         double dot = 0.0f, angleRads, test_v1 = 0.0f, test_v2 = 0.0f;
408         int x, size;
409
410         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2))
411                 goto AttributeError1; //not vectors
412         if(vec1->size != vec2->size)
413                 goto AttributeError1; //bad sizes
414
415         //since size is the same....
416         size = vec1->size;
417
418         for(x = 0; x < size; x++) {
419                 test_v1 += vec1->vec[x] * vec1->vec[x];
420                 test_v2 += vec2->vec[x] * vec2->vec[x];
421         }
422         if (!test_v1 || !test_v2){
423                 goto AttributeError2; //zero-length vector
424         }
425
426         //dot product
427         for(x = 0; x < size; x++) {
428                 dot += vec1->vec[x] * vec2->vec[x];
429         }
430         dot /= (sqrt(test_v1) * sqrt(test_v2));
431
432         angleRads = (double)saacos(dot);
433
434         return PyFloat_FromDouble(angleRads * (180/ Py_PI));
435
436 AttributeError1:
437         PyErr_SetString(PyExc_AttributeError, "Mathutils.AngleBetweenVecs(): expects (2) VECTOR objects of the same size\n");
438         return NULL;
439
440 AttributeError2:
441         PyErr_SetString(PyExc_AttributeError, "Mathutils.AngleBetweenVecs(): zero length vectors are not acceptable arguments\n");
442         return NULL;
443 }
444 //----------------------------------Mathutils.MidpointVecs() -------------
445 //calculates the midpoint between 2 vectors
446 PyObject *M_Mathutils_MidpointVecs(PyObject * self, PyObject * args)
447 {
448         VectorObject *vec1 = NULL, *vec2 = NULL;
449         float vec[4];
450         int x;
451         
452         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2)) {
453                 PyErr_SetString(PyExc_TypeError, "Mathutils.MidpointVecs(): expects (2) vector objects of the same size\n");
454                 return NULL;
455         }
456         if(vec1->size != vec2->size) {
457                 PyErr_SetString(PyExc_AttributeError, "Mathutils.MidpointVecs(): expects (2) vector objects of the same size\n");
458                 return NULL;
459         }
460
461         for(x = 0; x < vec1->size; x++) {
462                 vec[x] = 0.5f * (vec1->vec[x] + vec2->vec[x]);
463         }
464         return newVectorObject(vec, vec1->size, Py_NEW);
465 }
466 //----------------------------------Mathutils.ProjectVecs() -------------
467 //projects vector 1 onto vector 2
468 PyObject *M_Mathutils_ProjectVecs(PyObject * self, PyObject * args)
469 {
470         VectorObject *vec1 = NULL, *vec2 = NULL;
471         float vec[4]; 
472         double dot = 0.0f, dot2 = 0.0f;
473         int x, size;
474
475         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec1, &vector_Type, &vec2)) {
476                 PyErr_SetString(PyExc_TypeError, "Mathutils.ProjectVecs(): expects (2) vector objects of the same size\n");
477                 return NULL;
478         }
479         if(vec1->size != vec2->size) {
480                 PyErr_SetString(PyExc_AttributeError, "Mathutils.ProjectVecs(): expects (2) vector objects of the same size\n");
481                 return NULL;
482         }
483
484         //since they are the same size...
485         size = vec1->size;
486
487         //get dot products
488         for(x = 0; x < size; x++) {
489                 dot += vec1->vec[x] * vec2->vec[x];
490                 dot2 += vec2->vec[x] * vec2->vec[x];
491         }
492         //projection
493         dot /= dot2;
494         for(x = 0; x < size; x++) {
495                 vec[x] = (float)(dot * vec2->vec[x]);
496         }
497         return newVectorObject(vec, size, Py_NEW);
498 }
499 //----------------------------------MATRIX FUNCTIONS--------------------
500 //----------------------------------Mathutils.Matrix() -----------------
501 //mat is a 1D array of floats - row[0][0],row[0][1], row[1][0], etc.
502 //create a new matrix type
503 PyObject *M_Mathutils_Matrix(PyObject * self, PyObject * args)
504 {
505         PyObject *listObject = NULL;
506         PyObject *argObject, *m, *s, *f;
507         MatrixObject *mat;
508         int argSize, seqSize = 0, i, j;
509         float matrix[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         argSize = PySequence_Length(args);
513         if(argSize > 4){        //bad arg nums
514                 PyErr_SetString(PyExc_AttributeError, "Mathutils.Matrix(): expects 0-4 numeric sequences of the same size\n");
515                 return NULL;
516         } else if (argSize == 0) { //return empty 4D matrix
517                 return (PyObject *) newMatrixObject(NULL, 4, 4, Py_NEW);
518         }else if (argSize == 1){
519                 //copy constructor for matrix objects
520                 argObject = PySequence_GetItem(args, 0);
521                 if(MatrixObject_Check(argObject)){
522                         mat = (MatrixObject*)argObject;
523
524                         argSize = mat->rowSize; //rows
525                         seqSize = mat->colSize; //col
526                         for(i = 0; i < (seqSize * argSize); i++){
527                                 matrix[i] = mat->contigPtr[i];
528                         }
529                 }
530                 Py_DECREF(argObject);
531         }else{ //2-4 arguments (all seqs? all same size?)
532                 for(i =0; i < argSize; i++){
533                         argObject = PySequence_GetItem(args, i);
534                         if (PySequence_Check(argObject)) { //seq?
535                                 if(seqSize){ //0 at first
536                                         if(PySequence_Length(argObject) != seqSize){ //seq size not same
537                                                 Py_DECREF(argObject);
538                                                 PyErr_SetString(PyExc_AttributeError, "Mathutils.Matrix(): expects 0-4 numeric sequences of the same size\n");
539                                                 return NULL;
540                                         }
541                                 }
542                                 seqSize = PySequence_Length(argObject);
543                         }else{ //arg not a sequence
544                                 Py_XDECREF(argObject);
545                                 PyErr_SetString(PyExc_TypeError, "Mathutils.Matrix(): expects 0-4 numeric sequences of the same size\n");
546                                 return NULL;
547                         }
548                         Py_DECREF(argObject);
549                 }
550                 //all is well... let's continue parsing
551                 listObject = args;
552                 for (i = 0; i < argSize; i++){
553                         m = PySequence_GetItem(listObject, i);
554                         if (m == NULL) { // Failed to read sequence
555                                 PyErr_SetString(PyExc_RuntimeError, "Mathutils.Matrix(): failed to parse arguments...\n");
556                                 return NULL;
557                         }
558
559                         for (j = 0; j < seqSize; j++) {
560                                 s = PySequence_GetItem(m, j);
561                                 if (s == NULL) { // Failed to read sequence
562                                         Py_DECREF(m);
563                                         PyErr_SetString(PyExc_RuntimeError, "Mathutils.Matrix(): failed to parse arguments...\n");
564                                         return NULL;
565                                 }
566
567                                 f = PyNumber_Float(s);
568                                 if(f == NULL) { // parsed item is not a number
569                                         Py_DECREF(m);
570                                         Py_DECREF(s);
571                                         PyErr_SetString(PyExc_AttributeError, "Mathutils.Matrix(): expects 0-4 numeric sequences of the same size\n");
572                                         return NULL;
573                                 }
574
575                                 matrix[(seqSize*i)+j]=(float)PyFloat_AS_DOUBLE(f);
576                                 Py_DECREF(f);
577                                 Py_DECREF(s);
578                         }
579                         Py_DECREF(m);
580                 }
581         }
582         return newMatrixObject(matrix, argSize, seqSize, Py_NEW);
583 }
584 //----------------------------------Mathutils.RotationMatrix() ----------
585 //mat is a 1D array of floats - row[0][0],row[0][1], row[1][0], etc.
586 //creates a rotation matrix
587 PyObject *M_Mathutils_RotationMatrix(PyObject * self, PyObject * args)
588 {
589         VectorObject *vec = NULL;
590         char *axis = NULL;
591         int matSize;
592         float angle = 0.0f, norm = 0.0f, cosAngle = 0.0f, sinAngle = 0.0f;
593         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
594                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
595
596         if(!PyArg_ParseTuple(args, "fi|sO!", &angle, &matSize, &axis, &vector_Type, &vec)) {
597                 PyErr_SetString(PyExc_TypeError, "Mathutils.RotationMatrix(): expected float int and optional string and vector\n");
598                 return NULL;
599         }
600         
601         /* Clamp to -360:360 */
602         while (angle<-360.0f)
603                 angle+=360.0;
604         while (angle>360.0f)
605                 angle-=360.0;
606         
607         if(matSize != 2 && matSize != 3 && matSize != 4) {
608                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
609                 return NULL;
610         }
611         if(matSize == 2 && (axis != NULL || vec != NULL)) {
612                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): cannot create a 2x2 rotation matrix around arbitrary axis\n");
613                 return NULL;
614         }
615         if((matSize == 3 || matSize == 4) && axis == NULL) {
616                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): please choose an axis of rotation for 3d and 4d matrices\n");
617                 return NULL;
618         }
619         if(axis) {
620                 if(((strcmp(axis, "r") == 0) || (strcmp(axis, "R") == 0)) && vec == NULL) {
621                         PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): please define the arbitrary axis of rotation\n");
622                         return NULL;
623                 }
624         }
625         if(vec) {
626                 if(vec->size != 3) {
627                         PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): the arbitrary axis must be a 3D vector\n");
628                         return NULL;
629                 }
630         }
631         //convert to radians
632         angle = angle * (float) (Py_PI / 180);
633         if(axis == NULL && matSize == 2) {
634                 //2D rotation matrix
635                 mat[0] = (float) cos (angle);
636                 mat[1] = (float) sin (angle);
637                 mat[2] = -((float) sin(angle));
638                 mat[3] = (float) cos(angle);
639         } else if((strcmp(axis, "x") == 0) || (strcmp(axis, "X") == 0)) {
640                 //rotation around X
641                 mat[0] = 1.0f;
642                 mat[4] = (float) cos(angle);
643                 mat[5] = (float) sin(angle);
644                 mat[7] = -((float) sin(angle));
645                 mat[8] = (float) cos(angle);
646         } else if((strcmp(axis, "y") == 0) || (strcmp(axis, "Y") == 0)) {
647                 //rotation around Y
648                 mat[0] = (float) cos(angle);
649                 mat[2] = -((float) sin(angle));
650                 mat[4] = 1.0f;
651                 mat[6] = (float) sin(angle);
652                 mat[8] = (float) cos(angle);
653         } else if((strcmp(axis, "z") == 0) || (strcmp(axis, "Z") == 0)) {
654                 //rotation around Z
655                 mat[0] = (float) cos(angle);
656                 mat[1] = (float) sin(angle);
657                 mat[3] = -((float) sin(angle));
658                 mat[4] = (float) cos(angle);
659                 mat[8] = 1.0f;
660         } else if((strcmp(axis, "r") == 0) || (strcmp(axis, "R") == 0)) {
661                 //arbitrary rotation
662                 //normalize arbitrary axis
663                 norm = (float) sqrt(vec->vec[0] * vec->vec[0] +
664                                        vec->vec[1] * vec->vec[1] +
665                                        vec->vec[2] * vec->vec[2]);
666                 vec->vec[0] /= norm;
667                 vec->vec[1] /= norm;
668                 vec->vec[2] /= norm;
669                 
670                 if (isnan(vec->vec[0]) || isnan(vec->vec[1]) || isnan(vec->vec[2])) {
671                         /* zero length vector, return an identity matrix, could also return an error */
672                         mat[0]= mat[4] = mat[8] = 1.0f;
673                 } else {        
674                         /* create matrix */
675                         cosAngle = (float) cos(angle);
676                         sinAngle = (float) sin(angle);
677                         mat[0] = ((vec->vec[0] * vec->vec[0]) * (1 - cosAngle)) +
678                                 cosAngle;
679                         mat[1] = ((vec->vec[0] * vec->vec[1]) * (1 - cosAngle)) +
680                                 (vec->vec[2] * sinAngle);
681                         mat[2] = ((vec->vec[0] * vec->vec[2]) * (1 - cosAngle)) -
682                                 (vec->vec[1] * sinAngle);
683                         mat[3] = ((vec->vec[0] * vec->vec[1]) * (1 - cosAngle)) -
684                                 (vec->vec[2] * sinAngle);
685                         mat[4] = ((vec->vec[1] * vec->vec[1]) * (1 - cosAngle)) +
686                                 cosAngle;
687                         mat[5] = ((vec->vec[1] * vec->vec[2]) * (1 - cosAngle)) +
688                                 (vec->vec[0] * sinAngle);
689                         mat[6] = ((vec->vec[0] * vec->vec[2]) * (1 - cosAngle)) +
690                                 (vec->vec[1] * sinAngle);
691                         mat[7] = ((vec->vec[1] * vec->vec[2]) * (1 - cosAngle)) -
692                                 (vec->vec[0] * sinAngle);
693                         mat[8] = ((vec->vec[2] * vec->vec[2]) * (1 - cosAngle)) +
694                                 cosAngle;
695                 }
696         } else {
697                 PyErr_SetString(PyExc_AttributeError, "Mathutils.RotationMatrix(): unrecognizable axis of rotation type - expected x,y,z or r\n");
698                 return NULL;
699         }
700         if(matSize == 4) {
701                 //resize matrix
702                 mat[10] = mat[8];
703                 mat[9] = mat[7];
704                 mat[8] = mat[6];
705                 mat[7] = 0.0f;
706                 mat[6] = mat[5];
707                 mat[5] = mat[4];
708                 mat[4] = mat[3];
709                 mat[3] = 0.0f;
710         }
711         //pass to matrix creation
712         return newMatrixObject(mat, matSize, matSize, Py_NEW);
713 }
714 //----------------------------------Mathutils.TranslationMatrix() -------
715 //creates a translation matrix
716 PyObject *M_Mathutils_TranslationMatrix(PyObject * self, VectorObject * vec)
717 {
718         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
719                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
720         
721         if(!VectorObject_Check(vec)) {
722                 PyErr_SetString(PyExc_TypeError, "Mathutils.TranslationMatrix(): expected vector\n");
723                 return NULL;
724         }
725         if(vec->size != 3 && vec->size != 4) {
726                 PyErr_SetString(PyExc_TypeError, "Mathutils.TranslationMatrix(): vector must be 3D or 4D\n");
727                 return NULL;
728         }
729         //create a identity matrix and add translation
730         Mat4One((float(*)[4]) mat);
731         mat[12] = vec->vec[0];
732         mat[13] = vec->vec[1];
733         mat[14] = vec->vec[2];
734
735         return newMatrixObject(mat, 4, 4, Py_NEW);
736 }
737 //----------------------------------Mathutils.ScaleMatrix() -------------
738 //mat is a 1D array of floats - row[0][0],row[0][1], row[1][0], etc.
739 //creates a scaling matrix
740 PyObject *M_Mathutils_ScaleMatrix(PyObject * self, PyObject * args)
741 {
742         VectorObject *vec = NULL;
743         float norm = 0.0f, factor;
744         int matSize, x;
745         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
746                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
747
748         if(!PyArg_ParseTuple(args, "fi|O!", &factor, &matSize, &vector_Type, &vec)) {
749                 PyErr_SetString(PyExc_TypeError, "Mathutils.ScaleMatrix(): expected float int and optional vector\n");
750                 return NULL;
751         }
752         if(matSize != 2 && matSize != 3 && matSize != 4) {
753                 PyErr_SetString(PyExc_AttributeError, "Mathutils.ScaleMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
754                 return NULL;
755         }
756         if(vec) {
757                 if(vec->size > 2 && matSize == 2) {
758                         PyErr_SetString(PyExc_AttributeError, "Mathutils.ScaleMatrix(): please use 2D vectors when scaling in 2D\n");
759                         return NULL;
760                 }
761         }
762         if(vec == NULL) {       //scaling along axis
763                 if(matSize == 2) {
764                         mat[0] = factor;
765                         mat[3] = factor;
766                 } else {
767                         mat[0] = factor;
768                         mat[4] = factor;
769                         mat[8] = factor;
770                 }
771         } else { //scaling in arbitrary direction
772                 //normalize arbitrary axis
773                 for(x = 0; x < vec->size; x++) {
774                         norm += vec->vec[x] * vec->vec[x];
775                 }
776                 norm = (float) sqrt(norm);
777                 for(x = 0; x < vec->size; x++) {
778                         vec->vec[x] /= norm;
779                 }
780                 if(matSize == 2) {
781                         mat[0] = 1 +((factor - 1) *(vec->vec[0] * vec->vec[0]));
782                         mat[1] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
783                         mat[2] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
784                         mat[3] = 1 + ((factor - 1) *(vec->vec[1] * vec->vec[1]));
785                 } else {
786                         mat[0] = 1 + ((factor - 1) *(vec->vec[0] * vec->vec[0]));
787                         mat[1] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
788                         mat[2] =((factor - 1) *(vec->vec[0] * vec->vec[2]));
789                         mat[3] =((factor - 1) *(vec->vec[0] * vec->vec[1]));
790                         mat[4] = 1 + ((factor - 1) *(vec->vec[1] * vec->vec[1]));
791                         mat[5] =((factor - 1) *(vec->vec[1] * vec->vec[2]));
792                         mat[6] =((factor - 1) *(vec->vec[0] * vec->vec[2]));
793                         mat[7] =((factor - 1) *(vec->vec[1] * vec->vec[2]));
794                         mat[8] = 1 + ((factor - 1) *(vec->vec[2] * vec->vec[2]));
795                 }
796         }
797         if(matSize == 4) {
798                 //resize matrix
799                 mat[10] = mat[8];
800                 mat[9] = mat[7];
801                 mat[8] = mat[6];
802                 mat[7] = 0.0f;
803                 mat[6] = mat[5];
804                 mat[5] = mat[4];
805                 mat[4] = mat[3];
806                 mat[3] = 0.0f;
807         }
808         //pass to matrix creation
809         return newMatrixObject(mat, matSize, matSize, Py_NEW);
810 }
811 //----------------------------------Mathutils.OrthoProjectionMatrix() ---
812 //mat is a 1D array of floats - row[0][0],row[0][1], row[1][0], etc.
813 //creates an ortho projection matrix
814 PyObject *M_Mathutils_OrthoProjectionMatrix(PyObject * self, PyObject * args)
815 {
816         VectorObject *vec = NULL;
817         char *plane;
818         int matSize, x;
819         float norm = 0.0f;
820         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
821                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
822         
823         if(!PyArg_ParseTuple(args, "si|O!", &plane, &matSize, &vector_Type, &vec)) {
824                 PyErr_SetString(PyExc_TypeError, "Mathutils.OrthoProjectionMatrix(): expected string and int and optional vector\n");
825                 return NULL;
826         }
827         if(matSize != 2 && matSize != 3 && matSize != 4) {
828                 PyErr_SetString(PyExc_AttributeError,"Mathutils.OrthoProjectionMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
829                 return NULL;
830         }
831         if(vec) {
832                 if(vec->size > 2 && matSize == 2) {
833                         PyErr_SetString(PyExc_AttributeError, "Mathutils.OrthoProjectionMatrix(): please use 2D vectors when scaling in 2D\n");
834                         return NULL;
835                 }
836         }
837         if(vec == NULL) {       //ortho projection onto cardinal plane
838                 if(((strcmp(plane, "x") == 0)
839                       || (strcmp(plane, "X") == 0)) && matSize == 2) {
840                         mat[0] = 1.0f;
841                 } else if(((strcmp(plane, "y") == 0) 
842                         || (strcmp(plane, "Y") == 0))
843                            && matSize == 2) {
844                         mat[3] = 1.0f;
845                 } else if(((strcmp(plane, "xy") == 0)
846                              || (strcmp(plane, "XY") == 0))
847                            && matSize > 2) {
848                         mat[0] = 1.0f;
849                         mat[4] = 1.0f;
850                 } else if(((strcmp(plane, "xz") == 0)
851                              || (strcmp(plane, "XZ") == 0))
852                            && matSize > 2) {
853                         mat[0] = 1.0f;
854                         mat[8] = 1.0f;
855                 } else if(((strcmp(plane, "yz") == 0)
856                              || (strcmp(plane, "YZ") == 0))
857                            && matSize > 2) {
858                         mat[4] = 1.0f;
859                         mat[8] = 1.0f;
860                 } else {
861                         PyErr_SetString(PyExc_AttributeError, "Mathutils.OrthoProjectionMatrix(): unknown plane - expected: x, y, xy, xz, yz\n");
862                         return NULL;
863                 }
864         } else { //arbitrary plane
865                 //normalize arbitrary axis
866                 for(x = 0; x < vec->size; x++) {
867                         norm += vec->vec[x] * vec->vec[x];
868                 }
869                 norm = (float) sqrt(norm);
870                 for(x = 0; x < vec->size; x++) {
871                         vec->vec[x] /= norm;
872                 }
873                 if(((strcmp(plane, "r") == 0)
874                       || (strcmp(plane, "R") == 0)) && matSize == 2) {
875                         mat[0] = 1 - (vec->vec[0] * vec->vec[0]);
876                         mat[1] = -(vec->vec[0] * vec->vec[1]);
877                         mat[2] = -(vec->vec[0] * vec->vec[1]);
878                         mat[3] = 1 - (vec->vec[1] * vec->vec[1]);
879                 } else if(((strcmp(plane, "r") == 0)
880                              || (strcmp(plane, "R") == 0))
881                            && matSize > 2) {
882                         mat[0] = 1 - (vec->vec[0] * vec->vec[0]);
883                         mat[1] = -(vec->vec[0] * vec->vec[1]);
884                         mat[2] = -(vec->vec[0] * vec->vec[2]);
885                         mat[3] = -(vec->vec[0] * vec->vec[1]);
886                         mat[4] = 1 - (vec->vec[1] * vec->vec[1]);
887                         mat[5] = -(vec->vec[1] * vec->vec[2]);
888                         mat[6] = -(vec->vec[0] * vec->vec[2]);
889                         mat[7] = -(vec->vec[1] * vec->vec[2]);
890                         mat[8] = 1 - (vec->vec[2] * vec->vec[2]);
891                 } else {
892                         PyErr_SetString(PyExc_AttributeError, "Mathutils.OrthoProjectionMatrix(): unknown plane - expected: 'r' expected for axis designation\n");
893                         return NULL;
894                 }
895         }
896         if(matSize == 4) {
897                 //resize matrix
898                 mat[10] = mat[8];
899                 mat[9] = mat[7];
900                 mat[8] = mat[6];
901                 mat[7] = 0.0f;
902                 mat[6] = mat[5];
903                 mat[5] = mat[4];
904                 mat[4] = mat[3];
905                 mat[3] = 0.0f;
906         }
907         //pass to matrix creation
908         return newMatrixObject(mat, matSize, matSize, Py_NEW);
909 }
910 //----------------------------------Mathutils.ShearMatrix() -------------
911 //creates a shear matrix
912 PyObject *M_Mathutils_ShearMatrix(PyObject * self, PyObject * args)
913 {
914         int matSize;
915         char *plane;
916         float factor;
917         float mat[16] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
918                 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f};
919
920         if(!PyArg_ParseTuple(args, "sfi", &plane, &factor, &matSize)) {
921                 PyErr_SetString(PyExc_TypeError,"Mathutils.ShearMatrix(): expected string float and int\n");
922                 return NULL;
923         }
924         if(matSize != 2 && matSize != 3 && matSize != 4) {
925                 PyErr_SetString(PyExc_AttributeError,"Mathutils.ShearMatrix(): can only return a 2x2 3x3 or 4x4 matrix\n");
926                 return NULL;
927         }
928
929         if(((strcmp(plane, "x") == 0) || (strcmp(plane, "X") == 0))
930             && matSize == 2) {
931                 mat[0] = 1.0f;
932                 mat[2] = factor;
933                 mat[3] = 1.0f;
934         } else if(((strcmp(plane, "y") == 0)
935                      || (strcmp(plane, "Y") == 0)) && matSize == 2) {
936                 mat[0] = 1.0f;
937                 mat[1] = factor;
938                 mat[3] = 1.0f;
939         } else if(((strcmp(plane, "xy") == 0)
940                      || (strcmp(plane, "XY") == 0)) && matSize > 2) {
941                 mat[0] = 1.0f;
942                 mat[4] = 1.0f;
943                 mat[6] = factor;
944                 mat[7] = factor;
945         } else if(((strcmp(plane, "xz") == 0)
946                      || (strcmp(plane, "XZ") == 0)) && matSize > 2) {
947                 mat[0] = 1.0f;
948                 mat[3] = factor;
949                 mat[4] = 1.0f;
950                 mat[5] = factor;
951                 mat[8] = 1.0f;
952         } else if(((strcmp(plane, "yz") == 0)
953                      || (strcmp(plane, "YZ") == 0)) && matSize > 2) {
954                 mat[0] = 1.0f;
955                 mat[1] = factor;
956                 mat[2] = factor;
957                 mat[4] = 1.0f;
958                 mat[8] = 1.0f;
959         } else {
960                 PyErr_SetString(PyExc_AttributeError, "Mathutils.ShearMatrix(): expected: x, y, xy, xz, yz or wrong matrix size for shearing plane\n");
961                 return NULL;
962         }
963         if(matSize == 4) {
964                 //resize matrix
965                 mat[10] = mat[8];
966                 mat[9] = mat[7];
967                 mat[8] = mat[6];
968                 mat[7] = 0.0f;
969                 mat[6] = mat[5];
970                 mat[5] = mat[4];
971                 mat[4] = mat[3];
972                 mat[3] = 0.0f;
973         }
974         //pass to matrix creation
975         return newMatrixObject(mat, matSize, matSize, Py_NEW);
976 }
977 //----------------------------------QUATERNION FUNCTIONS-----------------
978 //----------------------------------Mathutils.Quaternion() --------------
979 PyObject *M_Mathutils_Quaternion(PyObject * self, PyObject * args)
980 {
981         PyObject *listObject = NULL, *n, *q, *f;
982         int size, i;
983         float quat[4];
984         double norm = 0.0f, angle = 0.0f;
985
986         size = PySequence_Length(args);
987         if (size == 1 || size == 2) { //seq?
988                 listObject = PySequence_GetItem(args, 0);
989                 if (PySequence_Check(listObject)) {
990                         size = PySequence_Length(listObject);
991                         if ((size == 4 && PySequence_Length(args) !=1) || 
992                                 (size == 3 && PySequence_Length(args) !=2) || (size >4 || size < 3)) { 
993                                 // invalid args/size
994                                 Py_DECREF(listObject);
995                                 PyErr_SetString(PyExc_AttributeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
996                                 return NULL;
997                         }
998                         if(size == 3){ //get angle in axis/angle
999                                 n = PySequence_GetItem(args, 1);
1000                                 if(n == NULL) { // parsed item not a number or getItem fail
1001                                         Py_DECREF(listObject);
1002                                         PyErr_SetString(PyExc_TypeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1003                                         return NULL;
1004                                 }
1005                                 
1006                                 angle = PyFloat_AsDouble(n);
1007                                 Py_DECREF(n);
1008                                 
1009                                 if (angle==-1 && PyErr_Occurred()) {
1010                                         Py_DECREF(listObject);
1011                                         PyErr_SetString(PyExc_TypeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1012                                         return NULL;
1013                                 }
1014                         }
1015                 }else{
1016                         Py_DECREF(listObject); /* assume the list is teh second arg */
1017                         listObject = PySequence_GetItem(args, 1);
1018                         if (size>1 && PySequence_Check(listObject)) {
1019                                 size = PySequence_Length(listObject);
1020                                 if (size != 3) { 
1021                                         // invalid args/size
1022                                         Py_DECREF(listObject);
1023                                         PyErr_SetString(PyExc_AttributeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1024                                         return NULL;
1025                                 }
1026                                 n = PySequence_GetItem(args, 0);
1027                                 if(n == NULL) { // parsed item not a number or getItem fail
1028                                         Py_DECREF(listObject);
1029                                         PyErr_SetString(PyExc_TypeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1030                                         return NULL;
1031                                 }
1032                                 angle = PyFloat_AsDouble(n);
1033                                 Py_DECREF(n);
1034                                 
1035                                 if (angle==-1 && PyErr_Occurred()) {
1036                                         Py_DECREF(listObject);
1037                                         PyErr_SetString(PyExc_TypeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1038                                         return NULL;
1039                                 }
1040                         } else { // argument was not a sequence
1041                                 Py_XDECREF(listObject);
1042                                 PyErr_SetString(PyExc_TypeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1043                                 return NULL;
1044                         }
1045                 }
1046         } else if (size == 0) { //returns a new empty quat
1047                 return newQuaternionObject(NULL, Py_NEW); 
1048         } else {
1049                 Py_INCREF(args);
1050                 listObject = args;
1051         }
1052
1053         if (size == 3) { // invalid quat size
1054                 if(PySequence_Length(args) != 2){
1055                         Py_DECREF(listObject);
1056                         PyErr_SetString(PyExc_AttributeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1057                         return NULL;
1058                 }
1059         }else{
1060                 if(size != 4){
1061                         Py_DECREF(listObject);
1062                         PyErr_SetString(PyExc_AttributeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1063                         return NULL;
1064                 }
1065         }
1066
1067         for (i=0; i<size; i++) { //parse
1068                 q = PySequence_GetItem(listObject, i);
1069                 if (q == NULL) { // Failed to read sequence
1070                         Py_DECREF(listObject);
1071                         PyErr_SetString(PyExc_RuntimeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1072                         return NULL;
1073                 }
1074
1075                 f = PyNumber_Float(q);
1076                 if(f == NULL) { // parsed item not a number
1077                         Py_DECREF(q);
1078                         Py_DECREF(listObject);
1079                         PyErr_SetString(PyExc_TypeError, "Mathutils.Quaternion(): 4d numeric sequence expected or 3d vector and number\n");
1080                         return NULL;
1081                 }
1082
1083                 quat[i] = (float)PyFloat_AS_DOUBLE(f);
1084                 Py_DECREF(f);
1085                 Py_DECREF(q);
1086         }
1087         if(size == 3){ //calculate the quat based on axis/angle
1088                 norm = sqrt(quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2]);
1089                 quat[0] /= (float)norm;
1090                 quat[1] /= (float)norm;
1091                 quat[2] /= (float)norm;
1092
1093                 angle = angle * (Py_PI / 180);
1094                 quat[3] =(float) (sin(angle/ 2.0f)) * quat[2];
1095                 quat[2] =(float) (sin(angle/ 2.0f)) * quat[1];
1096                 quat[1] =(float) (sin(angle/ 2.0f)) * quat[0];
1097                 quat[0] =(float) (cos(angle/ 2.0f));
1098         }
1099
1100         Py_DECREF(listObject);
1101         return newQuaternionObject(quat, Py_NEW);
1102 }
1103 //----------------------------------Mathutils.CrossQuats() ----------------
1104 //quaternion multiplication - associate not commutative
1105 PyObject *M_Mathutils_CrossQuats(PyObject * self, PyObject * args)
1106 {
1107         QuaternionObject *quatU = NULL, *quatV = NULL;
1108         float quat[4];
1109
1110         if(!PyArg_ParseTuple(args, "O!O!", &quaternion_Type, &quatU, &quaternion_Type, &quatV)) {
1111                 PyErr_SetString(PyExc_TypeError,"Mathutils.CrossQuats(): expected Quaternion types");
1112                 return NULL;
1113         }
1114         QuatMul(quat, quatU->quat, quatV->quat);
1115
1116         return newQuaternionObject(quat, Py_NEW);
1117 }
1118 //----------------------------------Mathutils.DotQuats() ----------------
1119 //returns the dot product of 2 quaternions
1120 PyObject *M_Mathutils_DotQuats(PyObject * self, PyObject * args)
1121 {
1122         QuaternionObject *quatU = NULL, *quatV = NULL;
1123         double dot = 0.0f;
1124         int x;
1125
1126         if(!PyArg_ParseTuple(args, "O!O!", &quaternion_Type, &quatU, &quaternion_Type, &quatV)) {
1127                 PyErr_SetString(PyExc_TypeError, "Mathutils.DotQuats(): expected Quaternion types");
1128                 return NULL;
1129         }
1130
1131         for(x = 0; x < 4; x++) {
1132                 dot += quatU->quat[x] * quatV->quat[x];
1133         }
1134         return PyFloat_FromDouble(dot);
1135 }
1136 //----------------------------------Mathutils.DifferenceQuats() ---------
1137 //returns the difference between 2 quaternions
1138 PyObject *M_Mathutils_DifferenceQuats(PyObject * self, PyObject * args)
1139 {
1140         QuaternionObject *quatU = NULL, *quatV = NULL;
1141         float quat[4], tempQuat[4];
1142         double dot = 0.0f;
1143         int x;
1144
1145         if(!PyArg_ParseTuple(args, "O!O!", &quaternion_Type, &quatU, &quaternion_Type, &quatV)) {
1146                 PyErr_SetString(PyExc_TypeError, "Mathutils.DifferenceQuats(): expected Quaternion types");
1147                 return NULL;
1148         }
1149         tempQuat[0] = quatU->quat[0];
1150         tempQuat[1] = -quatU->quat[1];
1151         tempQuat[2] = -quatU->quat[2];
1152         tempQuat[3] = -quatU->quat[3];
1153
1154         dot = sqrt(tempQuat[0] * tempQuat[0] + tempQuat[1] *  tempQuat[1] +
1155                                tempQuat[2] * tempQuat[2] + tempQuat[3] * tempQuat[3]);
1156
1157         for(x = 0; x < 4; x++) {
1158                 tempQuat[x] /= (float)(dot * dot);
1159         }
1160         QuatMul(quat, tempQuat, quatV->quat);
1161         return newQuaternionObject(quat, Py_NEW);
1162 }
1163 //----------------------------------Mathutils.Slerp() ------------------
1164 //attemps to interpolate 2 quaternions and return the result
1165 PyObject *M_Mathutils_Slerp(PyObject * self, PyObject * args)
1166 {
1167         QuaternionObject *quatU = NULL, *quatV = NULL;
1168         float quat[4], quat_u[4], quat_v[4], param;
1169         double x, y, dot, sinT, angle, IsinT;
1170         int z;
1171
1172         if(!PyArg_ParseTuple(args, "O!O!f", &quaternion_Type, &quatU, &quaternion_Type, &quatV, &param)) {
1173                 PyErr_SetString(PyExc_TypeError, "Mathutils.Slerp(): expected Quaternion types and float");
1174                 return NULL;
1175         }
1176         if(param > 1.0f || param < 0.0f) {
1177                 PyErr_SetString(PyExc_AttributeError, "Mathutils.Slerp(): interpolation factor must be between 0.0 and 1.0");
1178                 return NULL;
1179         }
1180
1181         //copy quats
1182         for(z = 0; z < 4; z++){
1183                 quat_u[z] = quatU->quat[z];
1184                 quat_v[z] = quatV->quat[z];
1185         }
1186
1187         //dot product
1188         dot = quat_u[0] * quat_v[0] + quat_u[1] * quat_v[1] +
1189                 quat_u[2] * quat_v[2] + quat_u[3] * quat_v[3];
1190
1191         //if negative negate a quat (shortest arc)
1192         if(dot < 0.0f) {
1193                 quat_v[0] = -quat_v[0];
1194                 quat_v[1] = -quat_v[1];
1195                 quat_v[2] = -quat_v[2];
1196                 quat_v[3] = -quat_v[3];
1197                 dot = -dot;
1198         }
1199         if(dot > .99999f) { //very close
1200                 x = 1.0f - param;
1201                 y = param;
1202         } else {
1203                 //calculate sin of angle
1204                 sinT = sqrt(1.0f - (dot * dot));
1205                 //calculate angle
1206                 angle = atan2(sinT, dot);
1207                 //caluculate inverse of sin(theta)
1208                 IsinT = 1.0f / sinT;
1209                 x = sin((1.0f - param) * angle) * IsinT;
1210                 y = sin(param * angle) * IsinT;
1211         }
1212         //interpolate
1213         quat[0] = (float)(quat_u[0] * x + quat_v[0] * y);
1214         quat[1] = (float)(quat_u[1] * x + quat_v[1] * y);
1215         quat[2] = (float)(quat_u[2] * x + quat_v[2] * y);
1216         quat[3] = (float)(quat_u[3] * x + quat_v[3] * y);
1217
1218         return newQuaternionObject(quat, Py_NEW);
1219 }
1220 //----------------------------------EULER FUNCTIONS----------------------
1221 //----------------------------------Mathutils.Euler() -------------------
1222 //makes a new euler for you to play with
1223 PyObject *M_Mathutils_Euler(PyObject * self, PyObject * args)
1224 {
1225
1226         PyObject *listObject = NULL;
1227         int size, i;
1228         float eul[3];
1229         PyObject *e, *f;
1230
1231         size = PySequence_Length(args);
1232         if (size == 1) {
1233                 listObject = PySequence_GetItem(args, 0);
1234                 if (PySequence_Check(listObject)) {
1235                         size = PySequence_Length(listObject);
1236                 } else { // Single argument was not a sequence
1237                         Py_DECREF(listObject);
1238                         PyErr_SetString(PyExc_TypeError, "Mathutils.Euler(): 3d numeric sequence expected\n");
1239                         return NULL;
1240                 }
1241         } else if (size == 0) {
1242                 //returns a new empty 3d euler
1243                 return newEulerObject(NULL, Py_NEW); 
1244         } else {
1245                 Py_INCREF(args);
1246                 listObject = args;
1247         }
1248
1249         if (size != 3) { // Invalid euler size
1250                 Py_DECREF(listObject);
1251                 PyErr_SetString(PyExc_AttributeError, "Mathutils.Euler(): 3d numeric sequence expected\n");
1252                 return NULL;
1253         }
1254
1255         for (i=0; i<size; i++) {
1256                 e = PySequence_GetItem(listObject, i);
1257                 if (e == NULL) { // Failed to read sequence
1258                         Py_DECREF(listObject);
1259                         PyErr_SetString(PyExc_RuntimeError, "Mathutils.Euler(): 3d numeric sequence expected\n");
1260                         return NULL;
1261                 }
1262
1263                 f = PyNumber_Float(e);
1264                 if(f == NULL) { // parsed item not a number
1265                         Py_DECREF(e);
1266                         Py_DECREF(listObject);
1267                         PyErr_SetString(PyExc_TypeError, "Mathutils.Euler(): 3d numeric sequence expected\n");
1268                         return NULL;
1269                 }
1270
1271                 eul[i]=(float)PyFloat_AS_DOUBLE(f);
1272                 Py_DECREF(f);
1273                 Py_DECREF(e);
1274         }
1275         Py_DECREF(listObject);
1276         return newEulerObject(eul, Py_NEW);
1277 }
1278
1279 //---------------------------------INTERSECTION FUNCTIONS--------------------
1280 //----------------------------------Mathutils.Intersect() -------------------
1281 PyObject *M_Mathutils_Intersect( PyObject * self, PyObject * args )
1282 {
1283         VectorObject *ray, *ray_off, *vec1, *vec2, *vec3;
1284         float dir[3], orig[3], v1[3], v2[3], v3[3], e1[3], e2[3], pvec[3], tvec[3], qvec[3];
1285         float det, inv_det, u, v, t;
1286         int clip = 1;
1287
1288         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)) {
1289                 PyErr_SetString( PyExc_TypeError, "expected 5 vector types\n" );
1290                 return NULL;
1291         }
1292         if(vec1->size != 3 || vec2->size != 3 || vec3->size != 3 || ray->size != 3 || ray_off->size != 3) {
1293                 PyErr_SetString( PyExc_TypeError, "only 3D vectors for all parameters\n");
1294                 return NULL;
1295         }
1296
1297         VECCOPY(v1, vec1->vec);
1298         VECCOPY(v2, vec2->vec);
1299         VECCOPY(v3, vec3->vec);
1300
1301         VECCOPY(dir, ray->vec);
1302         Normalize(dir);
1303
1304         VECCOPY(orig, ray_off->vec);
1305
1306         /* find vectors for two edges sharing v1 */
1307         VecSubf(e1, v2, v1);
1308         VecSubf(e2, v3, v1);
1309
1310         /* begin calculating determinant - also used to calculated U parameter */
1311         Crossf(pvec, dir, e2);  
1312
1313         /* if determinant is near zero, ray lies in plane of triangle */
1314         det = Inpf(e1, pvec);
1315
1316         if (det > -0.000001 && det < 0.000001) {
1317                 Py_RETURN_NONE;
1318         }
1319
1320         inv_det = 1.0f / det;
1321
1322         /* calculate distance from v1 to ray origin */
1323         VecSubf(tvec, orig, v1);
1324
1325         /* calculate U parameter and test bounds */
1326         u = Inpf(tvec, pvec) * inv_det;
1327         if (clip && (u < 0.0f || u > 1.0f)) {
1328                 Py_RETURN_NONE;
1329         }
1330
1331         /* prepare to test the V parameter */
1332         Crossf(qvec, tvec, e1);
1333
1334         /* calculate V parameter and test bounds */
1335         v = Inpf(dir, qvec) * inv_det;
1336
1337         if (clip && (v < 0.0f || u + v > 1.0f)) {
1338                 Py_RETURN_NONE;
1339         }
1340
1341         /* calculate t, ray intersects triangle */
1342         t = Inpf(e2, qvec) * inv_det;
1343
1344         VecMulf(dir, t);
1345         VecAddf(pvec, orig, dir);
1346
1347         return newVectorObject(pvec, 3, Py_NEW);
1348 }
1349 //----------------------------------Mathutils.LineIntersect() -------------------
1350 /* Line-Line intersection using algorithm from mathworld.wolfram.com */
1351 PyObject *M_Mathutils_LineIntersect( PyObject * self, PyObject * args )
1352 {
1353         PyObject * tuple;
1354         VectorObject *vec1, *vec2, *vec3, *vec4;
1355         float v1[3], v2[3], v3[3], v4[3], i1[3], i2[3];
1356
1357         if( !PyArg_ParseTuple( args, "O!O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2, &vector_Type, &vec3, &vector_Type, &vec4 ) ) {
1358                 PyErr_SetString( PyExc_TypeError, "expected 4 vector types\n" );
1359                 return NULL;
1360         }
1361         if( vec1->size != vec2->size || vec1->size != vec3->size || vec1->size != vec2->size) {
1362                 PyErr_SetString( PyExc_TypeError,"vectors must be of the same size\n" );
1363                 return NULL;
1364         }
1365         if( vec1->size == 3 || vec1->size == 2) {
1366                 int result;
1367                 
1368                 if (vec1->size == 3) {
1369                         VECCOPY(v1, vec1->vec);
1370                         VECCOPY(v2, vec2->vec);
1371                         VECCOPY(v3, vec3->vec);
1372                         VECCOPY(v4, vec4->vec);
1373                 }
1374                 else {
1375                         v1[0] = vec1->vec[0];
1376                         v1[1] = vec1->vec[1];
1377                         v1[2] = 0.0f;
1378
1379                         v2[0] = vec2->vec[0];
1380                         v2[1] = vec2->vec[1];
1381                         v2[2] = 0.0f;
1382
1383                         v3[0] = vec3->vec[0];
1384                         v3[1] = vec3->vec[1];
1385                         v3[2] = 0.0f;
1386
1387                         v4[0] = vec4->vec[0];
1388                         v4[1] = vec4->vec[1];
1389                         v4[2] = 0.0f;
1390                 }
1391                 
1392                 result = LineIntersectLine(v1, v2, v3, v4, i1, i2);
1393
1394                 if (result == 0) {
1395                         /* colinear */
1396                         Py_RETURN_NONE;
1397                 }
1398                 else {
1399                         tuple = PyTuple_New( 2 );
1400                         PyTuple_SetItem( tuple, 0, newVectorObject(i1, vec1->size, Py_NEW) );
1401                         PyTuple_SetItem( tuple, 1, newVectorObject(i2, vec1->size, Py_NEW) );
1402                         return tuple;
1403                 }
1404         }
1405         else {
1406                 PyErr_SetString( PyExc_TypeError, "2D/3D vectors only\n" );
1407                 return NULL;
1408         }
1409 }
1410
1411
1412
1413 //---------------------------------NORMALS FUNCTIONS--------------------
1414 //----------------------------------Mathutils.QuadNormal() -------------------
1415 PyObject *M_Mathutils_QuadNormal( PyObject * self, PyObject * args )
1416 {
1417         VectorObject *vec1;
1418         VectorObject *vec2;
1419         VectorObject *vec3;
1420         VectorObject *vec4;
1421         float v1[3], v2[3], v3[3], v4[3], e1[3], e2[3], n1[3], n2[3];
1422
1423         if( !PyArg_ParseTuple( args, "O!O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2, &vector_Type, &vec3, &vector_Type, &vec4 ) ) {
1424                 PyErr_SetString( PyExc_TypeError, "expected 4 vector types\n" );
1425                 return NULL;
1426         }
1427         if( vec1->size != vec2->size || vec1->size != vec3->size || vec1->size != vec4->size) {
1428                 PyErr_SetString( PyExc_TypeError,"vectors must be of the same size\n" );
1429                 return NULL;
1430         }
1431         if( vec1->size != 3 ) {
1432                 PyErr_SetString( PyExc_TypeError, "only 3D vectors\n" );
1433                 return NULL;
1434         }
1435         VECCOPY(v1, vec1->vec);
1436         VECCOPY(v2, vec2->vec);
1437         VECCOPY(v3, vec3->vec);
1438         VECCOPY(v4, vec4->vec);
1439
1440         /* find vectors for two edges sharing v2 */
1441         VecSubf(e1, v1, v2);
1442         VecSubf(e2, v3, v2);
1443
1444         Crossf(n1, e2, e1);
1445         Normalize(n1);
1446
1447         /* find vectors for two edges sharing v4 */
1448         VecSubf(e1, v3, v4);
1449         VecSubf(e2, v1, v4);
1450
1451         Crossf(n2, e2, e1);
1452         Normalize(n2);
1453
1454         /* adding and averaging the normals of both triangles */
1455         VecAddf(n1, n2, n1);
1456         Normalize(n1);
1457
1458         return newVectorObject(n1, 3, Py_NEW);
1459 }
1460
1461 //----------------------------Mathutils.TriangleNormal() -------------------
1462 PyObject *M_Mathutils_TriangleNormal( PyObject * self, PyObject * args )
1463 {
1464         VectorObject *vec1, *vec2, *vec3;
1465         float v1[3], v2[3], v3[3], e1[3], e2[3], n[3];
1466
1467         if( !PyArg_ParseTuple( args, "O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2, &vector_Type, &vec3 ) ) {
1468                 PyErr_SetString( PyExc_TypeError, "expected 3 vector types\n" );
1469                 return NULL;
1470         }
1471         if( vec1->size != vec2->size || vec1->size != vec3->size ) {
1472                 PyErr_SetString( PyExc_TypeError, "vectors must be of the same size\n" );
1473                 return NULL;
1474         }
1475         if( vec1->size != 3 ) {
1476                 PyErr_SetString( PyExc_TypeError, "only 3D vectors\n" );
1477                 return NULL;
1478         }
1479
1480         VECCOPY(v1, vec1->vec);
1481         VECCOPY(v2, vec2->vec);
1482         VECCOPY(v3, vec3->vec);
1483
1484         /* find vectors for two edges sharing v2 */
1485         VecSubf(e1, v1, v2);
1486         VecSubf(e2, v3, v2);
1487
1488         Crossf(n, e2, e1);
1489         Normalize(n);
1490
1491         return newVectorObject(n, 3, Py_NEW);
1492 }
1493
1494 //--------------------------------- AREA FUNCTIONS--------------------
1495 //----------------------------------Mathutils.TriangleArea() -------------------
1496 PyObject *M_Mathutils_TriangleArea( PyObject * self, PyObject * args )
1497 {
1498         VectorObject *vec1, *vec2, *vec3;
1499         float v1[3], v2[3], v3[3];
1500
1501         if( !PyArg_ParseTuple
1502             ( args, "O!O!O!", &vector_Type, &vec1, &vector_Type, &vec2
1503                 , &vector_Type, &vec3 ) ) {
1504                 PyErr_SetString( PyExc_TypeError, "expected 3 vector types\n");
1505                 return NULL;
1506         }
1507         if( vec1->size != vec2->size || vec1->size != vec3->size ) {
1508                 PyErr_SetString( PyExc_TypeError, "vectors must be of the same size\n" );
1509                 return NULL;
1510         }
1511
1512         if (vec1->size == 3) {
1513                 VECCOPY(v1, vec1->vec);
1514                 VECCOPY(v2, vec2->vec);
1515                 VECCOPY(v3, vec3->vec);
1516
1517                 return PyFloat_FromDouble( AreaT3Dfl(v1, v2, v3) );
1518         }
1519         else if (vec1->size == 2) {
1520                 v1[0] = vec1->vec[0];
1521                 v1[1] = vec1->vec[1];
1522
1523                 v2[0] = vec2->vec[0];
1524                 v2[1] = vec2->vec[1];
1525
1526                 v3[0] = vec3->vec[0];
1527                 v3[1] = vec3->vec[1];
1528
1529                 return PyFloat_FromDouble( AreaF2Dfl(v1, v2, v3) );
1530         }
1531         else {
1532                 PyErr_SetString( PyExc_TypeError, "only 2D,3D vectors are supported\n" );
1533                 return NULL;
1534         }
1535 }
1536 //#############################DEPRECATED################################
1537 //#######################################################################
1538 //----------------------------------Mathutils.CopyMat() -----------------
1539 //copies a matrix into a new matrix
1540 PyObject *M_Mathutils_CopyMat(PyObject * self, PyObject * args)
1541 {
1542         PyObject *matrix = NULL;
1543         static char warning = 1;
1544
1545         if( warning ) {
1546                 printf("Mathutils.CopyMat(): deprecated :use Mathutils.Matrix() to copy matrices\n");
1547                 --warning;
1548         }
1549
1550         matrix = M_Mathutils_Matrix(self, args);
1551         if(matrix == NULL)
1552                 return NULL; //error string already set if we get here
1553         else
1554                 return matrix;
1555 }
1556 //----------------------------------Mathutils.CopyVec() -----------------
1557 //makes a new vector that is a copy of the input
1558 PyObject *M_Mathutils_CopyVec(PyObject * self, PyObject * args)
1559 {
1560         PyObject *vec = NULL;
1561         static char warning = 1;
1562
1563         if( warning ) {
1564                 printf("Mathutils.CopyVec(): Deprecated: use Mathutils.Vector() to copy vectors\n");
1565                 --warning;
1566         }
1567
1568         vec = M_Mathutils_Vector(self, args);
1569         if(vec == NULL)
1570                 return NULL; //error string already set if we get here
1571         else
1572                 return vec;
1573 }
1574 //----------------------------------Mathutils.CopyQuat() --------------
1575 //Copies a quaternion to a new quat
1576 PyObject *M_Mathutils_CopyQuat(PyObject * self, PyObject * args)
1577 {
1578         PyObject *quat = NULL;
1579         static char warning = 1;
1580
1581         if( warning ) {
1582                 printf("Mathutils.CopyQuat(): Deprecated: use Mathutils.Quaternion() to copy vectors\n");
1583                 --warning;
1584         }
1585
1586         quat = M_Mathutils_Quaternion(self, args);
1587         if(quat == NULL)
1588                 return NULL; //error string already set if we get here
1589         else
1590                 return quat;
1591 }
1592 //----------------------------------Mathutils.CopyEuler() ---------------
1593 //copies a euler to a new euler
1594 PyObject *M_Mathutils_CopyEuler(PyObject * self, PyObject * args)
1595 {
1596         PyObject *eul = NULL;
1597         static char warning = 1;
1598
1599         if( warning ) {
1600                 printf("Mathutils.CopyEuler(): deprecated:use Mathutils.Euler() to copy vectors\n");
1601                 --warning;
1602         }
1603
1604         eul = M_Mathutils_Euler(self, args);
1605         if(eul == NULL)
1606                 return NULL; //error string already set if we get here
1607         else
1608                 return eul;
1609 }
1610 //----------------------------------Mathutils.RotateEuler() ------------
1611 //rotates a euler a certain amount and returns the result
1612 //should return a unique euler rotation (i.e. no 720 degree pitches :)
1613 PyObject *M_Mathutils_RotateEuler(PyObject * self, PyObject * args)
1614 {
1615         EulerObject *Eul = NULL;
1616         float angle;
1617         char *axis;
1618         static char warning = 1;
1619
1620         if( warning ) {
1621                 printf("Mathutils.RotateEuler(): Deprecated:use Euler.rotate() to rotate a euler\n");
1622                 --warning;
1623         }
1624
1625         if(!PyArg_ParseTuple(args, "O!fs", &euler_Type, &Eul, &angle, &axis)) {
1626                 PyErr_SetString(PyExc_TypeError, "Mathutils.RotateEuler(): expected euler type & float & string");
1627                 return NULL;
1628         }
1629
1630         Euler_Rotate(Eul, Py_BuildValue("fs", angle, axis));
1631         Py_RETURN_NONE;
1632 }
1633 //----------------------------------Mathutils.MatMultVec() --------------
1634 //COLUMN VECTOR Multiplication (Matrix X Vector)
1635 PyObject *M_Mathutils_MatMultVec(PyObject * self, PyObject * args)
1636 {
1637         MatrixObject *mat = NULL;
1638         VectorObject *vec = NULL;
1639         static char warning = 1;
1640
1641         if( warning ) {
1642                 printf("Mathutils.MatMultVec(): Deprecated: use matrix * vec to perform column vector multiplication\n");
1643                 --warning;
1644         }
1645
1646         //get pyObjects
1647         if(!PyArg_ParseTuple(args, "O!O!", &matrix_Type, &mat, &vector_Type, &vec)) {
1648                 PyErr_SetString(PyExc_TypeError, "Mathutils.MatMultVec(): MatMultVec() expects a matrix and a vector object - in that order\n");
1649                 return NULL;
1650         }
1651
1652         return column_vector_multiplication(mat, vec);
1653 }
1654 //----------------------------------Mathutils.VecMultMat() ---------------
1655 //ROW VECTOR Multiplication - Vector X Matrix
1656 PyObject *M_Mathutils_VecMultMat(PyObject * self, PyObject * args)
1657 {
1658         MatrixObject *mat = NULL;
1659         VectorObject *vec = NULL;
1660         static char warning = 1;
1661
1662         if( warning ) {
1663                 printf("Mathutils.VecMultMat(): Deprecated: use vec * matrix to perform row vector multiplication\n");
1664                 --warning;
1665         }
1666
1667         //get pyObjects
1668         if(!PyArg_ParseTuple(args, "O!O!", &vector_Type, &vec, &matrix_Type, &mat)) {
1669                 PyErr_SetString(PyExc_TypeError, "Mathutils.VecMultMat(): VecMultMat() expects a vector and matrix object - in that order\n");
1670                 return NULL;
1671         }
1672
1673         return row_vector_multiplication(vec, mat);
1674 }
1675
1676 /* Utility functions */
1677
1678 /*---------------------- EXPP_FloatsAreEqual -------------------------
1679   Floating point comparisons 
1680   floatStep = number of representable floats allowable in between
1681    float A and float B to be considered equal. */
1682 int EXPP_FloatsAreEqual(float A, float B, int floatSteps)
1683 {
1684         int a, b, delta;
1685     assert(floatSteps > 0 && floatSteps < (4 * 1024 * 1024));
1686     a = *(int*)&A;
1687     if (a < 0)  
1688                 a = 0x80000000 - a;
1689     b = *(int*)&B;
1690     if (b < 0)  
1691                 b = 0x80000000 - b;
1692     delta = abs(a - b);
1693     if (delta <= floatSteps)    
1694                 return 1;
1695     return 0;
1696 }
1697 /*---------------------- EXPP_VectorsAreEqual -------------------------
1698   Builds on EXPP_FloatsAreEqual to test vectors */
1699 int EXPP_VectorsAreEqual(float *vecA, float *vecB, int size, int floatSteps){
1700
1701         int x;
1702         for (x=0; x< size; x++){
1703                 if (EXPP_FloatsAreEqual(vecA[x], vecB[x], floatSteps) == 0)
1704                         return 0;
1705         }
1706         return 1;
1707 }
1708
1709
1710
1711 //#######################################################################
1712 //#############################DEPRECATED################################