From 64f17707c10d6b32f154b38776c73f992c11ddfc Mon Sep 17 00:00:00 2001 From: cecilkorik Date: Sat, 27 Nov 2010 22:46:17 +0000 Subject: [PATCH] py3dutil changes --HG-- branch : py3dutil --- quat.c | 129 ++++++++++++++++++++++++++++++++++++++++++++++++++----- quat.h | 3 ++ setup.py | 2 +- vect.c | 29 ++++++++++++- vect.h | 47 ++++++++++---------- 5 files changed, 175 insertions(+), 35 deletions(-) diff --git a/quat.c b/quat.c index 9634961..d5e1d48 100644 --- a/quat.c +++ b/quat.c @@ -102,9 +102,9 @@ void quat_multiply_internal(QuatObject* q1, QuatObject* q2, QuatObject* qr) #define w1 q1->elements[3] #define w2 q2->elements[3] - 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[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); @@ -149,6 +149,9 @@ void quat_normalize_internal(QuatObject *self) { int i; double mag = quat_mag2_internal(self); + if (mag == 0.0) + return; + if (mag > (1.0 + -1e-7) && mag < (1.0 + 1e-7)) return; mag = sqrt(mag); @@ -161,7 +164,7 @@ void quat_normalize_internal(QuatObject *self) void quat_multiply_vect_internal(QuatObject *self, VectObject *v, VectObject *rv) { QuatObject *conj, *rq, *other; - double mag = quat_mag_internal(other); + double mag; int i; // used in calculation, must be freed! @@ -169,10 +172,17 @@ void quat_multiply_vect_internal(QuatObject *self, VectObject *v, VectObject *rv rq = PyObject_New(QuatObject, &QuatObjectType); other = PyObject_New(QuatObject, &QuatObjectType); + for (i = 0; i < 3; i++) + other->elements[i] = v->elements[i]; + other->elements[3] = 0.0; + + mag = quat_mag_internal(other); quat_get_conjugate_internal(self, conj); for (i = 0; i < 3; i++) rq->elements[i] = v->elements[i]; + rq->elements[3] = 0.0; + quat_multiply_internal(self, rq, other); quat_multiply_internal(other, conj, rq); for (i = 0; i < 3; i++) @@ -186,6 +196,8 @@ void quat_multiply_vect_internal(QuatObject *self, VectObject *v, VectObject *rv PyObject* Quat_mul(PyObject *self_in, PyObject *other_in) { + int i; + int bValid = 1; QuatObject *self, *rq, *other; if (!Quat_Check(self_in)) { @@ -196,12 +208,23 @@ PyObject* Quat_mul(PyObject *self_in, PyObject *other_in) if (!Quat_Check(other_in) && !Vect_Check(other_in)) self = (QuatObject*)self_in; + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat input"); + return NULL; + } if (Quat_Check(other_in)) { other = (QuatObject*)other_in; rq = PyObject_New(QuatObject, &QuatObjectType); quat_multiply_internal(self, other, rq); + if (!quat_validate(rq)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat calculation"); + Py_DECREF(rq); + return NULL; + } return (PyObject*)rq; } else if (Vect_Check(other_in)) @@ -211,6 +234,14 @@ PyObject* Quat_mul(PyObject *self_in, PyObject *other_in) rv = PyObject_New(VectObject, &VectObjectType); quat_multiply_vect_internal(self, v, rv); + for (i = 0; i < 3; i++) + if (isnan(rv->elements[i])) { bValid = 0; break; } + if (!bValid) + { + PyErr_SetString(PyExc_ValueError, "invalid quat calculation"); + Py_DECREF(rv); + return NULL; + } return (PyObject*)rv; } else @@ -230,8 +261,19 @@ PyObject* Quat_get_conjugate(PyObject *self_in, PyObject *unused) return NULL; } self = (QuatObject*)self_in; + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat input"); + return NULL; + } rv = PyObject_New(QuatObject, &QuatObjectType); quat_get_conjugate_internal(self, rv); + if (!quat_validate(rv)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat calculation"); + Py_DECREF(rv); + return NULL; + } return (PyObject*)rv; } @@ -253,6 +295,11 @@ PyObject* Quat_get_matrix(PyObject *self_in, PyObject *unused) return NULL; } self = (QuatObject*)self_in; + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat input"); + return NULL; + } 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; @@ -289,7 +336,7 @@ PyObject* Quat_get_matrix(PyObject *self_in, PyObject *unused) PyObject* Quat_get_angle(PyObject *self_in, PyObject *unused) { - QuatObject *self, *other; + QuatObject *self; int i; double x, y, z, w; double xx, yy, zz; @@ -317,8 +364,8 @@ PyObject* Quat_get_angle(PyObject *self_in, PyObject *unused) 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]; + /*for (i = 0; i < 4; i++) + self->elements[i] -= other->elements[i];*/ list = PyList_New(3); for (i = 0; i < 3; i++) @@ -344,6 +391,11 @@ PyObject* Quat_slerp(PyObject *self_in, PyObject *args) return NULL; } self = (QuatObject*)self_in; + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat input"); + return NULL; + } cv = 0.0; for (i = 0; i < 4; i++) cv += self->elements[i] * other->elements[i]; @@ -366,6 +418,14 @@ PyObject* Quat_slerp(PyObject *self_in, PyObject *args) for (i = 0; i < 4; i++) rv->elements[i] = (self->elements[i] * ss) + (other->elements[i] * ts); + if (!quat_validate(rv)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat calculation"); + Py_DECREF(rv); + return NULL; + } + + return (PyObject*)rv; } @@ -387,6 +447,13 @@ PyObject* Quat_slerp_turn(PyObject *self_in, PyObject *args) PyErr_SetString(PyExc_TypeError, "arguments must be a quat and a float"); return NULL; } + + self = (QuatObject*)self_in; + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat input"); + return NULL; + } // these need to be freed axis = PyObject_New(VectObject, &VectObjectType); @@ -427,6 +494,13 @@ PyObject* Quat_slerp_turn(PyObject *self_in, PyObject *args) for (i = 0; i < 4; i++) rv->elements[i] = (self->elements[i] * ss) + (other->elements[i] * ts); + if (!quat_validate(rv)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat calculation"); + Py_DECREF(rv); + return NULL; + } + return (PyObject*)rv; } @@ -436,7 +510,7 @@ PyObject* Quat_copy(PyObject *self_in, PyObject *unused) int i; if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "not a vector"); + PyErr_SetString(PyExc_TypeError, "not a quat"); return NULL; } self = (QuatObject*)self_in; @@ -452,12 +526,23 @@ PyObject* Quat_ip_normalize(PyObject *self_in, PyObject *unused) QuatObject *self; if (!Quat_Check(self_in)) { - PyErr_SetString(PyExc_TypeError, "not a vector"); + PyErr_SetString(PyExc_TypeError, "not a quat"); return NULL; } self = (QuatObject*)self_in; + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat input"); + return NULL; + } + quat_normalize_internal(self); + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat calculation"); + return NULL; + } Py_INCREF(self); return (PyObject*)self; } @@ -483,6 +568,11 @@ PyObject* Quat_mag2(PyObject *self_in, PyObject *unused) return NULL; } self = (QuatObject*)self_in; + if (!quat_validate(self)) + { + PyErr_SetString(PyExc_ValueError, "invalid quat input"); + return NULL; + } return PyFloat_FromDouble(quat_mag_internal(self)); } @@ -552,7 +642,26 @@ PyObject* Quat_richcompare(PyObject* a, PyObject* b, int op) return Py_NotImplemented; } +PyObject* Quat_negate_ip(PyObject* self_in) +{ + return Quat_get_conjugate(self_in, NULL); +} +int quat_validate(QuatObject* input) +{ + int bValid = 1; + int i; + + for (i = 0; i < 4; i++) + { + if (isnan(input->elements[i])) + { + bValid = 0; + break; + } + } + return bValid; +} @@ -564,7 +673,7 @@ PyNumberMethods Quat_as_number[] = { 0, /* nb_remainder */ 0, /* nb_divmod */ 0, /* nb_power */ - Quat_get_conjugate, /* nb_negative */ + Quat_negate_ip, /* nb_negative */ 0, /* nb_positive */ 0, /* nb_absolute */ Quat_true, /* nb_nonzero */ diff --git a/quat.h b/quat.h index 8a620af..9b2e0e7 100644 --- a/quat.h +++ b/quat.h @@ -24,6 +24,7 @@ double quat_mag_internal(QuatObject* self); double quat_mag2_internal(QuatObject* self); void quat_normalize_internal(QuatObject* self); void quat_get_conjugate_internal(QuatObject* self, QuatObject* rv); +int quat_validate(QuatObject* input); // Python API functions int Quat_init(QuatObject *self, PyObject *args, PyObject *kwds); @@ -41,11 +42,13 @@ PyObject* Quat_mag2(PyObject *self_in, PyObject *unused); PyObject* Quat_copy(PyObject *self_in, PyObject *unused); PyObject* Quat_get_angle(PyObject *self_in, PyObject *unused); PyObject* Quat_get_conjugate(PyObject *self_in, PyObject *unused); +PyObject* Quat_ip_negate(PyObject *self_in); PyObject* Quat_get_matrix(PyObject *self_in, PyObject *unused); PyObject* Quat_slerp(PyObject *self_in, PyObject *args); PyObject* Quat_slerp_turn(PyObject *self_in, PyObject *args); Py_ssize_t Quat_len(PyObject *self_in); PyObject* Quat_item(PyObject *self_in, Py_ssize_t index); +PyObject* Quat_richcompare(PyObject* a, PyObject* b, int op); diff --git a/setup.py b/setup.py index db783cb..d743ab0 100644 --- a/setup.py +++ b/setup.py @@ -11,7 +11,7 @@ if os.path.exists('buildno'): dump(buildno, open('buildno', 'wb')) setup (name = 'py3dutil', - version = '0.2.%.4d' % (buildno,), + version = '0.3.%.4d' % (buildno,), description = 'Accelerator library for 3d games', author = 'Bradley Lawrence', author_email = 'py3dutil@iambitter.org', diff --git a/vect.c b/vect.c index 7c69289..57334ce 100644 --- a/vect.c +++ b/vect.c @@ -619,7 +619,34 @@ PyObject* Vect_item(PyObject *self_in, Py_ssize_t index) } +PyObject* Vect_richcompare(PyObject* a, PyObject* b, int op) +{ + VectObject *v1, *v2; + double diff; + if (op == Py_EQ) + { + if (!Vect_Check(a) || !Vect_Check(b)) + { + PyErr_SetString(PyExc_TypeError, "can only compare two vects"); + return NULL; + } + v1 = (VectObject*)a; + v2 = (VectObject*)b; + diff = v1->elements[0] - v2->elements[0]; + if (diff > 1e-9 || -diff > 1e-9) + return Py_False; + diff = v1->elements[1] - v2->elements[1]; + if (diff > 1e-9 || -diff > 1e-9) + return Py_False; + diff = v1->elements[2] - v2->elements[2]; + if (diff > 1e-9 || -diff > 1e-9) + return Py_False; + return Py_True; + + } + return Py_NotImplemented; +} PyNumberMethods Vect_as_number[] = { @@ -739,7 +766,7 @@ PyTypeObject VectObjectType = { "Vector objects are simple.", /* tp_doc */ 0, /* tp_traverse */ 0, /* tp_clear */ - 0, /* tp_richcompare */ + Vect_richcompare, /* tp_richcompare */ 0, /* tp_weaklistoffset */ 0, /* tp_iter */ 0, /* tp_iternext */ diff --git a/vect.h b/vect.h index f4d75f1..a864a62 100644 --- a/vect.h +++ b/vect.h @@ -2,13 +2,13 @@ #define VECT_H_INCLUDED #include -#include - -#if PY_VERSION_HEX < 0x02050000 && !defined(PY_SSIZE_T_MIN) -typedef int Py_ssize_t; -#define PY_SSIZE_T_MAX INT_MAX -#define PY_SSIZE_T_MIN INT_MIN -#endif +#include + +#if PY_VERSION_HEX < 0x02050000 && !defined(PY_SSIZE_T_MIN) +typedef int Py_ssize_t; +#define PY_SSIZE_T_MAX INT_MAX +#define PY_SSIZE_T_MIN INT_MIN +#endif #define VEC3D @@ -20,26 +20,26 @@ typedef int Py_ssize_t; #define VECLEN 2 #endif - + typedef struct VectObject { PyObject_HEAD double elements[VECLEN]; -} VectObject; - +} VectObject; + #define Vect_Check(op) PyObject_TypeCheck(op, &VectObjectType) - - -// internal functions -PyObject* vect_get_element(PyObject* self_in, long index); -double vect_dotprod_internal(VectObject *self, VectObject *other); - + + +// internal functions +PyObject* vect_get_element(PyObject* self_in, long index); +double vect_dotprod_internal(VectObject *self, VectObject *other); + // Python API functions int Vect_init(VectObject *self, PyObject *args, PyObject *kwds); PyObject* Vect_getx(PyObject* self_in, void* closure); -PyObject* Vect_gety(PyObject* self_in, void* closure); +PyObject* Vect_gety(PyObject* self_in, void* closure); #ifdef VEC3D -PyObject* Vect_getz(PyObject* self_in, void* closure); +PyObject* Vect_getz(PyObject* self_in, void* closure); #endif int Vect_set_notallowed(PyObject* self_in, PyObject* value, void* closure); PyObject* Vect_repr(PyObject *self_in); @@ -68,11 +68,12 @@ PyObject* Vect_slerp(PyObject *self_in, PyObject *args); PyObject* Vect_sserp(PyObject *self_in, PyObject *args); Py_ssize_t Vect_len(PyObject *self_in); PyObject* Vect_item(PyObject *self_in, Py_ssize_t index); - - - - - +PyObject* Vect_richcompare(PyObject* a, PyObject* b, int op); + + + + + extern PyNumberMethods Vect_as_number[]; extern PySequenceMethods Vect_as_seq[]; extern PyGetSetDef Vect_getset[];