//------------------------obj - obj------------------------------ //subtraction static PyObject *Quaternion_sub(PyObject *q1, PyObject *q2) { int x; float quat[QUAT_SIZE]; QuaternionObject *quat1 = NULL, *quat2 = NULL; if (!QuaternionObject_Check(q1) || !QuaternionObject_Check(q2)) { PyErr_Format(PyExc_TypeError, "Quaternion subtraction: (%s - %s) " "invalid type for this operation", Py_TYPE(q1)->tp_name, Py_TYPE(q2)->tp_name); return NULL; } quat1 = (QuaternionObject *)q1; quat2 = (QuaternionObject *)q2; if (BaseMath_ReadCallback(quat1) == -1 || BaseMath_ReadCallback(quat2) == -1) return NULL; for (x = 0; x < QUAT_SIZE; x++) { quat[x] = quat1->quat[x] - quat2->quat[x]; } return Quaternion_CreatePyObject(quat, Py_NEW, Py_TYPE(q1)); }
static PyObject *Quaternion_richcmpr(PyObject *a, PyObject *b, int op) { PyObject *res; int ok = -1; /* zero is true */ if (QuaternionObject_Check(a) && QuaternionObject_Check(b)) { QuaternionObject *quatA = (QuaternionObject *)a; QuaternionObject *quatB = (QuaternionObject *)b; if (BaseMath_ReadCallback(quatA) == -1 || BaseMath_ReadCallback(quatB) == -1) return NULL; ok = (EXPP_VectorsAreEqual(quatA->quat, quatB->quat, QUAT_SIZE, 1)) ? 0 : -1; } switch (op) { case Py_NE: ok = !ok; /* pass through */ case Py_EQ: res = ok ? Py_False : Py_True; break; case Py_LT: case Py_LE: case Py_GT: case Py_GE: res = Py_NotImplemented; break; default: PyErr_BadArgument(); return NULL; } return Py_INCREF(res), res; }
/* on error, -1 is returned and no allocation is made */ int mathutils_array_parse_alloc(float **array, int array_min, PyObject *value, const char *error_prefix) { int size; #if 1 /* approx 6x speedup for mathutils types */ if ((size = VectorObject_Check(value) ? ((VectorObject *)value)->size : 0) || (size = EulerObject_Check(value) ? 3 : 0) || (size = QuaternionObject_Check(value) ? 4 : 0) || (size = ColorObject_Check(value) ? 3 : 0)) { if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) { return -1; } if (size < array_min) { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected > %d", error_prefix, size, array_min); return -1; } *array = PyMem_Malloc(size * sizeof(float)); memcpy(*array, ((BaseMathObject *)value)->data, size * sizeof(float)); return size; } else #endif { PyObject *value_fast = NULL; // *array = NULL; int ret; /* non list/tuple cases */ if (!(value_fast = PySequence_Fast(value, error_prefix))) { /* PySequence_Fast sets the error */ return -1; } size = PySequence_Fast_GET_SIZE(value_fast); if (size < array_min) { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected > %d", error_prefix, size, array_min); return -1; } *array = PyMem_Malloc(size * sizeof(float)); ret = mathutils_array_parse_fast(*array, size, value_fast, error_prefix); if (ret == -1) { PyMem_Free(*array); } return ret; } }
//------------------------NUMERIC PROTOCOLS---------------------- //------------------------obj + obj------------------------------ //addition static PyObject *Quaternion_add(PyObject *q1, PyObject *q2) { float quat[QUAT_SIZE]; QuaternionObject *quat1 = NULL, *quat2 = NULL; if (!QuaternionObject_Check(q1) || !QuaternionObject_Check(q2)) { PyErr_Format(PyExc_TypeError, "Quaternion addition: (%s + %s) " "invalid type for this operation", Py_TYPE(q1)->tp_name, Py_TYPE(q2)->tp_name); return NULL; } quat1 = (QuaternionObject *)q1; quat2 = (QuaternionObject *)q2; if (BaseMath_ReadCallback(quat1) == -1 || BaseMath_ReadCallback(quat2) == -1) return NULL; add_qt_qtqt(quat, quat1->quat, quat2->quat, 1.0f); return Quaternion_CreatePyObject(quat, Py_NEW, Py_TYPE(q1)); }
int mathutils_any_to_rotmat(float rmat[3][3], PyObject *value, const char *error_prefix) { if (EulerObject_Check(value)) { if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) { return -1; } else { eulO_to_mat3(rmat, ((EulerObject *)value)->eul, ((EulerObject *)value)->order); return 0; } } else if (QuaternionObject_Check(value)) { if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) { return -1; } else { float tquat[4]; normalize_qt_qt(tquat, ((QuaternionObject *)value)->quat); quat_to_mat3(rmat, tquat); return 0; } } else if (MatrixObject_Check(value)) { if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) { return -1; } else if (((MatrixObject *)value)->num_row < 3 || ((MatrixObject *)value)->num_col < 3) { PyErr_Format( PyExc_ValueError, "%.200s: matrix must have minimum 3x3 dimensions", error_prefix); return -1; } else { matrix_as_3x3(rmat, (MatrixObject *)value); normalize_m3(rmat); return 0; } } else { PyErr_Format(PyExc_TypeError, "%.200s: expected a Euler, Quaternion or Matrix type, " "found %.200s", error_prefix, Py_TYPE(value)->tp_name); return -1; } }
/* helper functionm returns length of the 'value', -1 on error */ int mathutils_array_parse(float *array, int array_min, int array_max, PyObject *value, const char *error_prefix) { #if 1 /* approx 6x speedup for mathutils types */ int size; if ( (size= VectorObject_Check(value) ? ((VectorObject *)value)->size : 0) || (size= EulerObject_Check(value) ? 3 : 0) || (size= QuaternionObject_Check(value) ? 4 : 0) || (size= ColorObject_Check(value) ? 3 : 0)) { if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) { return -1; } if (size > array_max || size < array_min) { if (array_max == array_min) { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected %d", error_prefix, size, array_max); } else { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected [%d - %d]", error_prefix, size, array_min, array_max); } return -1; } memcpy(array, ((BaseMathObject *)value)->data, size * sizeof(float)); return size; } else #endif { return mathutils_array_parse_fast(array, array_min, array_max, value, error_prefix); } }
/* helper functionm returns length of the 'value', -1 on error */ int mathutils_array_parse(float *array, int array_min, int array_max, PyObject *value, const char *error_prefix) { int size; #if 1 /* approx 6x speedup for mathutils types */ if ((size = VectorObject_Check(value) ? ((VectorObject *)value)->size : 0) || (size = EulerObject_Check(value) ? 3 : 0) || (size = QuaternionObject_Check(value) ? 4 : 0) || (size = ColorObject_Check(value) ? 3 : 0)) { if (BaseMath_ReadCallback((BaseMathObject *)value) == -1) { return -1; } if (size > array_max || size < array_min) { if (array_max == array_min) { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected %d", error_prefix, size, array_max); } else { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected [%d - %d]", error_prefix, size, array_min, array_max); } return -1; } memcpy(array, ((BaseMathObject *)value)->data, size * sizeof(float)); return size; } else #endif { PyObject *value_fast = NULL; /* non list/tuple cases */ if (!(value_fast = PySequence_Fast(value, error_prefix))) { /* PySequence_Fast sets the error */ return -1; } size = PySequence_Fast_GET_SIZE(value_fast); if (size > array_max || size < array_min) { if (array_max == array_min) { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected %d", error_prefix, size, array_max); } else { PyErr_Format(PyExc_ValueError, "%.200s: sequence size is %d, expected [%d - %d]", error_prefix, size, array_min, array_max); } Py_DECREF(value_fast); return -1; } return mathutils_array_parse_fast(array, size, value_fast, error_prefix); } }
//------------------------obj * obj------------------------------ //mulplication static PyObject *Quaternion_mul(PyObject *q1, PyObject *q2) { float quat[QUAT_SIZE], scalar; QuaternionObject *quat1 = NULL, *quat2 = NULL; if (QuaternionObject_Check(q1)) { quat1 = (QuaternionObject *)q1; if (BaseMath_ReadCallback(quat1) == -1) return NULL; } if (QuaternionObject_Check(q2)) { quat2 = (QuaternionObject *)q2; if (BaseMath_ReadCallback(quat2) == -1) return NULL; } if (quat1 && quat2) { /* QUAT * QUAT (cross product) */ mul_qt_qtqt(quat, quat1->quat, quat2->quat); return Quaternion_CreatePyObject(quat, Py_NEW, Py_TYPE(q1)); } /* the only case this can happen (for a supported type is "FLOAT * QUAT") */ else if (quat2) { /* FLOAT * QUAT */ if (((scalar = PyFloat_AsDouble(q1)) == -1.0f && PyErr_Occurred()) == 0) { return quat_mul_float(quat2, scalar); } } else if (quat1) { /* QUAT * VEC */ if (VectorObject_Check(q2)) { VectorObject *vec2 = (VectorObject *)q2; float tvec[3]; if (vec2->size != 3) { PyErr_SetString(PyExc_ValueError, "Vector multiplication: " "only 3D vector rotations (with quats) " "currently supported"); return NULL; } if (BaseMath_ReadCallback(vec2) == -1) { return NULL; } copy_v3_v3(tvec, vec2->vec); mul_qt_v3(quat1->quat, tvec); return Vector_CreatePyObject(tvec, 3, Py_NEW, Py_TYPE(vec2)); } /* QUAT * FLOAT */ else if ((((scalar = PyFloat_AsDouble(q2)) == -1.0f && PyErr_Occurred()) == 0)) { return quat_mul_float(quat1, scalar); } } else { BLI_assert(!"internal error"); } PyErr_Format(PyExc_TypeError, "Quaternion multiplication: " "not supported between '%.200s' and '%.200s' types", Py_TYPE(q1)->tp_name, Py_TYPE(q2)->tp_name); return NULL; }