source/blender/python/generic/Mathutils.c

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