--- a/src/kdtree.cc	Fri Nov 23 16:14:38 2007 +0100
+++ b/src/kdtree.cc	Sat Nov 24 21:55:41 2007 +0100
@@ -1,7 +1,63 @@
 #include <algorithm>
+#include <stack>
 
 #include "kdtree.h"
 
+class SortableShape
+{
+public:
+	Shape *shape;
+	BBox bbox;
+	short axis;
+	short mark;
+
+	SortableShape(Shape *aShape, short &aAxis): shape(aShape), axis(aAxis), mark(0)
+		{ bbox = shape->get_bbox(); };
+	friend bool operator<(const SortableShape& a, const SortableShape& b)
+		{ return a.bbox.L.cell[a.axis] < b.bbox.L.cell[b.axis]; };
+	void setAxis(short aAxis) { axis = aAxis; };
+	void setMark() { mark = 1; };
+	short hasMark() { return mark; };
+};
+
+class SortableShapeList: public vector<SortableShape>
+{
+public:
+	SortableShapeList(ShapeList &shapes, short axis)
+	{
+		ShapeList::iterator shape;
+		for (shape = shapes.begin(); shape != shapes.end(); shape++)
+			push_back(SortableShape(*shape, axis));
+	};
+};
+
+class SplitPos
+{
+public:
+	float pos;
+	int lnum, rnum;
+	SplitPos(): pos(0.0), lnum(0), rnum(0) {};
+	SplitPos(float &aPos): pos(aPos), lnum(0), rnum(0) {};
+	friend bool operator<(const SplitPos& a, const SplitPos& b)
+		{ return a.pos < b.pos; };
+	friend bool operator==(const SplitPos& a, const SplitPos& b)
+		{ return a.pos == b.pos; };
+};
+
+class SplitList: public vector<SplitPos>
+{
+};
+
+// stack element for kd-tree traversal
+struct StackElem
+{
+	KdNode* node; /* pointer to far child */
+	float t; /* the entry/exit signed distance */
+	Vector3 pb; /* the coordinates of entry/exit point */
+};
+
+// ----------------------------------------
+
 void Container::addShape(Shape* aShape)
 {
 	shapes.push_back(aShape);
@@ -238,6 +294,132 @@
 	built = true;
 }
 
+/* algorithm by Vlastimil Havran, Heuristic Ray Shooting Algorithms, appendix C */
+Shape *KdTree::nearest_intersection(const Shape *origin_shape, const Ray &ray,
+	float &nearest_distance)
+{
+	float a, b; /* entry/exit signed distance */
+	float t;    /* signed distance to the splitting plane */
+
+	/* if we have no tree, fall back to naive test */
+	if (!built)
+		return Container::nearest_intersection(origin_shape, ray, nearest_distance);
+
+	if (!bbox.intersect(ray, a, b))
+		return NULL;
+
+	stack<StackElem*> st;
+
+	/* pointers to the far child node and current node */
+	KdNode *farchild, *node;
+	node = root; /* start from the kd-tree root node */
+
+	StackElem *enPt = new StackElem();
+	enPt->t = a; /* set the signed distance */
+	enPt->node = NULL;
+
+	/* distinguish between internal and external origin */
+	if (a >= 0.0) /* a ray with external origin */
+		enPt->pb = ray.a + ray.dir * a;
+	else /* a ray with internal origin */
+		enPt->pb = ray.a;
+
+	/* setup initial exit point in the stack */
+	StackElem *exPt = new StackElem();
+	exPt->t = b;
+	exPt->pb = ray.a + ray.dir * b;
+	exPt->node = NULL; /* set termination flag */
+
+	st.push(enPt);
+
+	/* loop, traverse through the whole kd-tree, until an object is intersected or ray leaves the scene */
+	while (node)
+	{
+		/* loop until a leaf is found */
+		while (!node->isLeaf())
+		{
+			/* retrieve position of splitting plane */
+			float splitVal = node->getSplit();
+			short axis = node->getAxis();
+
+			if (enPt->pb[axis] <= splitVal)
+			{
+				if (exPt->pb[axis] <= splitVal)
+				{ /* case N1, N2, N3, P5, Z2, and Z3 */
+					node = node->getLeftChild();
+					continue;
+				}
+				if (exPt->pb[axis] == splitVal)
+				{ /* case Z1 */
+					node = node->getRightChild();
+					continue;
+				}
+				/* case N4 */
+				farchild = node->getRightChild();
+				node = node->getLeftChild();
+			}
+			else
+			{ /* (enPt->pb[axis] > splitVal) */
+				if (splitVal < exPt->pb[axis])
+				{
+					/* case P1, P2, P3, and N5 */
+					node = node->getRightChild();
+					continue;
+				}
+				/* case P4*/
+				farchild = node->getLeftChild();
+				node = node->getRightChild();
+			}
+
+			/* case P4 or N4 . . . traverse both children */
+
+			/* signed distance to the splitting plane */
+			t = (splitVal - ray.a.cell[axis]) / ray.dir.cell[axis];
+
+			/* setup the new exit point */
+			st.push(exPt);
+			exPt = new StackElem();
+
+			/* push values onto the stack */
+			exPt->t = t;
+			exPt->node = farchild;
+			exPt->pb[axis] = splitVal;
+			exPt->pb[(axis+1)%3] = ray.a.cell[(axis+1)%3] + t * ray.dir.cell[(axis+1)%3];
+			exPt->pb[(axis+2)%3] = ray.a.cell[(axis+2)%3] + t * ray.dir.cell[(axis+2)%3];
+		} /* while */
+
+		/* current node is the leaf . . . empty or full */
+		/* "intersect ray with each object in the object list, discarding "
+		"those lying before stack[enPt].t or farther than stack[exPt].t" */
+		Shape *nearest_shape = NULL;
+		float dist = exPt->t;
+		ShapeList::iterator shape;
+		for (shape = node->shapes.begin(); shape != node->shapes.end(); shape++)
+			if (*shape != origin_shape && (*shape)->intersect(ray, dist)
+			&& dist >= enPt->t)
+				nearest_shape = *shape;
+
+		if (nearest_shape)
+		{
+			nearest_distance = dist;
+			return nearest_shape;
+		}
+
+		/* pop from the stack */
+		enPt = exPt; /* the signed distance intervals are adjacent */
+
+		/* retrieve the pointer to the next node, it is possible that ray traversal terminates */
+		node = enPt->node;
+
+		// pop
+		exPt = st.top();
+		st.pop();
+	} /* while */
+
+	/* ray leaves the scene */
+	return NULL;
+}
+
 // this should save whole kd-tree with triangles distributed into leaves
 void KdTree::save(ostream &str, KdNode *node)
 {
@@ -260,5 +442,5 @@
 // load kd-tree from file/stream
 void KdTree::load(istream &str, KdNode *node)
 {
-	
+
 }