--- a/src/kdtree.cc	Thu Nov 22 18:10:10 2007 +0100
+++ b/src/kdtree.cc	Thu Nov 22 21:46:09 2007 +0100
@@ -20,13 +20,8 @@
 	}
 };
 
-void KdNode::subdivide(BBox bbox, int depth, int count)
+void KdNode::subdivide(BBox bbox, int depth)
 {
-	int axis;
-	float pos;
-
-	//if (stopcriterionmet()) return
-
 	// choose split axis
 	axis = 0;
 	if (bbox.R.y - bbox.L.y > bbox.R.x - bbox.L.x)
@@ -49,81 +44,149 @@
 	sort(splitlist.begin(), splitlist.end());
 
 	// find all posible splits
-	SplitList::iterator split;
+	SplitList::iterator spl;
 	int rest;
-	for (split = splitlist.begin(); split != splitlist.end(); split++)
+	for (spl = splitlist.begin(); spl != splitlist.end(); spl++)
 	{
 		for (sh = sslist.begin(), rest = sslist.size(); sh != sslist.end(); sh++, rest--)
 		{
 			if (sh->hasMark())
 				continue;
-			// if shape is on left side of split plane
-			if (sh->bbox.R.cell[axis] <= split->pos)
+			// if shape is completely contained in split plane
+			if (spl->pos == sh->bbox.L.cell[axis] == sh->bbox.R.cell[axis])
 			{
-				sh->setMark();
-				split->lnum++;
-			}
-			// if shape is completely contained in split plane
-			if (sh->bbox.L.cell[axis] == sh->bbox.R.cell[axis] == split->pos)
-			{
-				if (split->pos - bbox.L.cell[axis] < bbox.R.cell[axis] - split->pos)
+				if (spl->pos - bbox.L.cell[axis] < bbox.R.cell[axis] - spl->pos)
 				{
 					// left subcell is smaller -> if not empty, put shape here
-					if (split->lnum)
-						split->lnum++;
+					if (spl->lnum)
+						spl->lnum++;
 					else
-						split->rnum++;
+						spl->rnum++;
 				} else {
 					// right subcell is smaller
-					if (split->rnum)
-						split->rnum++;
+					if (spl->rnum)
+						spl->rnum++;
 					else
-						split->lnum++;
+						spl->lnum++;
 				}
-			}
-			// if shape is on right side of split plane
-			if (sh->bbox.L.cell[axis] >= split->pos)
+			} else
+			// if shape is on left side of split plane
+			if (sh->bbox.R.cell[axis] <= spl->pos)
 			{
-				split->rnum += rest;
-				break;
-			}
+				sh->setMark();
+				spl->lnum++;
+			} else
 			// if shape occupies both sides of split plane
-			if (sh->bbox.L.cell[axis] < split->pos && sh->bbox.R.cell[axis] > split->pos)
+			if (sh->bbox.L.cell[axis] < spl->pos && sh->bbox.R.cell[axis] > spl->pos)
 			{
-				split->lnum++;
-				split->rnum++;
+				spl->lnum++;
+				spl->rnum++;
+			} else
+			// if shape is on right side of split plane
+			if (sh->bbox.L.cell[axis] >= spl->pos)
+			{
+				spl->rnum += rest;
+				break;
 			}
 		}
 	}
 
 	// choose best split pos
-	// ... //
-
-	KdNode *children = new KdNode[2];
-
-	ShapeList::iterator shape;
-	for (shape = shapes.begin(); shape != shapes.end(); shape++)
+	const float K = 1.4; // constant, K = cost of traversal / cost of ray-triangle intersection
+	float SAV = 2*(bbox.w()*bbox.h() + bbox.w()*bbox.d() + bbox.h()*bbox.d()); // surface area of node
+	float cost = SAV * (K + shapes.size()); // initial cost = non-split cost
+	SplitPos *splitpos = NULL;
+	leaf = true;
+	BBox lbb = bbox;
+	BBox rbb = bbox;
+	for (spl = splitlist.begin(); spl != splitlist.end(); spl++)
 	{
-		if (((Triangle*)*shape)->A.cell[axis-1] < pos
-		|| ((Triangle*)*shape)->B.cell[axis-1] < pos
-		|| ((Triangle*)*shape)->C.cell[axis-1] < pos)
-			children[0].addShape(*shape);
-		if (((Triangle*)*shape)->A.cell[axis-1] >= pos
-		|| ((Triangle*)*shape)->B.cell[axis-1] >= pos
-		|| ((Triangle*)*shape)->C.cell[axis-1] >= pos)
-			children[1].addShape(*shape);
+		// calculate SAH cost of this split
+		lbb.R.cell[axis] = spl->pos;
+		rbb.L.cell[axis] = spl->pos;
+		float SAL = 2*(lbb.w()*lbb.h() + lbb.w()*lbb.d() + lbb.h()*lbb.d());
+        float SAR = 2*(rbb.w()*rbb.h() + rbb.w()*rbb.d() + rbb.h()*rbb.d());
+        float splitcost = K + SAL/SAV*(K+spl->lnum) + SAR/SAV*(K+spl->rnum);
+
+		if (splitcost < cost)
+		{
+			leaf = false;
+			cost = splitcost;
+			splitpos = &*spl;
+		}
 	}
 
-	bbox.R[axis-1] = pos;
-	children[0].subdivide(bbox, depth+1, children[0].shapes.size());
+	if (leaf)
+		return;
+
+	split = splitpos->pos;
+	float lnum = splitpos->lnum;
+	float rnum = splitpos->rnum;
 
-	bbox.L[axis-1] = pos;
-	children[1].subdivide(bbox, depth+1, children[1].shapes.size());
+	// split this node
+	children = new KdNode[2];
+	int state = 0;
+	for (sh = sslist.begin(); sh != sslist.end(); sh++)
+	{
+		// if shape is on left side of split plane
+		if (state == 1)
+		{ // only right
+			children[1].addShape(sh->shape);
+			continue;
+		}
+		if (state == 0)
+		{
+			if (sh->bbox.R.cell[axis] < split)
+			{ // left
+				children[0].addShape(sh->shape);
+			} else
+			if (sh->bbox.R.cell[axis] > split)
+			{
+				if (sh->bbox.L.cell[axis] < split)
+				{ // both
+					children[0].addShape(sh->shape);
+					children[1].addShape(sh->shape);
+				} else
+				{ // right
+					children[1].addShape(sh->shape);
+					state = 1;
+				}
+			} else
+			{ // R == split
+				if (sh->bbox.L.cell[axis] < split)
+				{ // left
+					children[0].addShape(sh->shape);
+				} else
+				{ // contained
+					if (split - bbox.L.cell[axis] < bbox.R.cell[axis] - split)
+					{
+						// left subcell is smaller -> if not empty, put shape here
+						if (lnum)
+							children[0].addShape(sh->shape);
+						else
+							children[1].addShape(sh->shape);
+					} else {
+						// right subcell is smaller
+						if (rnum)
+							children[1].addShape(sh->shape);
+						else
+							children[0].addShape(sh->shape);
+					}
+				}
+			}
+		}
+	}
+
+	lbb.R.cell[axis] = split;
+	rbb.L.cell[axis] = split;
+
+	children[0].subdivide(lbb, depth+1);
+	children[1].subdivide(rbb, depth+1);
 }
 
 void KdTree::build()
 {
 	root = new KdNode();
 	root->shapes = shapes;
-	root->subdivide(bbox, 0, shapes.size());
+	root->subdivide(bbox, 0);
 }