NlogN building:

	sort once
	select subsets and kept the order (on X, Y and Z)

5 files changed, 558 insertions, 250 deletions

diff --git a/source/blender/render/intern/raytrace/rayobject_bih.cpp b/source/blender/render/intern/raytrace/rayobject_bih.cpp
new file mode 100644
index 00000000000..102a347b470
--- /dev/null
+++ b/source/blender/render/intern/raytrace/rayobject_bih.cpp
@@ -0,0 +1,248 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version. 
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2009 Blender Foundation.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Contributor(s): André Pinto.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+#include <assert.h>
+#include <stdio.h>
+
+#include "MEM_guardedalloc.h"
+#include "BKE_utildefines.h"
+#include "BLI_arithb.h"
+#include "RE_raytrace.h"
+#include "rayobject_rtbuild.h"
+#include "rayobject.h"
+
+#define BIH_NCHILDS	4
+typedef struct BIHTree BIHTree;
+
+static int  bih_intersect(BIHTree *obj, Isect *isec);
+static void bih_add(BIHTree *o, RayObject *ob);
+static void bih_done(BIHTree *o);
+static void bih_free(BIHTree *o);
+static void bih_bb(BIHTree *o, float *min, float *max);
+
+static RayObjectAPI bih_api =
+{
+	(RE_rayobject_raycast_callback) bih_intersect,
+	(RE_rayobject_add_callback)     bih_add,
+	(RE_rayobject_done_callback)    bih_done,
+	(RE_rayobject_free_callback)    bih_free,
+	(RE_rayobject_merge_bb_callback)bih_bb
+};
+
+typedef struct BIHNode BIHNode;
+struct BIHNode
+{
+	BIHNode *child[BIH_NCHILDS];
+	float bi[BIH_NCHILDS][2];
+	int split_axis;
+};
+
+struct BIHTree
+{
+	RayObject rayobj;
+
+	BIHNode *root;
+
+	BIHNode *node_alloc, *node_next;
+	RTBuilder *builder;
+
+	float bb[2][3];
+};
+
+
+RayObject *RE_rayobject_bih_create(int size)
+{
+	BIHTree *obj= (BIHTree*)MEM_callocN(sizeof(BIHTree), "BIHTree");
+	assert( RayObject_isAligned(obj) ); /* RayObject API assumes real data to be 4-byte aligned */	
+	
+	obj->rayobj.api = &bih_api;
+	obj->root = NULL;
+	
+	obj->node_alloc = obj->node_next = NULL;
+	obj->builder    = rtbuild_create( size );
+	
+	return RayObject_unalignRayAPI((RayObject*) obj);
+}
+
+static void bih_free(BIHTree *obj)
+{
+	if(obj->builder)
+		rtbuild_free(obj->builder);
+
+	if(obj->node_alloc)
+		MEM_freeN(obj->node_alloc);
+
+	MEM_freeN(obj);
+}
+
+static void bih_bb(BIHTree *obj, float *min, float *max)
+{
+	DO_MIN(obj->bb[0], min);
+	DO_MAX(obj->bb[1], max);
+}
+
+/*
+ * Tree transverse
+ */
+static int dfs_raycast(const BIHNode *const node, Isect *isec, float tmin, float tmax)
+{
+	int i;
+	int hit = 0;
+
+	const int *const offset = isec->bv_index + node->split_axis*2;
+
+	//TODO diving heuristic
+	for(i=0; i<BIH_NCHILDS; i++)
+	{
+
+		float t1 = (node->bi[i][offset[0]] - isec->start[node->split_axis]) * isec->idot_axis[node->split_axis];
+		float t2 = (node->bi[i][offset[1]] - isec->start[node->split_axis]) * isec->idot_axis[node->split_axis];
+
+		if(t1 < tmin) t1 = tmin; //t1 = MAX2(t1, tmin);
+		if(t2 > tmax) t2 = tmax; //t2 = MIN2(t2, tmax);
+
+		if(t1 <= t2)
+		{
+				if(RayObject_isAligned(node->child[i]))
+				{
+					if(node->child[i] == 0) break;
+					
+					hit |= dfs_raycast(node->child[i], isec, t1, t2);
+					if(hit && isec->mode == RE_RAY_SHADOW) return hit;
+				}
+				else
+				{
+					hit |= RE_rayobject_intersect( (RayObject*)node->child[i], isec);
+					if(hit && isec->mode == RE_RAY_SHADOW) return hit;
+				}
+
+				if(tmax > isec->labda)
+					tmax = isec->labda;
+		}
+	}
+
+	return hit;
+}
+
+static int bih_intersect(BIHTree *obj, Isect *isec)
+{
+	if(RayObject_isAligned(obj->root))
+		return dfs_raycast(obj->root, isec, 0, isec->labda);
+	else
+		return RE_rayobject_intersect( (RayObject*)obj->root, isec);
+}
+
+
+/*
+ * Builds a BIH tree from builder object
+ */
+static void bih_add(BIHTree *obj, RayObject *ob)
+{
+	rtbuild_add( obj->builder, ob );
+}
+
+static BIHNode *bih_new_node(BIHTree *tree, int nid)
+{
+	BIHNode *node = tree->node_alloc + nid - 1;
+	assert(RayObject_isAligned(node));
+	if(node+1 > tree->node_next)
+		tree->node_next = node+1;
+		
+	return node;
+}
+
+static int child_id(int pid, int nchild)
+{
+	//N child of node A = A * K + (2 - K) + N, (0 <= N < K)
+	return pid*BIH_NCHILDS+(2-BIH_NCHILDS)+nchild;
+}
+
+static BIHNode *bih_rearrange(BIHTree *tree, RTBuilder *builder, int nid, float *bb)
+{
+	if(rtbuild_size(builder) == 1)
+	{
+		RayObject *child = rtbuild_get_primitive( builder, 0 );
+		assert(!RayObject_isAligned(child));
+
+		INIT_MINMAX(bb, bb+3);
+		RE_rayobject_merge_bb( (RayObject*)child, bb, bb+3);
+
+		return (BIHNode*)child;
+	}
+	else
+	{
+		int i;
+		int nc = rtbuild_mean_split_largest_axis(builder, BIH_NCHILDS);
+		RTBuilder tmp;
+	
+		BIHNode *parent = bih_new_node(tree, nid);
+
+		INIT_MINMAX(bb, bb+3);
+		parent->split_axis = builder->split_axis;
+		for(i=0; i<nc; i++)
+		{
+			float cbb[6];
+			parent->child[i] = bih_rearrange( tree, rtbuild_get_child(builder, i, &tmp), child_id(nid,i), cbb );
+
+			parent->bi[i][0] = cbb[parent->split_axis];
+			parent->bi[i][1] = cbb[parent->split_axis+3];
+
+			DO_MIN(cbb  , bb);
+			DO_MAX(cbb+3, bb+3);
+		}
+		for(; i<BIH_NCHILDS; i++)
+		{
+			parent->bi[i][0] =  1.0;
+			parent->bi[i][1] = -1.0;
+			parent->child[i] = 0;
+		}
+
+		return parent;
+	}
+}
+
+static void bih_done(BIHTree *obj)
+{
+	int needed_nodes;
+	assert(obj->root == NULL && obj->node_alloc == NULL && obj->builder);
+
+	//TODO exact calculate needed nodes
+	needed_nodes = (rtbuild_size(obj->builder)+1)*2;
+	assert(needed_nodes > 0);
+
+	obj->node_alloc = (BIHNode*)MEM_mallocN( sizeof(BIHNode)*needed_nodes, "BIHTree.Nodes");
+	obj->node_next  = obj->node_alloc;
+
+	obj->root = bih_rearrange( obj, obj->builder, 1, (float*)obj->bb );
+
+	rtbuild_free( obj->builder );
+	obj->builder = NULL;
+	
+	assert(obj->node_alloc+needed_nodes >= obj->node_next);
+}
+
diff --git a/source/blender/render/intern/raytrace/rayobject_bvh.cpp b/source/blender/render/intern/raytrace/rayobject_bvh.cpp
index 435c49cdc42..48c1ac07cd4 100644
--- a/source/blender/render/intern/raytrace/rayobject_bvh.cpp
+++ b/source/blender/render/intern/raytrace/rayobject_bvh.cpp
@@ -115,7 +115,7 @@ static BVHNode *bvh_rearrange(BVHTree *tree, RTBuilder *builder, int nid, float
 
 	if(rtbuild_size(builder) == 1)
 	{
-		RayObject *child = builder->begin[0];
+		RayObject *child = rtbuild_get_primitive( builder, 0 );
 
 		if(RayObject_isRayFace(child))
 		{
@@ -127,7 +127,7 @@ static BVHNode *bvh_rearrange(BVHTree *tree, RTBuilder *builder, int nid, float
 
 			for(i=0; i<1; i++)
 			{
-				parent->child[i] = (BVHNode*)builder->begin[i];
+				parent->child[i] = (BVHNode*)rtbuild_get_primitive( builder, i );
 				bvh_node_merge_bb(parent->child[i], parent->bb, parent->bb+3);
 			}
 			for(; i<BVH_NCHILDS; i++)
@@ -176,6 +176,8 @@ static BVHNode *bvh_rearrange(BVHTree *tree, RTBuilder *builder, int nid, float
 		*cost += nc*RAY_BB_TEST_COST;
 		return parent;
 	}
+
+	assert(false);
 }
 
 template<>
diff --git a/source/blender/render/intern/raytrace/rayobject_rtbuild.cpp b/source/blender/render/intern/raytrace/rayobject_rtbuild.cpp
index b0ac54bef2c..fb2d05a0a14 100644
--- a/source/blender/render/intern/raytrace/rayobject_rtbuild.cpp
+++ b/source/blender/render/intern/raytrace/rayobject_rtbuild.cpp
@@ -8,21 +8,23 @@
 #include "BLI_arithb.h"
 #include "BKE_utildefines.h"
 
-static int partition_nth_element(RTBuilder *b, int _begin, int _end, int n);
-static void split_leafs(RTBuilder *b, int *nth, int partitions, int split_axis);
-static int split_leafs_by_plane(RTBuilder *b, int begin, int end, float plane);
-
-static void rtbuild_init(RTBuilder *b, RayObject **begin, RayObject **end)
+static bool selected_node(RTBuilder::Object *node)
 {
-	int i;
+	return node->selected;
+}
 
-	b->begin = begin;
-	b->end   = end;
-	b->split_axis = 0;
-	b->child_sorted_axis = -1;
+static void rtbuild_init(RTBuilder *b)
+{
+	b->split_axis = -1;
+	b->primitives.begin   = 0;
+	b->primitives.end     = 0;
+	b->primitives.maxsize = 0;
 	
-	for(i=0; i<RTBUILD_MAX_CHILDS; i++)
+	for(int i=0; i<RTBUILD_MAX_CHILDS; i++)
 		b->child_offset[i] = 0;
+
+	for(int i=0; i<3; i++)
+		b->sorted_begin[i] = b->sorted_end[i] = 0;
 		
 	INIT_MINMAX(b->bb, b->bb+3);
 }
@@ -30,60 +32,129 @@ static void rtbuild_init(RTBuilder *b, RayObject **begin, RayObject **end)
 RTBuilder* rtbuild_create(int size)
 {
 	RTBuilder *builder  = (RTBuilder*) MEM_mallocN( sizeof(RTBuilder), "RTBuilder" );
-	RayObject **memblock= (RayObject**)MEM_mallocN( sizeof(RayObject*)*size,"RTBuilder.objects");
-	rtbuild_init(builder, memblock, memblock);
+	RTBuilder::Object *memblock= (RTBuilder::Object*)MEM_mallocN( sizeof(RTBuilder::Object)*size,"RTBuilder.objects");
+
+
+	rtbuild_init(builder);
+	
+	builder->primitives.begin = builder->primitives.end = memblock;
+	builder->primitives.maxsize = size;
+	
+	for(int i=0; i<3; i++)
+	{
+		builder->sorted_begin[i] = (RTBuilder::Object**)MEM_mallocN( sizeof(RTBuilder::Object*)*size,"RTBuilder.sorted_objects");
+		builder->sorted_end[i]   = builder->sorted_begin[i];
+	} 
+	
+
 	return builder;
 }
 
 void rtbuild_free(RTBuilder *b)
 {
-	MEM_freeN(b->begin);
+	if(b->primitives.begin) MEM_freeN(b->primitives.begin);
+
+	for(int i=0; i<3; i++)
+		if(b->sorted_begin[i])
+			MEM_freeN(b->sorted_begin[i]);
+
 	MEM_freeN(b);
 }
 
 void rtbuild_add(RTBuilder *b, RayObject *o)
 {
-	*(b->end++) = o;
-}
+	assert( b->primitives.begin + b->primitives.maxsize != b->primitives.end );
+	
+	b->primitives.end->obj = o;
+	b->primitives.end->cost = RE_rayobject_cost(o);
 
-void rtbuild_calc_bb(RTBuilder *b)
-{
-	if(b->bb[0] == 1.0e30f)
+	INIT_MINMAX(b->primitives.end->bb, b->primitives.end->bb+3);
+	RE_rayobject_merge_bb(o, b->primitives.end->bb, b->primitives.end->bb+3);
+	
+	for(int i=0; i<3; i++)
 	{
-		RayObject **index = b->begin;
-		for(; index != b->end; index++)
-			RE_rayobject_merge_bb(*index, b->bb, b->bb+3);
+		*(b->sorted_end[i]) = b->primitives.end;
+		b->sorted_end[i]++;
 	}
+	b->primitives.end++;
 }
 
-void rtbuild_merge_bb(RTBuilder *b, float *min, float *max)
+int rtbuild_size(RTBuilder *b)
 {
-	rtbuild_calc_bb(b);
-	DO_MIN(b->bb, min);
-	DO_MAX(b->bb+3, max);
+	return b->sorted_end[0] - b->sorted_begin[0];
 }
 
-int rtbuild_get_largest_axis(RTBuilder *b)
+
+template<class Obj,int Axis>
+static bool obj_bb_compare(const Obj &a, const Obj &b)
 {
-	rtbuild_calc_bb(b);
-	return bb_largest_axis(b->bb, b->bb+3);
+	if(a->bb[Axis] != b->bb[Axis])
+		return a->bb[Axis] < b->bb[Axis];
+	return a->obj < b->obj;
 }
 
+template<class Item>
+static void object_sort(Item *begin, Item *end, int axis)
+{
+	if(axis == 0) return std::sort(begin, end, obj_bb_compare<Item,0> );
+	if(axis == 1) return std::sort(begin, end, obj_bb_compare<Item,1> );
+	if(axis == 2) return std::sort(begin, end, obj_bb_compare<Item,2> );
+	assert(false);
+}
 
-int rtbuild_size(RTBuilder *b)
+void rtbuild_done(RTBuilder *b)
 {
-	return b->end - b->begin;
+	for(int i=0; i<3; i++)
+	if(b->sorted_begin[i])
+		object_sort( b->sorted_begin[i], b->sorted_end[i], i );
 }
 
+RayObject* rtbuild_get_primitive(RTBuilder *b, int index)
+{
+	return b->sorted_begin[0][index]->obj;
+}
 
 RTBuilder* rtbuild_get_child(RTBuilder *b, int child, RTBuilder *tmp)
 {
-	rtbuild_init( tmp, b->begin + b->child_offset[child], b->begin + b->child_offset[child+1] );
-	tmp->child_sorted_axis = b->child_sorted_axis;
+	rtbuild_init( tmp );
+
+	for(int i=0; i<3; i++)
+		if(b->sorted_begin[i])
+		{
+			tmp->sorted_begin[i] = b->sorted_begin[i] +  b->child_offset[child  ];
+			tmp->sorted_end  [i] = b->sorted_begin[i] +  b->child_offset[child+1];
+		}
+		else
+		{
+			tmp->sorted_begin[i] = 0;
+			tmp->sorted_end  [i] = 0;
+		}
+	
 	return tmp;
 }
 
+void rtbuild_calc_bb(RTBuilder *b)
+{
+	if(b->bb[0] == 1.0e30f)
+	{
+		for(RTBuilder::Object **index = b->sorted_begin[0]; index != b->sorted_end[0]; index++)
+			RE_rayobject_merge_bb( (*index)->obj , b->bb, b->bb+3);
+	}
+}
 
+void rtbuild_merge_bb(RTBuilder *b, float *min, float *max)
+{
+	rtbuild_calc_bb(b);
+	DO_MIN(b->bb, min);
+	DO_MAX(b->bb+3, max);
+}
+
+/*
+int rtbuild_get_largest_axis(RTBuilder *b)
+{
+	rtbuild_calc_bb(b);
+	return bb_largest_axis(b->bb, b->bb+3);
+}
 
 //Left balanced tree
 int rtbuild_mean_split(RTBuilder *b, int nchilds, int axis)
@@ -142,11 +213,13 @@ int rtbuild_mean_split_largest_axis(RTBuilder *b, int nchilds)
 	int axis = rtbuild_get_largest_axis(b);
 	return rtbuild_mean_split(b, nchilds, axis);
 }
+*/
 
 /*
  * "separators" is an array of dim NCHILDS-1
  * and indicates where to cut the childs
  */
+/*
 int rtbuild_median_split(RTBuilder *b, float *separators, int nchilds, int axis)
 {
 	int size = rtbuild_size(b);
@@ -189,120 +262,84 @@ int rtbuild_median_split_largest_axis(RTBuilder *b, int nchilds)
 		
 	return rtbuild_median_split(b, separators, nchilds, la);
 }
+*/
 
 //Heuristics Object Splitter
-typedef struct CostObject CostObject;
-struct CostObject
-{
-	RayObject *obj;
-	float cost;
-	float bb[6];
-};
 
-template<class Obj,int Axis>
-bool obj_bb_compare(const Obj &a, const Obj &b)
-{
-	if(a.bb[Axis] != b.bb[Axis])
-		return a.bb[Axis] < b.bb[Axis];
-	return a.obj < b.obj;
-}
 
-void costobject_sort(CostObject *begin, CostObject *end, int axis)
+struct SweepCost
 {
-	if(axis == 0) return std::sort(begin, end, obj_bb_compare<CostObject,0> );
-	if(axis == 1) return std::sort(begin, end, obj_bb_compare<CostObject,1> );
-	if(axis == 2) return std::sort(begin, end, obj_bb_compare<CostObject,2> );
-	assert(false);
-}
-
-
+	float bb[6];
+	float cost;
+};
 
 /* Object Surface Area Heuristic splitter */
 int rtbuild_heuristic_object_split(RTBuilder *b, int nchilds)
 {
 	int size = rtbuild_size(b);		
 	assert(nchilds == 2);
+	assert(size > 1);
+	int baxis = -1, boffset = 0;
 
-	if(size <= nchilds)
-	{
-		return rtbuild_mean_split_largest_axis(b, nchilds);
-	}
-	else
+	if(size > nchilds)
 	{
 		float bcost = FLT_MAX;
-		float childrens_cost = 0;
-		int i, axis, baxis = -1, boffset = size/2, k, try_axis[3];
-		CostObject *cost   = (CostObject*)MEM_mallocN( sizeof(CostObject)*size, "RTBuilder.HeuristicObjectSplitter" );
-		float      *acc_bb = (float*)MEM_mallocN( sizeof(float)*6*size, "RTBuilder.HeuristicObjectSplitterBB" );
+		baxis = -1, boffset = size/2;
 
-		for(i=0; i<size; i++)
-		{
-			cost[i].obj = b->begin[i];
-			INIT_MINMAX(cost[i].bb, cost[i].bb+3);
-			RE_rayobject_merge_bb(cost[i].obj, cost[i].bb, cost[i].bb+3);
-			cost[i].cost = RE_rayobject_cost(cost[i].obj);
-			childrens_cost += cost[i].cost;
-		}
-		
-		if(b->child_sorted_axis >= 0 && b->child_sorted_axis < 3)
-		{
-			try_axis[0] =  b->child_sorted_axis;
-			try_axis[1] = (b->child_sorted_axis+1)%3;
-			try_axis[2] = (b->child_sorted_axis+2)%3;
-		}
-		else
-		{
-			try_axis[0] = 0;
-			try_axis[1] = 1;
-			try_axis[2] = 2;
-		}
+		SweepCost *sweep = (SweepCost*)MEM_mallocN( sizeof(SweepCost)*size, "RTBuilder.HeuristicSweep" );
 		
-		for(axis=try_axis[k=0]; k<3; axis=try_axis[++k])
+		for(int axis=0; axis<3; axis++)
 		{
-			float left_cost, right_cost;
-			float other_bb[6];
+			SweepCost sweep_left;
 
-
-
-			costobject_sort(cost, cost+size, axis);
-			for(i=size-1; i>=0; i--)
+			RTBuilder::Object **obj = b->sorted_begin[axis];
+			
+//			float right_cost = 0;
+			for(int i=size-1; i>=0; i--)
 			{
-				float *bb = acc_bb+i*6;
 				if(i == size-1)
 				{
-					VECCOPY(bb, cost[i].bb);
-					VECCOPY(bb+3, cost[i].bb+3);
+					VECCOPY(sweep[i].bb, obj[i]->bb);
+					VECCOPY(sweep[i].bb+3, obj[i]->bb+3);
+					sweep[i].cost = obj[i]->cost;
 				}
 				else
 				{
-					bb[0] = MIN2(cost[i].bb[0], bb[6+0]);
-					bb[1] = MIN2(cost[i].bb[1], bb[6+1]);
-					bb[2] = MIN2(cost[i].bb[2], bb[6+2]);
-
-					bb[3] = MAX2(cost[i].bb[3], bb[6+3]);
-					bb[4] = MAX2(cost[i].bb[4], bb[6+4]);
-					bb[5] = MAX2(cost[i].bb[5], bb[6+5]);
+					sweep[i].bb[0] = MIN2(obj[i]->bb[0], sweep[i+1].bb[0]);
+					sweep[i].bb[1] = MIN2(obj[i]->bb[1], sweep[i+1].bb[1]);
+					sweep[i].bb[2] = MIN2(obj[i]->bb[2], sweep[i+1].bb[2]);					
+					sweep[i].bb[3] = MAX2(obj[i]->bb[3], sweep[i+1].bb[3]);
+					sweep[i].bb[4] = MAX2(obj[i]->bb[4], sweep[i+1].bb[4]);
+					sweep[i].bb[5] = MAX2(obj[i]->bb[5], sweep[i+1].bb[5]);
+					sweep[i].cost  = obj[i]->cost + sweep[i+1].cost;
 				}
+//				right_cost += obj[i]->cost;
 			}
 			
-			INIT_MINMAX(other_bb, other_bb+3);
-			DO_MIN( cost[0].bb,   other_bb   );
-			DO_MAX( cost[0].bb+3, other_bb+3 );
-			left_cost  = cost[0].cost;
-			right_cost = childrens_cost-cost[0].cost;
-			if(right_cost < 0) right_cost = 0;
-
-			for(i=1; i<size; i++)
+			sweep_left.bb[0] = obj[0]->bb[0];
+			sweep_left.bb[1] = obj[0]->bb[1];
+			sweep_left.bb[2] = obj[0]->bb[2];
+			sweep_left.bb[3] = obj[0]->bb[3];
+			sweep_left.bb[4] = obj[0]->bb[4];
+			sweep_left.bb[5] = obj[0]->bb[5];
+			sweep_left.cost  = obj[0]->cost;
+			
+//			right_cost -= obj[0]->cost;	if(right_cost < 0) right_cost = 0;
+
+			for(int i=1; i<size; i++)
 			{
 				//Worst case heuristic (cost of each child is linear)
 				float hcost, left_side, right_side;
 				
-				left_side = bb_area(other_bb, other_bb+3)*(left_cost+logf(i));
-				right_side= bb_area(acc_bb+i*6, acc_bb+i*6+3)*(right_cost+logf(i));
+				left_side = bb_area(sweep_left.bb, sweep_left.bb+3)*(sweep_left.cost+logf(i));
+				right_side= bb_area(sweep[i].bb, sweep[i].bb+3)*(sweep[i].cost+logf(size-i));
+				hcost = left_side+right_side;
+
+				assert(left_side > 0);
+				assert(right_side > 0);
 				
 				if(left_side > bcost) break;	//No way we can find a better heuristic in this axis
 
-				hcost = left_side+right_side;
 				assert(hcost >= 0);
 				if( hcost < bcost
 				|| (hcost == bcost && axis < baxis)) //this makes sure the tree built is the same whatever is the order of the sorting axis
@@ -311,142 +348,51 @@ int rtbuild_heuristic_object_split(RTBuilder *b, int nchilds)
 					baxis = axis;
 					boffset = i;
 				}
-				DO_MIN( cost[i].bb,   other_bb   );
-				DO_MAX( cost[i].bb+3, other_bb+3 );
-				left_cost  += cost[i].cost;
-				right_cost -= cost[i].cost;
-				if(right_cost < 0.0f) right_cost = 0.0;
-			}
-			
-			if(baxis == axis)
-			{
-				for(i=0; i<size; i++)
-					b->begin[i] = cost[i].obj;
-				b->child_sorted_axis = axis;
+				DO_MIN( obj[i]->bb,   sweep_left.bb   );
+				DO_MAX( obj[i]->bb+3, sweep_left.bb+3 );
+
+				sweep_left.cost += obj[i]->cost;
+//				right_cost -= obj[i]->cost; if(right_cost < 0) right_cost = 0;
 			}
 			
 			assert(baxis >= 0 && baxis < 3);
 		}
 			
-		b->child_offset[0] = 0;
-		b->child_offset[1] = boffset;
-		b->child_offset[2] = size;
 		
-		MEM_freeN(acc_bb);
-		MEM_freeN(cost);
-		return nchilds;		
+		MEM_freeN(sweep);
 	}
-}
-
-/*
- * Helper code
- * PARTITION code / used on mean-split
- * basicly this a std::nth_element (like on C++ STL algorithm)
- */
-static void sort_swap(RTBuilder *b, int i, int j)
-{
-	SWAP(RayObject*, b->begin[i], b->begin[j]);
-}
- 
-static float sort_get_value(RTBuilder *b, int i)
-{
-	float min[3], max[3];
-	INIT_MINMAX(min, max);
-	RE_rayobject_merge_bb(b->begin[i], min, max);
-	return max[b->split_axis];
-}
- 
-static int medianof3(RTBuilder *d, int a, int b, int c)
-{
-	float fa = sort_get_value( d, a );
-	float fb = sort_get_value( d, b );
-	float fc = sort_get_value( d, c );
-
-	if(fb < fa)
+	else if(size == 2)
 	{
-		if(fc < fb)
-			return b;
-		else
-		{
-			if(fc < fa)
-				return c;
-			else
-				return a;
-		}
+		baxis = 0;
+		boffset = 1;
 	}
-	else
+	else if(size == 1)
 	{
-		if(fc < fb)
-		{
-			if(fc < fa)
-				return a;
-			else
-				return c;
-		}
-		else
-			return b;
+		b->child_offset[0] = 0;
+		b->child_offset[1] = 1;
+		return 1;
 	}
-}
-
-static void insertionsort(RTBuilder *b, int lo, int hi)
-{
-	int i;
-	for(i=lo; i<hi; i++)
-	{
-		RayObject *t = b->begin[i];
-		float tv= sort_get_value(b, i);
-		int j=i;
 		
-		while( j != lo && tv < sort_get_value(b, j-1))
-		{
-			b->begin[j] = b->begin[j-1];
-			j--;
-		}
-		b->begin[j] = t;
-	}
-}
-
-static int partition(RTBuilder *b, int lo, int mid, int hi)
-{
-	float x = sort_get_value( b, mid );
+	b->child_offset[0] = 0;
+	b->child_offset[1] = boffset;
+	b->child_offset[2] = size;
 	
-	int i=lo, j=hi;
-	while (1)
-	{
-		while(sort_get_value(b,i) < x) i++;
-		j--;
-		while(x < sort_get_value(b,j)) j--;
-		if(!(i < j))
-			return i;
-
-		sort_swap(b, i, j);
-		i++;
-	}
-}
 
-//
-// PARTITION code / used on mean-split
-// basicly this is an adapted std::nth_element (C++ STL <algorithm>)
-//
-// after a call to this function you can expect one of:
-//      every node to left of a[n] are smaller or equal to it
-//      every node to the right of a[n] are greater or equal to it
-static int partition_nth_element(RTBuilder *b, int _begin, int n, int _end)
-{
-	int begin = _begin, end = _end, cut;
-	while(end-begin > 3)
-	{
-		cut = partition(b, begin, medianof3(b, begin, begin+(end-begin)/2, end-1), end);
-		if(cut <= n)
-			begin = cut;
-		else
-			end = cut;
-	}
-	insertionsort(b, begin, end);
+	/* Adjust sorted arrays for childs */
+	for(int i=0; i<boffset; i++) b->sorted_begin[baxis][i]->selected = true;
+	for(int i=boffset; i<size; i++) b->sorted_begin[baxis][i]->selected = false;
+	for(int i=0; i<3; i++)
+		std::stable_partition( b->sorted_begin[i], b->sorted_end[i], selected_node );
 
-	return n;
+	return nchilds;		
 }
 
+/*
+ * Helper code
+ * PARTITION code / used on mean-split
+ * basicly this a std::nth_element (like on C++ STL algorithm)
+ */
+/*
 static void split_leafs(RTBuilder *b, int *nth, int partitions, int split_axis)
 {
 	int i;
@@ -456,23 +402,12 @@ static void split_leafs(RTBuilder *b, int *nth, int partitions, int split_axis)
 	{
 		assert(nth[i] < nth[i+1] && nth[i+1] < nth[partitions]);
 
-		partition_nth_element(b, nth[i],  nth[i+1], nth[partitions] );
-	}
-}
-
-static int split_leafs_by_plane(RTBuilder *b, int begin, int end, float plane)
-{
-	int i;
-	for(i = begin; i != end; i++)
-	{
-		if(sort_get_value(b, i) < plane)
-		{
-			sort_swap(b, i, begin);
-			begin++;
-		}
+		if(split_axis == 0)	std::nth_element(b, nth[i],  nth[i+1], nth[partitions], obj_bb_compare<RTBuilder::Object,0>);
+		if(split_axis == 1)	std::nth_element(b, nth[i],  nth[i+1], nth[partitions], obj_bb_compare<RTBuilder::Object,1>);
+		if(split_axis == 2)	std::nth_element(b, nth[i],  nth[i+1], nth[partitions], obj_bb_compare<RTBuilder::Object,2>);
 	}
-	return begin;
 }
+*/
 
 /*
  * Bounding Box utils
diff --git a/source/blender/render/intern/raytrace/rayobject_rtbuild.h b/source/blender/render/intern/raytrace/rayobject_rtbuild.h
new file mode 100644
index 00000000000..8f471f095e2
--- /dev/null
+++ b/source/blender/render/intern/raytrace/rayobject_rtbuild.h
@@ -0,0 +1,121 @@
+/**
+ * $Id$
+ *
+ * ***** BEGIN GPL LICENSE BLOCK *****
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version. 
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software Foundation,
+ * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+ *
+ * The Original Code is Copyright (C) 2009 Blender Foundation.
+ * All rights reserved.
+ *
+ * The Original Code is: all of this file.
+ *
+ * Contributor(s): André Pinto.
+ *
+ * ***** END GPL LICENSE BLOCK *****
+ */
+#ifndef RE_RAYOBJECT_RTBUILD_H
+#define RE_RAYOBJECT_RTBUILD_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "rayobject.h"
+
+
+/*
+ * Ray Tree Builder
+ *	this structs helps building any type of tree
+ *	it contains several methods to organiza/split nodes
+ *	allowing to create a given tree on the fly.
+ *
+ * Idea is that other trees BVH, BIH can use this code to
+ * generate with simple calls, and then convert to the theirs
+ * specific structure on the fly.
+ */
+#define RTBUILD_MAX_CHILDS 32
+
+
+typedef struct RTBuilder
+{
+	struct Object
+	{
+		RayObject *obj;
+		float cost;
+		float bb[6];
+		int selected;
+	};
+	
+	/* list to all primitives added in this tree */
+	struct
+	{
+		Object *begin, *end;
+		int maxsize;
+	} primitives;
+	
+	/* sorted list of rayobjects */
+	struct Object **sorted_begin[3], **sorted_end[3];
+
+	/* axis used (if any) on the split method */
+	int split_axis;
+	
+	/* child partitions calculated during splitting */
+	int child_offset[RTBUILD_MAX_CHILDS+1];
+	
+//	int child_sorted_axis; /* -1 if not sorted */
+	
+	float bb[6];
+
+} RTBuilder;
+
+/* used during creation */
+RTBuilder* rtbuild_create(int size);
+void rtbuild_free(RTBuilder *b);
+void rtbuild_add(RTBuilder *b, RayObject *o);
+void rtbuild_done(RTBuilder *b);
+void rtbuild_merge_bb(RTBuilder *b, float *min, float *max);
+int rtbuild_size(RTBuilder *b);
+
+RayObject* rtbuild_get_primitive(RTBuilder *b, int offset);
+
+/* used during tree reorganization */
+RTBuilder* rtbuild_get_child(RTBuilder *b, int child, RTBuilder *tmp);
+
+/* Calculates child partitions and returns number of efectively needed partitions */
+int rtbuild_get_largest_axis(RTBuilder *b);
+
+//Object partition
+int rtbuild_mean_split(RTBuilder *b, int nchilds, int axis);
+int rtbuild_mean_split_largest_axis(RTBuilder *b, int nchilds);
+
+int rtbuild_heuristic_object_split(RTBuilder *b, int nchilds);
+
+//Space partition
+int rtbuild_median_split(RTBuilder *b, float *separators, int nchilds, int axis);
+int rtbuild_median_split_largest_axis(RTBuilder *b, int nchilds);
+
+
+/* bb utils */
+float bb_area(float *min, float *max);
+float bb_volume(float *min, float *max);
+int bb_largest_axis(float *min, float *max);
+int bb_fits_inside(float *outer_min, float *outer_max, float *inner_min, float *inner_max); /* only returns 0 if merging inner and outerbox would create a box larger than outer box */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/source/blender/render/intern/raytrace/rayobject_vbvh.cpp b/source/blender/render/intern/raytrace/rayobject_vbvh.cpp
index 17ed63e3669..2dde485d3d1 100644
--- a/source/blender/render/intern/raytrace/rayobject_vbvh.cpp
+++ b/source/blender/render/intern/raytrace/rayobject_vbvh.cpp
@@ -235,7 +235,7 @@ Node *bvh_rearrange(Tree *tree, Builder *builder)
 		Node *node = bvh_new_node(tree);
 		INIT_MINMAX(node->bb, node->bb+3);
 		rtbuild_merge_bb(builder, node->bb, node->bb+3);		
-		node->child = (BVHNode*)builder->begin[0];
+		node->child = (BVHNode*) rtbuild_get_primitive( builder, 0 );
 		return node;
 	}
 	else
@@ -266,6 +266,8 @@ Node *bvh_rearrange(Tree *tree, Builder *builder)
 template<>
 void bvh_done<BVHTree>(BVHTree *obj)
 {
+	rtbuild_done(obj->builder);
+	
 	int needed_nodes = (rtbuild_size(obj->builder)+1)*2;
 	if(needed_nodes > BLI_MEMARENA_STD_BUFSIZE)
 		needed_nodes = BLI_MEMARENA_STD_BUFSIZE;