//------------------------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;
}
