From 1b8f548735ce75d888afe63ba4bea439c3a08724 Mon Sep 17 00:00:00 2001 From: cecilkorik Date: Sun, 26 Oct 2008 06:26:37 +0000 Subject: [PATCH] added quat class upped version to v0.2 --HG-- branch : py3dutil --- quat.c | 878 +++++++++++++++++++++++++++++++-------------------------- 1 file changed, 479 insertions(+), 399 deletions(-) diff --git a/quat.c b/quat.c index 970efcd..5f0e98a 100644 --- a/quat.c +++ b/quat.c @@ -1,4 +1,5 @@ - +#include "quat.h" +#include "vect.h" #include #ifdef _MSC_VER @@ -6,19 +7,12 @@ #define isinf(x) ((x) != (x)) #endif -#define VECLEN 4 - #define MATH_PI (atan(1.0)*4.0) #define RAD2DEG (180.0 / MATH_PI) #define DEG2RAD (MATH_PI / 180.0) - - -staticforward PyTypeObject QuatObjectType; - -int Quat_init(VectObject *self, PyObject *args, PyObject *kwds) +int Quat_init(QuatObject *self, PyObject *args, PyObject *kwds) { -#if defined (VEC3D) double inx, iny, inz, inw; if (!PyArg_ParseTuple(args, "dddd", &inx, &iny, &inz, &inw)) return -1; @@ -31,67 +25,64 @@ int Quat_init(VectObject *self, PyObject *args, PyObject *kwds) return 0; } -PyObject* Quat_get_element(PyObject* self_in, int index) +PyObject* quat_get_element(PyObject* self_in, long index) { - VectObject* self = (VectObject*)self_in; + QuatObject* self = (QuatObject*)self_in; return PyFloat_FromDouble(self->elements[index]); } PyObject* Quat_getx(PyObject* self_in, void* closure) { - return Quat_get_element(self_in, 0); + return quat_get_element(self_in, 0); } PyObject* Quat_gety(PyObject* self_in, void* closure) { - return Quat_get_element(self_in, 1); + return quat_get_element(self_in, 1); } -#if defined (VEC3D) PyObject* Quat_getz(PyObject* self_in, void* closure) { - return Quat_get_element(self_in, 2); + return quat_get_element(self_in, 2); +} + +PyObject* Quat_getw(PyObject* self_in, void* closure) +{ + return quat_get_element(self_in, 3); } -#endif int Quat_set_notallowed(PyObject* self_in, PyObject* value, void* closure) { - PyErr_SetString(PyExc_TypeError, "Vectors cannot be set directly"); + PyErr_SetString(PyExc_TypeError, "Quaternions cannot be set directly"); return -1; } - - PyObject* Quat_repr(PyObject *self_in) { - VectObject *self; + QuatObject *self; PyObject *tuple, *fmtstring, *reprstring; if (!Quat_Check(self_in)) return PyString_FromString(""); - self = (VectObject*)self_in; + self = (QuatObject*)self_in; -#if defined (VEC3D) - tuple = Py_BuildValue("(ddd)", self->elements[0], self->elements[1], self->elements[2]); - fmtstring = PyString_FromString("vect(%f, %f, %f)"); + tuple = Py_BuildValue("(dddd)", self->elements[0], self->elements[1], self->elements[2], self->elements[3]); + fmtstring = PyString_FromString("quat(%f, %f, %f, %f)"); reprstring = PyString_Format(fmtstring, tuple); Py_DECREF(tuple); Py_DECREF(fmtstring); return reprstring; -#else - return PyString_FromString(""); -#endif } int Quat_true(PyObject *self_in) { - VectObject *self = (VectObject*)self_in; + QuatObject *self = (QuatObject*)self_in; int b = 1; int i; double x; - for (i = 0; i < VECLEN; i++) + for (i = 0; i < 4; i++) { x = self->elements[i]; b = b && (x == 0.0 || isnan(x) || isinf(x)); @@ -100,227 +91,372 @@ int Quat_true(PyObject *self_in) return !b; } -PyObject* Quat_add(PyObject *self_in, PyObject *other_in) +void quat_multiply_internal(QuatObject* q1, QuatObject* q2, QuatObject* qr) { - VectObject *self, *other, *rv; - int i; - if (!Quat_Check(self_in) || !Quat_Check(other_in)) - { - PyErr_SetString(PyExc_TypeError, "both arguments must be of type 'vect'"); - return NULL; - } - self = (VectObject*)self_in; - other = (VectObject*)other_in; - rv = PyObject_New(VectObject, &VectObjectType); - for (i = 0; i < VECLEN; i++) - rv->elements[i] = self->elements[i] + other->elements[i]; +#define x1 q1->elements[0] +#define x2 q2->elements[0] +#define y1 q1->elements[1] +#define y2 q2->elements[1] +#define z1 q1->elements[2] +#define z2 q2->elements[2] +#define w1 q1->elements[3] +#define w2 q2->elements[3] - return (PyObject*)rv; + qr->elements[0] = (w1 * x2) + (x1 * w2) + (y1 * z2) - (z1 * y2); + qr->elements[1] = (w1 * y2) + (y1 * w2) + (z1 * x2) - (x1 * z2); + qr->elements[2] = (w1 * z2) + (z1 * w2) + (x1 * y2) - (y1 * x2); + qr->elements[3] = (w1 * w2) - (x1 * x2) - (y1 * y2) - (z1 * z2); + + quat_normalize_internal(qr); + +#undef x1 +#undef x2 +#undef y1 +#undef y2 +#undef z1 +#undef z2 +#undef w1 +#undef w2 } -PyObject* Quat_sub(PyObject *self_in, PyObject *other_in) +double quat_mag2_internal(QuatObject* self) { - VectObject *self, *other, *rv; + double rv = 0.0; int i; - if (!Quat_Check(self_in) || !Quat_Check(other_in)) - { - PyErr_SetString(PyExc_TypeError, "both arguments must be of type 'vect'"); - return NULL; - } - self = (VectObject*)self_in; - other = (VectObject*)other_in; - rv = PyObject_New(VectObject, &VectObjectType); - for (i = 0; i < VECLEN; i++) - rv->elements[i] = self->elements[i] - other->elements[i]; + for (i = 0; i < 4; i++) + rv += self->elements[i] * self->elements[i]; + return rv; +} - return (PyObject*)rv; +double quat_mag_internal(QuatObject *self) +{ + double rv = 0.0; + int i; + for (i = 0; i < 4; i++) + rv += self->elements[i] * self->elements[i]; + return sqrt(rv); +} + +void quat_get_conjugate_internal(QuatObject *self, QuatObject *rv) +{ + int i; + for (i = 0; i < 3; i++) + rv->elements[i] = -self->elements[i]; + rv->elements[3] = self->elements[3]; +} + +void quat_normalize_internal(QuatObject *self) +{ + int i; + double mag = quat_mag2_internal(self); + if (mag > (1.0 + -1e-7) && mag < (1.0 + 1e-7)) + return; + mag = sqrt(mag); + + for (i = 0; i < 4; i++) + self->elements[i] = self->elements[i] / mag; + +} + +void quat_multiply_vect_internal(QuatObject *self, VectObject *v, VectObject *rv) +{ + QuatObject *conj, *rq, *other; + double mag = quat_mag_internal(other); + int i; + + // used in calculation, must be freed! + conj = PyObject_New(QuatObject, &QuatObjectType); + rq = PyObject_New(QuatObject, &QuatObjectType); + other = PyObject_New(QuatObject, &QuatObjectType); + + quat_get_conjugate_internal(self, conj); + + for (i = 0; i < 3; i++) + rq->elements[i] = v->elements[i]; + quat_multiply_internal(self, rq, other); + quat_multiply_internal(other, conj, rq); + for (i = 0; i < 3; i++) + rv->elements[i] = rq->elements[i] * mag; + + // free our calculation variables + Py_DECREF(conj); + Py_DECREF(other); + Py_DECREF(rq); } PyObject* Quat_mul(PyObject *self_in, PyObject *other_in) { - VectObject *self, *rv; - int i; - double scalar; - if (!Quat_Check(self_in) || !PyFloat_Check(other_in)) + QuatObject *self, *rq, *other; + if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "'vect' can only be multiplied by a scalar"); + PyErr_SetString(PyExc_TypeError, "incorrect object type received"); return NULL; } - self = (VectObject*)self_in; - scalar = PyFloat_AsDouble(other_in); - rv = PyObject_New(VectObject, &VectObjectType); - for (i = 0; i < VECLEN; i++) - rv->elements[i] = self->elements[i] * scalar; + if (!Quat_Check(other_in) && !Vect_Check(other_in)) + + self = (QuatObject*)self_in; + + if (Quat_Check(other_in)) + { + other = (QuatObject*)other_in; + rq = PyObject_New(QuatObject, &QuatObjectType); + quat_multiply_internal(self, other, rq); + return (PyObject*)rq; + } + else if (Vect_Check(other_in)) + { + VectObject *v, *rv; + v = (VectObject*)other_in; + rv = PyObject_New(VectObject, &VectObjectType); + + quat_multiply_vect_internal(self, v, rv); + return (PyObject*)rv; + } + else + { + PyErr_SetString(PyExc_TypeError, "quaternion can only be multiplied by a quaternion or vector"); + return NULL; + } + +} + +PyObject* Quat_get_conjugate(PyObject *self_in, PyObject *unused) +{ + QuatObject *self, *rv; + if (!Quat_Check(self_in)) + { + PyErr_SetString(PyExc_TypeError, "incorrect object type"); + return NULL; + } + self = (QuatObject*)self_in; + rv = PyObject_New(QuatObject, &QuatObjectType); + quat_get_conjugate_internal(self, rv); return (PyObject*)rv; } -PyObject* Quat_div(PyObject *self_in, PyObject *other_in) +PyObject* Quat_get_matrix(PyObject *self_in, PyObject *unused) { - VectObject *self, *rv; + QuatObject *self; int i; - double scalar; - if (!Quat_Check(self_in) || !PyFloat_Check(other_in)) + double mtx[16]; + double x, y, z, w; + double xx, yy, zz; + double xy, xz, yz; + double xw, yw, zw; + PyObject* list; + + if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "'vect' can only be divided by a scalar"); + PyErr_SetString(PyExc_TypeError, "invalid object type"); return NULL; } - self = (VectObject*)self_in; - scalar = PyFloat_AsDouble(other_in); - rv = PyObject_New(VectObject, &VectObjectType); - for (i = 0; i < VECLEN; i++) - rv->elements[i] = self->elements[i] / scalar; + self = (QuatObject*)self_in; + x = self->elements[0]; y = self->elements[1]; z = self->elements[2]; w = self->elements[3]; + xx = x * x; yy = y * y; zz = z * z; + xy = x * y; xz = x * z; yz = y * z; + xw = x * w; yw = y * w; zw = z * w; - return (PyObject*)rv; + // matrix diagonal + mtx[0] = 1.0 - (2.0 * (yy+zz)); + mtx[5] = 1.0 - (2.0 * (xx+zz)); + mtx[10] = 1.0 - (2.0 * (xx+yy)); + mtx[15] = 1.0; + + // rotation cells + mtx[1] = 2.0 * (xy-zw); + mtx[2] = 2.0 * (xz+yw); + mtx[4] = 2.0 * (xy+zw); + mtx[6] = 2.0 * (yz-xw); + mtx[8] = 2.0 * (xz-yw); + mtx[9] = 2.0 * (yz+xw); + + // right and bottom sides of matrix + mtx[3] = 0.0; + mtx[7] = 0.0; + mtx[11] = 0.0; + mtx[12] = 0.0; + mtx[13] = 0.0; + mtx[14] = 0.0; + + list = PyList_New(16); + for (i = 0; i < 16; i++) + PyList_SET_ITEM(list, i, PyFloat_FromDouble(mtx[i])); + + return (PyObject*)list; } -PyObject* Quat_ip_add(PyObject *self_in, PyObject *other_in) +PyObject* Quat_get_angle(PyObject *self_in, PyObject *unused) { - VectObject *self, *other; + QuatObject *self, *other; int i; - if (!Quat_Check(self_in) || !Quat_Check(other_in)) + double x, y, z, w; + double xx, yy, zz; + double xy, xz, yz; + double xw, yw, zw; + double a[3]; + PyObject* list; + + if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "both arguments must be of type 'vect'"); + PyErr_SetString(PyExc_TypeError, "invalid object type"); return NULL; } - self = (VectObject*)self_in; - other = (VectObject*)other_in; - for (i = 0; i < VECLEN; i++) - self->elements[i] += other->elements[i]; + self = (QuatObject*)self_in; - Py_INCREF(self); - return (PyObject*)self; -} + + x = self->elements[0]; y = self->elements[1]; z = self->elements[2]; w = self->elements[3]; + xx = x * x; yy = y * y; zz = z * z; + xy = x * y; xz = x * z; yz = y * z; + xw = x * w; yw = y * w; zw = z * w; -PyObject* Quat_ip_sub(PyObject *self_in, PyObject *other_in) -{ - VectObject *self, *other; - int i; - if (!Quat_Check(self_in) || !Quat_Check(other_in)) - { - PyErr_SetString(PyExc_TypeError, "both arguments must be of type 'vect'"); - return NULL; - } - self = (VectObject*)self_in; - other = (VectObject*)other_in; - for (i = 0; i < VECLEN; i++) + + a[0] = atan((2.0 * (xw+yz)) / (1.0 - (2.0 * (xx+yy)))) * RAD2DEG; + a[1] = asin(2.0 * (yw-xz)) * RAD2DEG; + a[2] = atan((2.0 * (zw+xy)) / (1.0 - (2.0 * (yy+zz)))) * RAD2DEG; + + for (i = 0; i < 4; i++) self->elements[i] -= other->elements[i]; - Py_INCREF(self); - return (PyObject*)self; + list = PyList_New(3); + for (i = 0; i < 3; i++) + PyList_SET_ITEM(list, i, PyFloat_FromDouble(a[i])); + + return (PyObject*)list; } -PyObject* Quat_ip_mul(PyObject *self_in, PyObject *other_in) +PyObject* Quat_slerp(PyObject *self_in, PyObject *args) { - VectObject *self; + QuatObject *self, *other, *rv; int i; - double scalar; - if (!Quat_Check(self_in) || !PyFloat_Check(other_in)) + double amt, ss, ts, cv, ang, as; + + if (!Quat_Check(self_in)) { PyErr_SetString(PyExc_TypeError, "'vect' can only be multiplied by a scalar"); return NULL; } - self = (VectObject*)self_in; - scalar = PyFloat_AsDouble(other_in); - for (i = 0; i < VECLEN; i++) - self->elements[i] *= scalar; - - Py_INCREF(self); - return (PyObject*)self; -} - -PyObject* Quat_ip_div(PyObject *self_in, PyObject *other_in) -{ - VectObject *self; - int i; - double scalar; - if (!Quat_Check(self_in) || !PyFloat_Check(other_in)) + if (!PyArg_ParseTuple(args, "O!d", &QuatObjectType, &other, &amt)) { - PyErr_SetString(PyExc_TypeError, "'vect' can only be divided by a scalar"); + PyErr_SetString(PyExc_TypeError, "arguments must be a quat and a float"); return NULL; } - self = (VectObject*)self_in; - scalar = PyFloat_AsDouble(other_in); - for (i = 0; i < VECLEN; i++) - self->elements[i] /= scalar; - - Py_INCREF(self); - return (PyObject*)self; -} - -PyObject* Quat_negate(PyObject *self_in) -{ - VectObject *self, *rv; - int i; - if (!Quat_Check(self_in)) + self = (QuatObject*)self_in; + cv = 0.0; + for (i = 0; i < 4; i++) + cv += self->elements[i] * other->elements[i]; + if (cv < 0.0) + cv = -cv; + if (1.0 - cv > 1e-7) { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; + ang = acos(cv); + as = sin(ang); + ss = sin((1.0 - amt) * ang) / as; + ts = sin(amt * ang) / as; } - self = (VectObject*)self_in; - rv = PyObject_New(VectObject, &VectObjectType); - for (i = 0; i < VECLEN; i++) - rv->elements[i] = -self->elements[i]; + else + { + ss = 1.0 - amt; + ts = amt; + } + + rv = PyObject_New(QuatObject, &QuatObjectType); + for (i = 0; i < 4; i++) + rv->elements[i] = (self->elements[i] * ss) + (other->elements[i] * ts); return (PyObject*)rv; } -PyObject* Quat_ip_negate(PyObject *self_in, PyObject *unused) +PyObject* Quat_slerp_turn(PyObject *self_in, PyObject *args) { - VectObject *self; + QuatObject *self, *other, *rv; + VectObject *axis, *v1, *v2; int i; + double amt, ss, ts, cv, ang, as; + double diff; + if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "not a vector"); + PyErr_SetString(PyExc_TypeError, "'vect' can only be multiplied by a scalar"); + return NULL; + } + if (!PyArg_ParseTuple(args, "O!d", &QuatObjectType, &other, &amt)) + { + PyErr_SetString(PyExc_TypeError, "arguments must be a quat and a float"); return NULL; } - self = (VectObject*)self_in; - for (i = 0; i < VECLEN; i++) - self->elements[i] = -self->elements[i]; - Py_INCREF(self); - return (PyObject*)self; + // these need to be freed + axis = PyObject_New(VectObject, &VectObjectType); + v1 = PyObject_New(VectObject, &VectObjectType); + v2 = PyObject_New(VectObject, &VectObjectType); + + axis->elements[0] = 0.0; axis->elements[1] = 1.0; axis->elements[2] = 0.0; + quat_multiply_vect_internal(self, axis, v1); + quat_multiply_vect_internal(other, axis, v2); + + diff = vect_dotprod_internal(v1, v2); + Py_DECREF(v1); + Py_DECREF(v2); + Py_DECREF(axis); + + if (amt >= diff) + return Quat_copy((PyObject*)other, NULL); + + cv = 0.0; + for (i = 0; i < 4; i++) + cv += self->elements[i] * other->elements[i]; + if (cv < 0.0) + cv = -cv; + if (1.0 - cv > 1e-7) + { + ang = acos(cv); + as = sin(ang); + ss = sin((1.0 - amt) * ang) / as; + ts = sin(amt * ang) / as; + } + else + { + ss = 1.0 - amt; + ts = amt; + } + + rv = PyObject_New(QuatObject, &QuatObjectType); + for (i = 0; i < 4; i++) + rv->elements[i] = (self->elements[i] * ss) + (other->elements[i] * ts); + + return (PyObject*)rv; } -PyObject* Quat_ip_zero(PyObject *self_in, PyObject *unused) +PyObject* Quat_copy(PyObject *self_in, PyObject *unused) { - VectObject *self; + QuatObject *self, *rv; int i; if (!Quat_Check(self_in)) { PyErr_SetString(PyExc_TypeError, "not a vector"); return NULL; } - self = (VectObject*)self_in; - for (i = 0; i < VECLEN; i++) - self->elements[i] = 0.0; + self = (QuatObject*)self_in; + rv = PyObject_New(QuatObject, &QuatObjectType); + for (i = 0; i < 4; i++) + rv->elements[i] = self->elements[i]; - Py_INCREF(self); - return (PyObject*)self; + return (PyObject*)rv; } PyObject* Quat_ip_normalize(PyObject *self_in, PyObject *unused) { - VectObject *self; - double mag, mag2; - int i; + QuatObject *self; if (!Quat_Check(self_in)) { PyErr_SetString(PyExc_TypeError, "not a vector"); return NULL; } - self = (VectObject*)self_in; - mag2 = 0.0; - for (i = 0; i < VECLEN; i++) - mag2 += self->elements[i] * self->elements[i]; - if ((1.0 - mag2) < -0.001 || (1.0 - mag2) > 0.001) - { - mag = sqrt(mag2); - for (i = 0; i < VECLEN; i++) - self->elements[i] /= mag; - } + self = (QuatObject*)self_in; + quat_normalize_internal(self); Py_INCREF(self); return (PyObject*)self; @@ -328,252 +464,36 @@ PyObject* Quat_ip_normalize(PyObject *self_in, PyObject *unused) PyObject* Quat_mag(PyObject *self_in, PyObject *unused) { - VectObject *self; - double d; - int i; + QuatObject *self; if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "not a vector"); + PyErr_SetString(PyExc_TypeError, "not a quat"); return NULL; } - self = (VectObject*)self_in; - d = 0.0; - for (i = 0; i < VECLEN; i++) - d += self->elements[i] * self->elements[i]; - d = sqrt(d); - - return PyFloat_FromDouble(d); + self = (QuatObject*)self_in; + return PyFloat_FromDouble(quat_mag_internal(self)); } PyObject* Quat_mag2(PyObject *self_in, PyObject *unused) { - VectObject *self; - double d; - int i; + QuatObject *self; if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "not a vector"); + PyErr_SetString(PyExc_TypeError, "not a quat"); return NULL; } - self = (VectObject*)self_in; - d = 0.0; - for (i = 0; i < VECLEN; i++) - d += self->elements[i] * self->elements[i]; - - return PyFloat_FromDouble(d); + self = (QuatObject*)self_in; + return PyFloat_FromDouble(quat_mag_internal(self)); } -PyObject* Quat_dotprod(PyObject *self_in, PyObject *args) -{ - VectObject *self, *other; - double d; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - if (!PyArg_ParseTuple(args, "O!", &VectObjectType, &other)) - { - PyErr_SetString(PyExc_TypeError, "argument is not a vector"); - return NULL; - } -/* Python code is: - value = sum([self[i] * other[i] for i in range(3)]) - if value >= 1.0: - return 0.0 - return math.acos(value) * 180.0 / math.pi -*/ - - d = 0.0; - for (i = 0; i < VECLEN; i++) - d += self->elements[i] * other->elements[i]; - if (d >= 1.0) - return PyFloat_FromDouble(0.0); - - return PyFloat_FromDouble(acos(d) * RAD2DEG); -} - -PyObject * -Quat_crossprod(PyObject *self_in, PyObject *unused) -{ - VectObject *self; - double d; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - d = 0.0; - for (i = 0; i < VECLEN; i++) - d += self->elements[i] * self->elements[i]; - - Py_INCREF(self); - return (PyObject*)self; -} - -PyObject* Quat_average(PyObject *self_in, PyObject *args) -{ - VectObject *self, *other, *rv; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - if (!PyArg_ParseTuple(args, "O!", &VectObjectType, &other)) - { - PyErr_SetString(PyExc_TypeError, "argument is not a vector"); - return NULL; - } - rv = PyObject_New(VectObject, &VectObjectType); - for (i = 0; i < VECLEN; i++) - rv->elements[i] = (self->elements[i] + other->elements[i]) / 2.0; - - return (PyObject*)rv; -} - -PyObject* Quat_dir(PyObject *self_in, PyObject *unused) -{ - VectObject *self; - double d; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - d = 0.0; - for (i = 0; i < VECLEN; i++) - d += self->elements[i] * self->elements[i]; - - return PyFloat_FromDouble(d); -} - -PyObject* Quat_copy(PyObject *self_in, PyObject *unused) -{ - VectObject *self, *rv; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - rv = PyObject_New(VectObject, &VectObjectType); - for (i = 0; i < VECLEN; i++) - rv->elements[i] = self->elements[i]; - - return (PyObject*)rv; -} - -PyObject* Quat_dist(PyObject *self_in, PyObject *args) -{ - VectObject *self, *other; - double d, dd; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - if (!PyArg_ParseTuple(args, "O!", &VectObjectType, &other)) - { - PyErr_SetString(PyExc_TypeError, "argument is not a vector"); - return NULL; - } - d = 0.0; - dd = 0.0; - for (i = 0; i < VECLEN; i++) - { - dd = self->elements[i] - other->elements[i]; - d += dd * dd; - } - - return PyFloat_FromDouble(sqrt(d)); -} - -PyObject* Quat_slerp(PyObject *self_in, PyObject *args) -{ - VectObject *self, *other, *rv; - double amt, oamt; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - amt = 0.0; - if (!PyArg_ParseTuple(args, "O!d", &VectObjectType, &other, &amt)) - { - PyErr_SetString(PyExc_TypeError, "arguments must be a vector and a float"); - return NULL; - } - oamt = 1.0 - amt; - rv = PyObject_New(VectObject, &VectObjectType); - - for (i = 0; i < VECLEN; i++) - { - rv->elements[i] = (self->elements[i] * oamt) + (other->elements[i] * amt); - } - - return (PyObject*)rv; -} - -PyObject* Quat_sserp(PyObject *self_in, PyObject *args) -{ - VectObject *self, *other, *rv, *norm; - double amt, oamt, smag, omag; - int i; - if (!Quat_Check(self_in)) - { - PyErr_SetString(PyExc_TypeError, "not a vector"); - return NULL; - } - self = (VectObject*)self_in; - amt = 0.0; - if (!PyArg_ParseTuple(args, "O!d", &VectObjectType, &other, &amt)) - { - PyErr_SetString(PyExc_TypeError, "arguments must be a vector and a float"); - return NULL; - } - oamt = 1.0 - amt; - rv = PyObject_New(VectObject, &VectObjectType); - smag = 0.0; - omag = 0.0; - for (i = 0; i < VECLEN; i++) - { - smag += self->elements[i] * self->elements[i]; - omag += other->elements[i] * other->elements[i]; - rv->elements[i] = (self->elements[i] * oamt) + (other->elements[i] * amt); - } - smag = sqrt(smag); - omag = sqrt(omag); - - norm = (VectObject*)Quat_ip_normalize((PyObject*)rv, NULL); - Py_XDECREF(norm); - - for (i = 0; i < VECLEN; i++) - rv->elements[i] *= (smag + omag) / 2.0; - return (PyObject*)rv; -} - - int Quat_len(PyObject *self_in) { if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "not a vector"); + PyErr_SetString(PyExc_TypeError, "not a quat"); return -1; } - return VECLEN; + return 4; } @@ -581,18 +501,178 @@ PyObject* Quat_item(PyObject *self_in, int index) { if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "not a vector"); + PyErr_SetString(PyExc_TypeError, "not a quat"); return NULL; } - if (index < 0 || index >= VECLEN) + if (index < 0 || index >= 4) { PyErr_SetString(PyExc_IndexError, "index not in range"); return NULL; } - return Quat_get_element(self_in, index); + return quat_get_element(self_in, index); } +PyObject* Quat_richcompare(PyObject* a, PyObject* b, int op) +{ + VectObject *axis, *v1, *v2; + QuatObject *self, *other; + double diff; + if (op == Py_EQ) + { + if (!Quat_Check(a) || !Quat_Check(b)) + { + PyErr_SetString(PyExc_TypeError, "can only compare two quats"); + return NULL; + } + self = (QuatObject*)a; + other = (QuatObject*)b; + + // these need to be freed + axis = PyObject_New(VectObject, &VectObjectType); + v1 = PyObject_New(VectObject, &VectObjectType); + v2 = PyObject_New(VectObject, &VectObjectType); + + axis->elements[0] = 0.0; axis->elements[1] = 1.0; axis->elements[2] = 0.0; + quat_multiply_vect_internal(self, axis, v1); + quat_multiply_vect_internal(other, axis, v2); + + diff = vect_dotprod_internal(v1, v2); + Py_DECREF(v1); + Py_DECREF(v2); + Py_DECREF(axis); + + if (diff < 1e-9) + return Py_True; + else + return Py_False; + + } + return Py_NotImplemented; +} + + + +PyNumberMethods Quat_as_number[] = { + 0, /* nb_add */ + 0, /* nb_subtract */ + Quat_mul, /* nb_multiply */ + 0, /* nb_divide */ + 0, /* nb_remainder */ + 0, /* nb_divmod */ + 0, /* nb_power */ + 0, /* nb_negative */ + 0, /* nb_positive */ + 0, /* nb_absolute */ + Quat_true, /* nb_nonzero */ + 0, /* nb_invert */ + 0, /* nb_lshift */ + 0, /* nb_rshift */ + 0, /* nb_and */ + 0, /* nb_xor */ + 0, /* nb_or */ + 0, /* nb_coerce */ + 0, /* nb_int */ + 0, /* nb_long */ + 0, /* nb_float */ + 0, /* nb_oct */ + 0, /* nb_hex */ + 0, /* nb_inplace_add */ + 0, /* nb_inplace_subtract */ + 0, /* nb_inplace_multiply */ + 0, /* nb_inplace_divide */ + 0, /* nb_inplace_remainder */ + 0, /* nb_inplace_power */ + 0, /* nb_inplace_lshift */ + 0, /* nb_inplace_rshift */ + 0, /* nb_inplace_and */ + 0, /* nb_inplace_xor */ + 0, /* nb_inplace_or */ + 0, /* nb_floordiv */ + 0, /* nb_truediv */ + 0, /* nb_inplace_floordiv */ + 0, /* nb_inplace_truediv */ + +}; + +PySequenceMethods Quat_as_seq[] = { + Quat_len, /* sq_length */ + 0, /* sq_concat */ + 0, /* sq_repeat */ + Quat_item, /* sq_item */ + 0, /* sq_slice */ + 0, /* sq_ass_item */ + 0, /* sq_ass_slice */ + 0, /* sq_contains */ +}; + +PyGetSetDef Quat_getset[] = { + {"x", Quat_getx, Quat_set_notallowed, "x", NULL}, + {"y", Quat_gety, Quat_set_notallowed, "y", NULL}, + {"z", Quat_getz, Quat_set_notallowed, "z", NULL}, + {"w", Quat_getw, Quat_set_notallowed, "w", NULL}, + {NULL} +}; + +PyMethodDef Quat_methods[] = { + {"__mul__", (PyCFunction)Quat_mul, METH_O|METH_COEXIST, "multiply a quat by a quat or vector"}, + {"__neg__", (PyCFunction)Quat_get_conjugate, METH_O|METH_COEXIST, "get the conjugate of the quat"}, + {"normalize", (PyCFunction)Quat_ip_normalize, METH_NOARGS, "normalize a quaternion in place"}, + {"get_matrix", (PyCFunction)Quat_get_matrix, METH_NOARGS, "get quat in matrix form (for plugging into OpenGL)"}, + {"get_angle", (PyCFunction)Quat_get_angle, METH_NOARGS, "get the angles in Euler angles"}, + {"mag", (PyCFunction)Quat_mag, METH_NOARGS, "compute the vector magnitude"}, + {"mag2", (PyCFunction)Quat_mag2, METH_NOARGS, "compute the squared vector magnitude"}, + {"get_conjugate", (PyCFunction)Quat_get_conjugate, METH_NOARGS, "get the conjugate of the quat"}, + {"copy", (PyCFunction)Quat_copy, METH_NOARGS, "makes a copy"}, + {"slerp", (PyCFunction)Quat_slerp, METH_VARARGS, "spherical linear interpolation"}, + {"slerp_turn", (PyCFunction)Quat_slerp_turn, METH_VARARGS, "limited slerp"}, + {NULL} +}; + +struct PyMemberDef Quat_members[] = { + {NULL} /* Sentinel */ +}; + + +PyTypeObject QuatObjectType = { + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ + "py3dutil.quat", /* tp_name */ + sizeof(QuatObject), /* tp_basicsize */ + 0, /* tp_itemsize */ + 0, /* tp_dealloc */ + 0, /* tp_print */ + 0, /* tp_getattr */ + 0, /* tp_setattr */ + 0, /* tp_compare */ + Quat_repr, /* tp_repr */ + Quat_as_number, /* tp_as_number */ + Quat_as_seq, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + 0, /* tp_hash */ + 0, /* tp_call */ + 0, /* tp_str */ + 0, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT|Py_TPFLAGS_CHECKTYPES|Py_TPFLAGS_HAVE_RICHCOMPARE, /* tp_flags */ + "Quaternion objects are simple.", /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + Quat_richcompare, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + Quat_methods, /* tp_methods */ + Quat_members, /* tp_members */ + Quat_getset, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + (initproc)Quat_init, /* tp_init */ +};