Stefan Röttger

// definition of plane orientations
enum {
   x_axis = 0,
   y_axis = 1,
   z_axis = 2
};

// definition of kdtree plane
typedef struct {
   Vector3D point;
   char orientation;
} Plane;

// node definition
typedef struct NodeStruct {
   struct NodeStruct *leftSpace, *rightSpace;
   Plane plane;
   Datum item;
} Node;

// get normal of plane
//  the normal points into the right half space
Vector3D getNormal(const Plane &plane)
{
   Vector3D normal(0,0,0);

   switch (plane.orientation)
   {
      case x_axis: normal = Vector3D(1,0,0); break;
      case y_axis: normal = Vector3D(0,1,0); break;
      case z_axis: normal = Vector3D(0,0,1); break;
   }

   return(normal);
}

// euclidean distance of two points
double getDistance(const Vector3D &point1, const Vector3D &point2)
{
   Vector3D vec = point2-point1;

   return(sqrt(vec * vec));
}

// signed distance of a point to a plane
//  negative values indicate position in left half space
//  positive values indicate position in right half space
double getDistance(const Vector3D &point, const Plane &plane)
{
   Vector3D vec = point-plane.point;
   Vector3D normal = getNormal(plane);

   return(vec * normal);
}

// determines whether or not a point is in the left half space of a plane
bool isInLeftHalfSpace(const Vector3D &point, const Plane &plane)
{
   double distance = getDistance(point, plane);

   return(distance < 0.0);
}

// binary tree root
Node *root = NULL;

// insert the Datum item
//  at the position point
//  below a specific node of a kd-tree
void insert(const Vector3D &point, const Datum &item, Node **node)
{
   int level = 0;

   while (*node!=NULL)
   {
      if (isInLeftHalfSpace(point, (*node)->plane))
         node = &((*node)->leftSpace);
      else
         node = &((*node)->rightSpace);

      level++;
   }

   *node = new Node;

   (*node)->item = item;
   (*node)->leftSpace = (*node)->rightSpace = NULL;

   (*node)->plane.point = point;
   (*node)->plane.orientation = level%3;
}

// insert the Datum item
//  at the position point
//  into a kd-tree given by its root node
void insert(const Vector3D &point, const Datum &item)
{
   insert(point, item, &root);
}