--HG--

branch : py3dutil

cgrid.c

@@ -1,6 +1,7 @@
 #include "cgrid.h"
 #include "obarr.h"
 #include "red_black_tree.h"
+#include <math.h>
 
 void cgrid_unroll(CgridObject* self)
 {
@@ -117,12 +118,128 @@ void cgrid_printinfo(void* a)
 	printf("<Obarr size %d>", arr->nSize);
 }
 
+long cgrid_coord_to_gridcoord(CgridObject* self, double coord)
+{
+	return long(floor(coord / self->dCellSize));
+}
+
+ObarrObject* cgrid_get_radius(CgridObject *self, PyObject *other, double dRadius)
+{
+	ObarrObject *pNeighbors;
+	pNeighbors = PyObject_New(ObarrObject, &ObarrObjectType);
+	if (!cgrid_get_radius_append(pNeighbors))
+	{
+		Py_DECREF(pNeighbors);
+		return NULL;
+	}
+	return pNeighbors;
+
+}
+void* cgrid_get_radius_append(CgridObject *self, PyObject *other, double dRadius, ObarrObject *pNeighbors)
+{
+	PyObject *el = NULL;
+	rb_red_blk_node* pNode = NULL;
+	PyObject *pAttr;
+	CgridInfo *pV;
+	CgridKey k;
+	double dRe;
+	double dX, dY, dZ;
+	double dXi, dYi, dZi;
+	double dXs, dYs, dZs;
+	double dXe, dYe, dZe;
+	long x, y, r, i;
+	
+    if (!PyArg_ParseTuple(args, "Od", &other, &dRadius))
+	{
+		PyErr_SetString(PyExc_TypeError, "wrong arguments");
+		return NULL;
+	}
+	
+	pAttr = PyObject_GetAttrString(other, "pos")
+    if (!pAttr)
+    {
+		PyErr_SetString(PyExc_TypeError, "missing 'pos' attribute of input object");
+		return NULL;
+    }
+    
+    if (!PyArg_ParseTuple(pAttr, "ddd", &dX, &dY, &dZ))
+	{
+		Py_DECREF(pAttr);
+		PyErr_SetString(PyExc_TypeError, "invalid 'pos' attribute of input object");
+		return NULL;
+	}
+	Py_DECREF(pAttr);
+
+	dXs = dX + dRadius + (self->dCellSize * 0.1);
+	dYs = dY + dRadius + (self->dCellSize * 0.1);
+	dZs = dZ + dRadius + (self->dCellSize * 0.1);
+	
+	for (dXi = (dX - dRadius); dXi <= dXs; dXi += self->dCellSize)
+	{
+		for (dYi = (dY - dRadius); dYi <= dYs; dYi += self->dCellSize)
+		{
+			for (dZi = (dZ - dRadius); dZi <= dZs; dZi += self->dCellSize)
+			{
+				k.x = cgrid_coord_to_gridcoord(dXi);
+				k.y = cgrid_coord_to_gridcoord(dYi);
+				k.z = cgrid_coord_to_gridcoord(dZi);
+				pNode = RBExactQuery(self->pTree, &k);
+				if (pNode)
+				{
+					pV = (CgridInfo*)pNode->info;
+					for (i = 0; i < pV->pContents->nSize; i++)
+					{
+						el = obarr_get_element(pV->pContents, i);
+						pAttr = PyObject_GetAttrString(other, "pos");
+					    if (!pAttr)
+						{
+							PyErr_SetString(PyExc_TypeError, "missing 'pos' attribute of object in grid");
+							return NULL;
+						}
+					    if (!PyArg_ParseTuple(pAttr, "ddd", &dXe, &dYe, &dZe))
+						{
+							Py_DECREF(pAttr);
+							PyErr_SetString(PyExc_TypeError, "invalid 'pos' attribute of object in grid");
+							return NULL;
+						}
+						Py_DECREF(pAttr);
+
+						pAttr = PyObject_GetAttrString(other, "radius");
+					    if (!pAttr)
+							dRe = 0.0;
+					    else if (!PyFloat_Check(pAttr))
+						{
+							Py_DECREF(pAttr);
+							PyErr_SetString(PyExc_TypeError, "invalid 'radius' attribute of object in grid");
+							return NULL;
+						}
+						else
+							dRe = PyFloat_AsDouble(pAttr);
+						Py_DECREF(pAttr);
+						
+						dDist = sqrt(SQR(dXe - dX) + SQR(dYe - dY) + SQR(dZe - dZ)) - dRe;
+						if (dDist > dRadius)
+							continue;
+						if (!obarr_append(pNeighbors, el))
+						{
+							PyErr_SetString(PyExc_MemoryError, "out of memory");
+							return NULL;
+						}
+					}
+				}
+			}
+		}
+	}
+	
+	return pNeighbors;
+}
+
 
 int Cgrid_init(CgridObject *self, PyObject *args, PyObject *kwds)
 {
-	double fCell;
+	double dCell;
 	
-    if (!PyArg_ParseTuple(args, "d", &fCell))
+    if (!PyArg_ParseTuple(args, "d", &dCell))
 	{
 		PyErr_SetString(PyExc_TypeError, "wrong arguments");
 		return 1;
@@ -131,7 +248,7 @@ int Cgrid_init(CgridObject *self, PyObject *args, PyObject *kwds)
 	self->pTree = RBTreeCreate(cgrid_compare, cgrid_destroykey, cgrid_destroyinfo, cgrid_printkey, cgrid_printinfo);
 	self->nSize = 0;
 	self->nCells = 0;
-	self->fCellSize = fCell;
+	self->dCellSize = dCell;
 	self->pUnrolled = NULL;
 	self->bUnrollDirty = 1;
 
@@ -355,6 +472,24 @@ PyObject* Cgrid_remove(PyObject *self_in, PyObject *args)
 
 }
 
+PyObject* Cgrid_get_radius(PyObject *self_in, PyObject *args)
+{
+	CgridObject *self = (CgridObject*)self_in;
+	PyObject *other = NULL;
+	double dRadius;
+		
+    if (!PyArg_ParseTuple(args, "Od", &other, &dRadius))
+	{
+		PyErr_SetString(PyExc_TypeError, "wrong arguments");
+		return NULL;
+	}
+	
+	return cgrid_get_radius(self, other, dRadius);	
+}
+	
+
+
+
 PySequenceMethods Cgrid_as_seq[] = {
 	Cgrid_len,			/* sq_length */
 	0,					/* sq_concat */
@@ -384,7 +519,7 @@ struct PyMemberDef Cgrid_members[] = {
 PyTypeObject CgridObjectType = {
 	PyObject_HEAD_INIT(NULL)
 	0,				/* ob_size        */
-	"pyobarr.cgrid",		/* tp_name        */
+	"py3dutil.cgrid",		/* tp_name        */
 	sizeof(CgridObject),		/* tp_basicsize   */
 	0,				/* tp_itemsize    */
 	Cgrid_dealloc,	/* tp_dealloc     */

cgrid.h

@@ -15,7 +15,7 @@ typedef struct CgridObject {
 	rb_red_blk_tree*	pTree;
 	long				nSize;
 	long				nCells;
-	double				fCellSize;
+	double				dCellSize;
 	ObarrObject*		pUnrolled;
 	int					bUnrollDirty;
 } CgridObject;
@@ -37,6 +37,7 @@ typedef struct CgridInfo {
 	
 } CgridInfo;
 
+#define SQR(x) ((x) * (x))
 
 /* internal functions */
 void cgrid_unroll(CgridObject* self);
@@ -47,6 +48,9 @@ void cgrid_destroykey(void* a);
 void cgrid_destroyinfo(void* a);
 void cgrid_printkey(const void* a);
 void cgrid_printinfo(void* a);
+long cgrid_coord_to_gridcoord(CgridObject* self, double coord);
+ObarrObject* cgrid_get_radius(CgridObject* self, PyObject* other, double dRadius);
+void cgrid_get_radius_append(CgridObject* self, PyObject* other, double dRadius, ObarrObject* pNeighbors);
 
 /* exported API functions */
 int Cgrid_init(CgridObject *self, PyObject *args, PyObject *kwds);
@@ -58,6 +62,7 @@ PyObject* Cgrid_repr(PyObject *self_in);
 PyObject* Cgrid_insert(PyObject *self_in, PyObject *args);
 PyObject* Cgrid_delete(PyObject *self_in, PyObject *args);
 PyObject* Cgrid_remove(PyObject *self_in, PyObject *args);
+PyObject* Cgrid_get_radius(PyObject *self_in, PyObject *args);
 /*PyObject* Cgrid_remove(PyObject *self_in, PyObject *args);*/
 
 extern PySequenceMethods Cgrid_as_seq[];

obarr.c

@@ -129,6 +129,16 @@ int obarr_set_size(ObarrObject* self, long size)
 	return 1;
 }
 
+int obarr_append(ObarrObject *self, PyObject *other)
+{
+	if (obarr_set_size(self, self->nSize + 1))
+	{
+		obarr_set_element(self, self->nSize - 1, other);
+		return 1;
+	}
+	return 0;
+}
+
 int Obarr_init(ObarrObject *self, PyObject *args, PyObject *kwds)
 {
 	self->nSize = 0;
@@ -423,9 +433,6 @@ PyObject* Obarr_debug(PyObject* self_in, PyObject* args)
 	
 }
 
-
-
-
 /* Python object definition structures */
 PySequenceMethods Obarr_as_seq[] = {
 	Obarr_len,			/* sq_length */

obarr.h

@@ -26,6 +26,7 @@ void obarr_empty(ObarrObject* self);
 void obarr_del_index(ObarrObject* self, long i);
 int obarr_valid_index(ObarrObject* self, long i);
 int obarr_set_size(ObarrObject* self, long size);
+int obarr_append(ObarrObject* self, PyObject* other);
 
 /* exposed API functions (note uppercase Obarr) */
 int Obarr_init(ObarrObject *self, PyObject *args, PyObject *kwds);

py3dutil.c

@@ -1,5 +1,5 @@
 #include "obarr.h"
-#include "cgrid.h"
+/*#include "cgrid.h"*/
 #include "vect.h"
 
 
@@ -13,12 +13,12 @@ initpy3dutil(void)
 	PyObject* m;
 
 	ObarrObjectType.tp_new = PyType_GenericNew;
-	CgridObjectType.tp_new = PyType_GenericNew;
+	/*CgridObjectType.tp_new = PyType_GenericNew;*/
 	VectObjectType.tp_new = PyType_GenericNew;
 	if (PyType_Ready(&ObarrObjectType) < 0)
 		return;
-	if (PyType_Ready(&CgridObjectType) < 0)
-		return;
+	/*if (PyType_Ready(&CgridObjectType) < 0)
+		return;*/
 	if (PyType_Ready(&VectObjectType) < 0)
 		return;
 
@@ -29,8 +29,9 @@ initpy3dutil(void)
 		return;
 
 	Py_INCREF(&ObarrObjectType);
-	Py_INCREF(&CgridObjectType);
+	/*Py_INCREF(&CgridObjectType);*/
+	Py_INCREF(&VectObjectType);
 	PyModule_AddObject(m, "obarr", (PyObject *)&ObarrObjectType);
-	PyModule_AddObject(m, "cgrid", (PyObject *)&CgridObjectType);
+	/*PyModule_AddObject(m, "cgrid", (PyObject *)&CgridObjectType);*/
 	PyModule_AddObject(m, "vect", (PyObject *)&VectObjectType);
 }

red_black_tree.c

@@ -630,6 +630,16 @@ void RBDelete(rb_red_blk_tree* tree, rb_red_blk_node* z){
 }
 
 
+void* RBExactQueryInfo(rb_red_blk_tree* tree, void* key)
+{
+	rb_red_blk_node* pNode = NULL;
+	pNode = RBExactQuery(tree, key);
+	if (pNode)
+		return pNode->info;
+	return NULL;
+}
+
+
 /***********************************************************************/
 /*  FUNCTION:  RBDEnumerate */
 /**/

red_black_tree.h

@@ -65,6 +65,8 @@ void RBTreeDestroy(rb_red_blk_tree*);
 rb_red_blk_node* TreePredecessor(rb_red_blk_tree*,rb_red_blk_node*);
 rb_red_blk_node* TreeSuccessor(rb_red_blk_tree*,rb_red_blk_node*);
 rb_red_blk_node* RBExactQuery(rb_red_blk_tree*, void*);
+void* RBExactQueryInfo(rb_red_blk_tree*, void*);
+
 #ifdef HAS_STACK
 stk_stack * RBEnumerate(rb_red_blk_tree* tree,void* low, void* high);
 #endif

setup.py

@@ -2,7 +2,7 @@ from distutils.core import setup, Extension
 from cPickle import load, dump
 import os
 
-module1 = Extension('py3dutil', sources = ['py3dutil.c', 'obarr.c', 'cgrid.c', 'red_black_tree.c', 'misc.c', 'vect.c'])
+module1 = Extension('py3dutil', sources = ['py3dutil.c', 'obarr.c', 'red_black_tree.c', 'misc.c', 'vect.c'])
 
 buildno = 0
 if os.path.exists('buildno'):

vect.c

@@ -407,23 +407,31 @@ PyObject* Vect_dotprod(PyObject *self_in, PyObject *args)
 	return PyFloat_FromDouble(acos(d) * RAD2DEG);
 }
 
-PyObject* Vect_crossprod(PyObject *self_in, PyObject *unused)
+#define A1 self->elements[0]
+#define A2 self->elements[1]
+#define A3 self->elements[2]
+#define B1 other->elements[0]
+#define B2 other->elements[1]
+#define B3 other->elements[2]
+PyObject* Vect_crossprod(PyObject *self_in, PyObject *args)
 {
-	VectObject *self;
-	double d;
-	long i;
+	VectObject *self, *other, *rv;
 	if (!Vect_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;
+	if (!PyArg_ParseTuple(args, "O!", &VectObjectType, &other))
+	{
+		PyErr_SetString(PyExc_TypeError, "argument is not a vector");
+		return NULL;
+	}
+	rv = PyObject_New(VectObject, &VectObjectType);
+	rv->elements[0] = (A2*B3) - (A3*B2);
+	rv->elements[1] = (A3*B1) - (A1*B3);
+	rv->elements[2] = (A1*B2) - (A2*B1);
+	return (PyObject*)rv;
 }
 
 PyObject* Vect_average(PyObject *self_in, PyObject *args)