static int
Scan_init(Scan *self, PyObject *args, PyObject *kwds)
{
Laser * py_laser = NULL;
int span = 1;
static char* argnames[] = {"laser", "span", NULL};
if(!PyArg_ParseTupleAndKeywords(args, kwds,"O|i", argnames, &py_laser, &span))
{
return error_on_raise_argument_exception("Scan");
}
if (error_on_check_argument_type((PyObject *)py_laser, &pybreezyslam_LaserType, 0,
"pybreezyslam.Laser", "Scan", "__init__"))
{
return 1;
}
self->laser = pylaser2claser(py_laser);
scan_init(&self->scan, py_laser->scan_size, span);
self->lidar_mm = int_alloc(py_laser->scan_size);
return 0;
}
// This function build a node of the kdtree with the
// points indexed by pidx with length "len"
// sortidx is a pre-computed array using the heapsort
// algorithm above
int KDTree::build_kdtree(int **sortidx, int dim,
int *pidx, int len, AABB ¤tBB)
{
static const Vec3 BufferOffset( KD_SPLIT_BUFFER_SIZE, KD_SPLIT_BUFFER_SIZE, KD_SPLIT_BUFFER_SIZE );
AABB b1, b2;
int ncnt = nodeMemCnt;
struct KDTreeNode *node = node_alloc();
node->bounds.Set( currentBB.m_Bounds[0] - BufferOffset, currentBB.m_Bounds[1] + BufferOffset );
node->axis = dim;
if (len <= m_KDCellSize &&
m_KDCellSize == 1 )
{
node->leftIdx = -1;
node->rightIdx = -1;
node->pntidx = pidx[0];
node->key = 0;
node->bounds.Set( currentBB.m_Bounds[0] - BufferOffset, currentBB.m_Bounds[1] + BufferOffset );
//add objects to this node
if( m_NodeCreationCallBack )
{
(*m_NodeCreationCallBack)( node );
}
return ncnt;
}
// If not, we must make a node
int pivot = -1;
int lcnt = 0, rcnt = 0;
int *larray, *rarray;
// Find the pivot (index of median point of available
// points on current dimension).
// If heapsorting the current list of points is quicker than
// iterating through all the points, just do that instead
// (heapsort if of order n*log(n)
// This test could probably use some fine tuning
if((double)len*log((double)len) < npoints) {
heapsort(dim,pidx,len);
larray = pidx;
rarray = pidx+len/2;
pivot = pidx[len/2];
lcnt = len/2;
rcnt = len/2 + (len%2==0 ? 0 : 1);
}
else
{
// Use the previously calculated sort index
// to make this a process linear in npoints
// This gets a little confusing, but it works.
// Sortidx:: sortidx[dim][idx] = val
// idx = the index to the point
// val = the order in the array
int *parray = workArr;
// Setting parray to -1 indicates we are not using
// the point
for (int i = 0; i < npoints; i++)
parray[i] = -1;
// Populate "parray" with the points that we
// are using, indexed in the order they occur
// on the current dimension
for (int i = 0; i < len; i++)
parray[sortidx[dim][pidx[i]]] = pidx[i];
int cnt = 0;
larray = int_alloc(len/2+1);
rarray = int_alloc(len/2+1);
// The middle valid value of parray is the pivot,
// the left go to a node on the left, the right
// go to a node on the right.
for (int i = 0; i < npoints; i++) {
if (parray[i] == -1)
continue;
if (cnt == len / 2) {
pivot = parray[i];
rarray[rcnt++] = parray[i];
} else if (cnt > len / 2)
rarray[rcnt++] = parray[i];
else
larray[lcnt++] = parray[i];
cnt++;
if(cnt>len)
break;
}
}
if (len <= m_KDCellSize &&
m_KDCellSize > 1 )
{
node->leftIdx = -1;
node->rightIdx = -1;
if( m_KDCellSize > 1 )
{
for( int i = 0; i < len; i++ )
{
node->fattyCellData.push_back( pidx[ i ] );
}
}
node->pntidx = pivot;
node->key = 0;
node->bounds.Set( currentBB.m_Bounds[0] - BufferOffset, currentBB.m_Bounds[1] + BufferOffset );
//add objects to this node
if( m_NodeCreationCallBack )
{
(*m_NodeCreationCallBack)( node );
}
return ncnt;
}
// Create the node
node->pntidx = -1;
node->key = points[pivot*ndim+dim] + KEY_EPSILON;
//calculate bounding boxes
if( m_NodeCreationCallBack )
{
b1 = b2 = currentBB; //b1 is for left, b2 for right
//split on the x axis
int realDim = dim%ndim;
if( realDim == 0 ){
b1.m_Bounds[1].x = node->key; //set tmax bounds of left bb to be new split point
b2.m_Bounds[0].x = node->key; //set tmin bounds of right bb to be new split point
}else if( realDim == 1 )
{
b1.m_Bounds[1].y = node->key; //set tmax bounds of left bb to be new split point
b2.m_Bounds[0].y = node->key; //set tmin bounds of right bb to be new split point
}else if( realDim == 2 )
{
b1.m_Bounds[1].z = node->key; //set tmax bounds of left bb to be new split point
b2.m_Bounds[0].z = node->key; //set tmin bounds of right bb to be new split point
}
}
// Create nodes to the left
node->leftIdx =
build_kdtree(sortidx, (dim + 1) % ndim, larray, lcnt, b1);
// Create nodes to the right
node->rightIdx =
build_kdtree(sortidx, (dim + 1) % ndim, rarray, rcnt, b2);
return ncnt;
} // end of build_kdtree
int
main ()
{
{
struct cons *c;
printf ("cons-alloc:\n");
c = int_alloc (0x10);
c = cons_alloc (c, NULL); // make a list out of it
printf ("%d\n", *((int *) c->first.c->first.p));
printf ("--------------------\n");
printf ("cons-insert-tail:\n");
struct cons *curs = cons_alloc (NULL, NULL);
cons_insert_tail (curs, int_alloc (10));
cons_insert_tail (curs, int_alloc (20));
cons_insert_tail (curs, int_alloc (30));
cons_insert_tail (curs, int_alloc (40));
c = (struct cons *) curs->first.p;
printf ("%d\n", *(int *) ((struct cons *) c->first.p)->first.p);
printf ("%d\n", *(int *) ((struct cons *) c->next->first.p)->first.p);
printf ("%d\n",
*(int *) ((struct cons *) c->next->next->first.p)->first.p);
printf ("%d\n",
*(int *) ((struct cons *) c->next->next->next->first.p)->first.p);
printf ("--------------------\n");
}
printf ("cons-alloc2:\n");
struct cons *c = // (798 "foo")
cons_alloc (int_alloc (798),
cons_alloc (string_alloc ("foo"),
NULL));
printf ("798 == %d\n", int_unbox (c->first.p));
printf ("foo == %s\n", string_unbox (c->next->first.p));
printf ("--------------------\n");
printf ("cons-insert-tail:\n");
struct cons *curs = cons_alloc (NULL, NULL);
cons_insert_tail (curs, int_alloc (10));
cons_insert_tail (curs, int_alloc (20));
cons_insert_tail (curs, int_alloc (30));
cons_insert_tail (curs, int_alloc (40));
c = (struct cons *) curs->first.p;
free (curs);
printf ("10 == %d\n", int_unbox (c->first.p));
printf ("20 == %d\n", int_unbox (c->next->first.p));
printf ("30 == %d\n", int_unbox (c->next->next->first.p));
printf ("40 == %d\n", int_unbox (c->next->next->next->first.p));
printf ("--------------------\n");
printf ("htrie-alloc:\n");
{
struct bin *b = NULL;
htrie_assoc (&b, "abc", int_alloc (10));
htrie_assoc (&b, "abcd", int_alloc (15));
htrie_assoc (&b, "abb", int_alloc (20));
htrie_assoc (&b, "aba", int_alloc (30));
htrie_assoc (&b, "ddd", int_alloc (40));
htrie_assoc (&b, "eff", int_alloc (50));
printf ("get abc -> %d\n", int_unbox (htrie_get (b, "abc")));
printf ("get abcd -> %d\n", int_unbox (htrie_get (b, "abcd")));
printf ("get abb -> %d\n", int_unbox (htrie_get (b, "abb")));
printf ("get aba -> %d\n", int_unbox (htrie_get (b, "aba")));
printf ("get ddd -> %d\n", int_unbox (htrie_get (b, "ddd")));
printf ("get eff -> %d\n", int_unbox (htrie_get (b, "eff")));
printf ("get abde -> %p\n", htrie_get (b, "abde"));
}
printf ("--------------------\n");
printf ("htrie dict test:\n");
{
FILE *f;
char line_storage[1024];
int i = 0;
struct bin *b = NULL;
f = fopen ("/usr/share/dict/american-english", "r");
while (fgets (line_storage, 1024, f))
{
line_storage[strlen (line_storage) - 1] = 0; // remove trailing \n.
printf ("\"%s\"\n", line_storage);
htrie_assoc (&b, line_storage, int_alloc (i++));
}
fclose (f);
printf ("get A's -> %d\n", int_unbox (htrie_get (b, "A's")));
printf ("get zoom -> %d\n", int_unbox (htrie_get (b, "zoom")));
}
printf ("--------------------\n");
return 0;
}