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;
}
Beispiel #2
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 &currentBB)
{
	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
Beispiel #3
0
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;


}