# HG changeset patch
# User Radek Brich <radek.brich@devl.cz>
# Date 1209282289 -7200
# Node ID 6f7fe14782c24ce626dbf472a2f640736ad80aed
# Parent  e3a2a5b26abb40a637bc180ad4e57146ce6b9e0a
prepare kd-tree traversal for packet tracing (4 rays at once)
RayPacket and VectorPacket structures for packet tracing
remove some redundant const's

diff -r e3a2a5b26abb -r 6f7fe14782c2 SConstruct
--- a/SConstruct	Thu Apr 24 18:12:32 2008 +0200
+++ b/SConstruct	Sun Apr 27 09:44:49 2008 +0200
@@ -150,6 +150,8 @@
 SConscript('demos/SConscript', exports='pymodule')
 env.Alias('demos', ['cc-demos', 'python-demos'])
 
+SConscript('tests/SConscript', build_dir='build/tests', duplicate=0, exports='env lib')
+
 SConscript('models/SConscript')
 
 env.Alias('docs', Command('docs/html', [], 'doxygen'))
diff -r e3a2a5b26abb -r 6f7fe14782c2 ccdemos/SConscript
--- a/ccdemos/SConscript	Thu Apr 24 18:12:32 2008 +0200
+++ b/ccdemos/SConscript	Sun Apr 27 09:44:49 2008 +0200
@@ -1,12 +1,13 @@
 Import('env lib')
-env.Append(CPPPATH = ['.','#include'], LIBPATH='#build/lib')
-env.Prepend(LIBS=['pyrit','png'])
+myenv = env.Clone()
+myenv.Append(CPPPATH = ['.','#include'], LIBPATH='#build/lib')
+myenv.Prepend(LIBS=['pyrit','png'])
 
-sdlenv = env.Clone()
+sdlenv = myenv.Clone()
 sdlenv.ParseConfig('sh sdl-config --cflags --libs')
 
 l = []
-image_obj = env.Object('image.c', CC="$CXX")
+image_obj = myenv.Object('image.c', CC="$CXX")
 l.append( image_obj )
 l.append( sdlenv.Program(['realtime.cc']) )
 l.append( sdlenv.Program(['realtime_bunny.cc']) )
@@ -14,4 +15,4 @@
 l.append( sdlenv.Program(['spheres_shadow.cc']+image_obj) )
 l.append( sdlenv.Program(['textures.cc']+image_obj) )
 
-env.Alias('cc-demos', l)
+myenv.Alias('cc-demos', l)
diff -r e3a2a5b26abb -r 6f7fe14782c2 include/container.h
--- a/include/container.h	Thu Apr 24 18:12:32 2008 +0200
+++ b/include/container.h	Sun Apr 27 09:44:49 2008 +0200
@@ -48,7 +48,7 @@
 	//void addShapeNoExtend(Shape* aShape) { shapes.push_back(aShape); };
 	virtual Shape *nearest_intersection(const Shape *origin_shape, const Ray &ray,
 		Float &nearest_distance);
-	virtual void packet_intersection(const Shape **origin_shapes, const Ray *rays,
+	virtual void packet_intersection(const Shape **origin_shapes, const RayPacket &rays,
 		Float *nearest_distances, Shape **nearest_shapes);
 
 	virtual void optimize() {};
diff -r e3a2a5b26abb -r 6f7fe14782c2 include/kdtree.h
--- a/include/kdtree.h	Thu Apr 24 18:12:32 2008 +0200
+++ b/include/kdtree.h	Sun Apr 27 09:44:49 2008 +0200
@@ -53,10 +53,10 @@
 	~KdNode();
 
 	void setLeaf() { flags |= 3; };
-	const bool isLeaf() const { return (flags & 3) == 3; };
+	bool isLeaf() const { return (flags & 3) == 3; };
 
 	void setAxis(int aAxis) { flags &= ~3; flags |= aAxis; };
-	const int getAxis() const { return flags & 3; };
+	int getAxis() const { return flags & 3; };
 
 	void setSplit(Float aSplit) { split = aSplit; };
 	const Float& getSplit() const { return split; };
@@ -86,9 +86,11 @@
 	void addShape(Shape* aShape) { Container::addShape(aShape); built = false; };
 	Shape *nearest_intersection(const Shape *origin_shape, const Ray &ray,
 		Float &nearest_distance);
+	void packet_intersection(const Shape **origin_shapes, const RayPacket &rays,
+		Float *nearest_distances, Shape **nearest_shapes);
 	void optimize() { build(); };
 	void build();
-	const bool isBuilt() const { return built; };
+	bool isBuilt() const { return built; };
 	KdNode *getRootNode() const { return root; };
 	void setMaxDepth(int md) { max_depth = md; };
 
diff -r e3a2a5b26abb -r 6f7fe14782c2 include/raytracer.h
--- a/include/raytracer.h	Thu Apr 24 18:12:32 2008 +0200
+++ b/include/raytracer.h	Sun Apr 27 09:44:49 2008 +0200
@@ -67,9 +67,9 @@
 	pthread_mutex_t sample_queue_mutex, sampler_mutex;
 	pthread_cond_t sample_queue_cond, worker_ready_cond;
 
-	Colour shader_evalulate(Ray &ray, int depth, Shape *origin_shape,
+	Colour shader_evalulate(const Ray &ray, int depth, Shape *origin_shape,
 		Float nearest_distance, Shape *nearest_shape);
-	void raytracePacket(Ray *rays, Colour *results);
+	void raytracePacket(RayPacket &rays, Colour *results);
 	static void *raytrace_worker(void *d);
 public:
 	Raytracer(): top(NULL), camera(NULL), lights(), bg_colour(0., 0., 0.),
diff -r e3a2a5b26abb -r 6f7fe14782c2 include/scene.h
--- a/include/scene.h	Thu Apr 24 18:12:32 2008 +0200
+++ b/include/scene.h	Sun Apr 27 09:44:49 2008 +0200
@@ -48,6 +48,23 @@
 };
 
 /**
+ * packet of 4 rays
+ */
+class RayPacket
+{
+public:
+	VectorPacket o, dir;
+
+	// index operator - get a ray
+	Ray operator[](int i) const
+	{
+		return Ray(
+			Vector3(o.x[i], o.y[i], o.z[i]),
+			Vector3(dir.x[i], dir.y[i], dir.z[i]));
+	};
+};
+
+/**
  * a camera
  */
 class Camera
@@ -78,65 +95,55 @@
 		return Ray(eye, dir);
 	};
 
-	void makeRayPacket(Sample *samples, Ray *rays)
+	void makeRayPacket(Sample *samples, RayPacket &rays)
 	{
 		__m128 m1x,m1y,m1z;
 		__m128 m2x,m2y,m2z;
 		__m128 m;
-		
+
 		// m1(xyz) = u * samples[i].x
-		m1x = _mm_set1_ps(u.x);
-		m1y = _mm_set1_ps(u.y);
-		m1z = _mm_set1_ps(u.z);
-		m = _mm_set_ps(samples[0].x, samples[1].x, samples[2].x, samples[3].x);
+		m1x = _mm_set_ps1(u.x);
+		m1y = _mm_set_ps1(u.y);
+		m1z = _mm_set_ps1(u.z);
+		m = _mm_set_ps(samples[3].x, samples[2].x, samples[1].x, samples[0].x);
 		m1x = _mm_mul_ps(m1x, m);
 		m1y = _mm_mul_ps(m1y, m);
 		m1z = _mm_mul_ps(m1z, m);
-		
+
 		// m2(xyz) = v * samples[i].y
-		m2x = _mm_set1_ps(v.x);
-		m2y = _mm_set1_ps(v.y);
-		m2z = _mm_set1_ps(v.z);
-		m = _mm_set_ps(samples[0].y, samples[1].y, samples[2].y, samples[3].y);
+		m2x = _mm_set_ps1(v.x);
+		m2y = _mm_set_ps1(v.y);
+		m2z = _mm_set_ps1(v.z);
+		m = _mm_set_ps(samples[3].y, samples[2].y, samples[1].y, samples[0].y);
 		m2x = _mm_mul_ps(m2x, m);
 		m2y = _mm_mul_ps(m2y, m);
 		m2z = _mm_mul_ps(m2z, m);
-		
+
 		// m1(xyz) = (m1 + m2) = (u*samples[i].x + v*samples[i].y)
 		m1x = _mm_add_ps(m1x, m2x);
 		m1y = _mm_add_ps(m1y, m2y);
 		m1z = _mm_add_ps(m1z, m2z);
-		
+
 		// m1(xyz) = m1*F = (u*samples[i].x + v*samples[i].y)*F
-		m = _mm_set_ps(F,F,F,F);
+		m = _mm_set_ps1(F);
 		m1x = _mm_mul_ps(m1x, m);
 		m1y = _mm_mul_ps(m1y, m);
 		m1z = _mm_mul_ps(m1z, m);
-		
+
 		// m1(xyz) = p - m1 = p - (u*samples[i].x + v*samples[i].y)*F = dir
-		m2x = _mm_set1_ps(p.x);
-		m2y = _mm_set1_ps(p.y);
-		m2z = _mm_set1_ps(p.z);
-		m2x = _mm_sub_ps(m2x, m1x);
-		m2y = _mm_sub_ps(m2y, m1y);
-		m2z = _mm_sub_ps(m2z, m1z);
-		
-		// normalize dir
-		m1x = _mm_mul_ps(m2x, m2x); // x*x
-		m1y = _mm_mul_ps(m2y, m2y); // y*y
-		m1z = _mm_mul_ps(m2z, m2z); // z*z
-		m = _mm_add_ps(m1x, m1y);   // x*x + y*y
-		m = _mm_add_ps(m, m1z);     // m = x*x + y*y + z*z
-		m = _mm_sqrt_ps(m);         // m = sqrt(m)
-		m2x = _mm_div_ps(m2x, m);   // dir(xyz) /= m
-		m2y = _mm_div_ps(m2y, m);
-		m2z = _mm_div_ps(m2z, m);
-		
-		for (int i = 0; i < 4; i++)
-		{
-			Vector3 dir(((float*)&m2x)[3-i], ((float*)&m2y)[3-i], ((float*)&m2z)[3-i]);
-			rays[i] = Ray(eye, dir);
-		}
+		m2x = _mm_set_ps1(p.x);
+		m2y = _mm_set_ps1(p.y);
+		m2z = _mm_set_ps1(p.z);
+		rays.dir.mx = _mm_sub_ps(m2x, m1x);
+		rays.dir.my = _mm_sub_ps(m2y, m1y);
+		rays.dir.mz = _mm_sub_ps(m2z, m1z);
+
+		// copy origin
+		rays.o.mx = _mm_set_ps1(eye.x);
+		rays.o.my = _mm_set_ps1(eye.y);
+		rays.o.mz = _mm_set_ps1(eye.z);
+
+		rays.dir.normalize();
 	};
 };
 
@@ -171,6 +178,13 @@
 	Float h() { return H.y-L.y; };
 	Float d() { return H.z-L.z; };
 	bool intersect(const Ray &ray, Float &a, Float &b);
+	bool intersect_packet(const RayPacket &rays, __m128 &a, __m128 &b)
+	{
+		return intersect(rays[0], ((float*)&a)[0], ((float*)&b)[0])
+			|| intersect(rays[1], ((float*)&a)[1], ((float*)&b)[1])
+			|| intersect(rays[2], ((float*)&a)[2], ((float*)&b)[2])
+			|| intersect(rays[3], ((float*)&a)[3], ((float*)&b)[3]);
+	};
 };
 
 #endif
diff -r e3a2a5b26abb -r 6f7fe14782c2 include/shapes.h
--- a/include/shapes.h	Thu Apr 24 18:12:32 2008 +0200
+++ b/include/shapes.h	Sun Apr 27 09:44:49 2008 +0200
@@ -55,6 +55,14 @@
 	// first intersection point
 	virtual bool intersect(const Ray &ray, Float &dist) const = 0;
 
+	virtual void intersect_packet(const RayPacket &rays, __m128 &dists, bool *results)
+	{
+		results[0] = intersect(rays[0], ((float*)&dists)[0]);
+		results[1] = intersect(rays[1], ((float*)&dists)[1]);
+		results[2] = intersect(rays[2], ((float*)&dists)[2]);
+		results[3] = intersect(rays[3], ((float*)&dists)[3]);
+	};
+
 	// all intersections (only for CSG)
 	virtual bool intersect_all(const Ray &ray, Float dist, vector<Float> &allts) const = 0;
 
@@ -95,7 +103,7 @@
 	const Vector3 normal(const Vector3 &P) const { return (P - center) * inv_radius; };
 	BBox get_bbox() const;
 	const Vector3 getCenter() const { return center; };
-	const Float getRadius() const { return radius; };
+	Float getRadius() const { return radius; };
 	ostream & dump(ostream &st) const;
 };
 
diff -r e3a2a5b26abb -r 6f7fe14782c2 include/vector.h
--- a/include/vector.h	Thu Apr 24 18:12:32 2008 +0200
+++ b/include/vector.h	Sun Apr 27 09:44:49 2008 +0200
@@ -191,4 +191,91 @@
 
 typedef Vector3 Colour;
 
+
+static const __m128 zeros = _mm_set_ps1(0.);
+static const __m128 ones = _mm_set_ps1(1.);
+static const __m128 mEps = _mm_set_ps1(Eps);
+
+class VectorPacket
+{
+public:
+	union {
+		__m128 ma[3];
+		struct {
+			__m128 mx;
+			__m128 my;
+			__m128 mz;
+		};
+		struct {
+			float x[4];
+			float y[4];
+			float z[4];
+		};
+	};
+
+	VectorPacket() {};
+	VectorPacket(__m128 ax, __m128 ay, __m128 az):
+		mx(ax), my(ay), mz(az) {};
+
+	Vector3 getVector(int i) const
+	{
+		return Vector3(x[i], y[i], z[i]);
+	};
+
+	void normalize()
+	{
+		__m128 m,x,y,z;
+		x = _mm_mul_ps(mx, mx); // x*x
+		y = _mm_mul_ps(my, my); // y*y
+		z = _mm_mul_ps(mz, mz); // z*z
+		m = _mm_add_ps(x, y);
+		m = _mm_add_ps(m, z);     // x*x + y*y + z*z
+		m = _mm_sqrt_ps(m);
+		m = _mm_div_ps(ones, m);   // m = 1/sqrt(m)
+		mx = _mm_mul_ps(mx, m);
+		my = _mm_mul_ps(my, m);
+		mz = _mm_mul_ps(mz, m);
+	};
+
+	friend VectorPacket operator+(const VectorPacket &a, const VectorPacket &b)
+	{
+		return VectorPacket(
+			_mm_add_ps(a.mx, b.mx),
+			_mm_add_ps(a.my, b.my),
+			_mm_add_ps(a.mz, b.mz));
+	};
+
+	friend VectorPacket operator-(const VectorPacket &a, const VectorPacket &b)
+	{
+		return VectorPacket(
+			_mm_sub_ps(a.mx, b.mx),
+			_mm_sub_ps(a.my, b.my),
+			_mm_sub_ps(a.mz, b.mz));
+	};
+
+	friend VectorPacket operator*(const VectorPacket &v,  const __m128 &m)
+	{
+		return VectorPacket(
+			_mm_mul_ps(v.mx, m),
+			_mm_mul_ps(v.my, m),
+			_mm_mul_ps(v.mz, m));
+	};
+
+	friend VectorPacket operator/(const __m128 &m, const VectorPacket &v)
+	{
+		return VectorPacket(
+			_mm_div_ps(m, v.mx),
+			_mm_div_ps(m, v.my),
+			_mm_div_ps(m, v.mz));
+	};
+
+	// write to character stream
+	friend ostream & operator<<(ostream &st, const VectorPacket &v)
+	{
+		return st << "[" << v.getVector(0) << "," << v.getVector(1) 
+			<< "," << v.getVector(2) << "," << v.getVector(3) << ")";
+	};
+
+};
+
 #endif
diff -r e3a2a5b26abb -r 6f7fe14782c2 src/container.cc
--- a/src/container.cc	Thu Apr 24 18:12:32 2008 +0200
+++ b/src/container.cc	Sun Apr 27 09:44:49 2008 +0200
@@ -60,7 +60,7 @@
 	return nearest_shape;
 }
 
-void Container::packet_intersection(const Shape **origin_shapes, const Ray *rays,
+void Container::packet_intersection(const Shape **origin_shapes, const RayPacket &rays,
 	Float *nearest_distances, Shape **nearest_shapes)
 {
 	for (int i = 0; i < 4; i++)
diff -r e3a2a5b26abb -r 6f7fe14782c2 src/kdtree.cc
--- a/src/kdtree.cc	Thu Apr 24 18:12:32 2008 +0200
+++ b/src/kdtree.cc	Sun Apr 27 09:44:49 2008 +0200
@@ -256,7 +256,7 @@
 					node = node->getLeftChild();
 					continue;
 				}
-				if (stack[exit].pb[axis] == splitVal)
+				if (stack[entry].pb[axis] == splitVal)
 				{ /* case Z1 */
 					node = node->getRightChild();
 					continue;
@@ -328,6 +328,219 @@
 	return NULL;
 }
 
+// stack element for kd-tree traversal (packet version)
+struct StackElem4
+{
+	KdNode* node; /* pointer to far child */
+	__m128 t; /* the entry/exit signed distance */
+	VectorPacket pb; /* the coordinates of entry/exit point */
+	int prev;
+};
+
+void KdTree::packet_intersection(const Shape **origin_shapes, const RayPacket &rays,
+		Float *nearest_distances, Shape **nearest_shapes)
+{
+	__m128 a, b; /* entry/exit signed distance */
+	__m128 t;    /* signed distance to the splitting plane */
+	__m128 mask = zeros;
+
+	/* if we have no tree, fall back to naive test */
+	if (!built)
+		Container::packet_intersection(origin_shapes, rays, nearest_distances, nearest_shapes);
+
+	nearest_shapes[0] = NULL;
+	nearest_shapes[1] = NULL;
+	nearest_shapes[2] = NULL;
+	nearest_shapes[3] = NULL;
+
+	//bbox.intersect_packet(rays, a, b)
+	if (bbox.intersect(rays[0], ((float*)&a)[0], ((float*)&b)[0]))
+		((int*)&mask)[0] = -1;
+	if (bbox.intersect(rays[1], ((float*)&a)[1], ((float*)&b)[1]))
+		((int*)&mask)[1] = -1;
+	if (bbox.intersect(rays[2], ((float*)&a)[2], ((float*)&b)[2]))
+		((int*)&mask)[2] = -1;
+	if (bbox.intersect(rays[3], ((float*)&a)[3], ((float*)&b)[3]))
+		((int*)&mask)[3] = -1;
+
+	if (!_mm_movemask_ps(mask))
+		return;
+
+	/* pointers to the far child node and current node */
+	KdNode *farchild, *node;
+	node = root;
+
+	StackElem4 stack[max_depth];
+
+	int entry = 0, exit = 1;
+	stack[entry].t = a;
+
+	/* distinguish between internal and external origin of a ray*/
+	stack[entry].pb = rays.o + rays.dir * a; /* external */
+	for (int i = 0; i < 4; i++)
+		if (((float*)&a)[i] < 0.0)
+		{
+			/* internal */
+			stack[entry].pb.x[i] = rays.o.x[i];
+			stack[entry].pb.y[i] = rays.o.y[i];
+			stack[entry].pb.z[i] = rays.o.z[i];
+		}
+
+	/* setup initial exit point in the stack */
+	stack[exit].t = b;
+	stack[exit].pb = rays.o + rays.dir * b;
+	stack[exit].node = NULL;
+
+	/* loop, traverse through the whole kd-tree,
+	until an object is intersected or ray leaves the scene */
+	__m128 splitVal;
+	int axis;
+	static const int mod3[] = {0,1,2,0,1};
+	const VectorPacket invdirs = ones / rays.dir;
+	while (node)
+	{
+		/* loop until a leaf is found */
+		while (!node->isLeaf())
+		{
+			/* retrieve position of splitting plane */
+			splitVal = _mm_set_ps1(node->getSplit());
+			axis = node->getAxis();
+
+			// mask out invalid rays with near > far
+			__m128 curmask = _mm_and_ps(mask, _mm_cmple_ps(stack[entry].t, stack[exit].t));
+			__m128 entry_lt = _mm_cmplt_ps(stack[entry].pb.ma[axis], splitVal);
+			__m128 entry_gt = _mm_cmpgt_ps(stack[entry].pb.ma[axis], splitVal);
+			__m128 exit_lt = _mm_cmplt_ps(stack[exit].pb.ma[axis], splitVal);
+			__m128 exit_gt = _mm_cmpgt_ps(stack[exit].pb.ma[axis], splitVal);
+
+			// if all of
+			// stack[entry].pb[axis] <= splitVal,
+			// stack[exit].pb[axis] <= splitVal
+			if (!_mm_movemask_ps(
+				_mm_and_ps(_mm_or_ps(entry_gt, exit_gt), curmask)))
+			{
+				node = node->getLeftChild();
+				continue;
+			}
+
+			// if all of
+			// stack[entry].pb[axis] >= splitVal,
+			// stack[exit].pb[axis] >= splitVal
+			if (!_mm_movemask_ps(
+				_mm_and_ps(_mm_or_ps(entry_lt, exit_lt), curmask)))
+			{
+				node = node->getRightChild();
+				continue;
+			}
+
+			// any of
+			// stack[entry].pb[axis] < splitVal,
+			// stack[exit].pb[axis] > splitVal
+			bool cond1 = _mm_movemask_ps(
+				_mm_and_ps(_mm_and_ps(entry_lt, exit_gt), curmask));
+
+			// any of
+			// stack[entry].pb[axis] > splitVal,
+			// stack[exit].pb[axis] < splitVal
+			bool cond2 = _mm_movemask_ps(
+				_mm_and_ps(_mm_and_ps(entry_gt, exit_lt), curmask));
+
+			if ((!cond1 && !cond2) || (cond1 && cond2))
+			{
+				// fall back to single rays
+				// FIXME: split rays and continue
+				for (int i = 0; i < 4; i++)
+					if(!nearest_shapes[i])
+						nearest_shapes[i] = nearest_intersection(origin_shapes[i],
+							rays[i], nearest_distances[i]);
+				return;
+			}
+
+			if (cond1)
+			{
+				farchild = node->getRightChild();
+				node = node->getLeftChild();
+			}
+			else
+			{
+				farchild = node->getLeftChild();
+				node = node->getRightChild();
+			}
+
+			/* traverse both children */
+
+			/* signed distance to the splitting plane */
+			t = _mm_mul_ps(_mm_sub_ps(splitVal, rays.o.ma[axis]), invdirs.ma[axis]);
+
+			/* setup the new exit point and push it onto stack */
+			register int tmp = exit;
+
+			exit++;
+			if (exit == entry)
+				exit++;
+			assert(exit < max_depth);
+
+			stack[exit].prev = tmp;
+			stack[exit].t = t;
+			stack[exit].node = farchild;
+			stack[exit].pb.ma[axis] = splitVal;
+			stack[exit].pb.ma[mod3[axis+1]] =
+				_mm_add_ps(rays.o.ma[mod3[axis+1]], _mm_mul_ps(t, rays.dir.ma[mod3[axis+1]]));
+			stack[exit].pb.ma[mod3[axis+2]] =
+				_mm_add_ps(rays.o.ma[mod3[axis+2]], _mm_mul_ps(t, rays.dir.ma[mod3[axis+2]]));
+		}
+
+		/* current node is the leaf . . . empty or full */
+		__m128 dists = stack[exit].t;
+		ShapeList::iterator shape;
+		for (shape = node->getShapes()->begin(); shape != node->getShapes()->end(); shape++)
+		{
+			for (int i = 0; i < 4; i++)
+				if ( ((_mm_movemask_ps(mask)>>(i))&1) &&
+				((float*)&stack[entry].t)[i] < ((float*)&stack[exit].t)[i] &&
+				*shape != origin_shapes[i] &&
+				(*shape)->intersect(rays[i], ((float*)&dists)[i])
+				&& ((float*)&dists)[i] >= ((float*)&stack[entry].t)[i] - Eps)
+				{
+					nearest_shapes[i] = *shape;
+					nearest_distances[i] = ((float*)&dists)[i];
+				}
+
+			/*
+			bool results[4];
+			(*shape)->intersect_packet(rays, dists, results);
+			int greater_than_entry = _mm_movemask_ps(
+				_mm_and_ps(_mm_cmpge_ps(dists, _mm_sub_ps(stack[entry].t, mEps)), mask));
+			for (int i = 0; i < 4; i++)
+			{
+				if (results[i] //&& *shape != origin_shapes[i]
+				&& ((greater_than_entry>>(3-i))&1))
+				{
+					nearest_shapes[i] = *shape;
+					nearest_distances[i] = ((float*)&dists)[i];
+				}
+			}
+			*/
+		}
+
+		for (int i = 0; i < 4; i++)
+			if (nearest_shapes[i])
+				((int*)&mask)[i] = 0;
+
+		if (!_mm_movemask_ps(mask))
+			return;
+
+		entry = exit;
+
+		/* retrieve the pointer to the next node,
+		it is possible that ray traversal terminates */
+		node = stack[entry].node;
+		exit = stack[entry].prev;
+	}
+
+	/* ray leaves the scene */
+}
+
 ostream & operator<<(ostream &st, KdNode &node)
 {
 	if (node.isLeaf())
diff -r e3a2a5b26abb -r 6f7fe14782c2 src/raytracer.cc
--- a/src/raytracer.cc	Thu Apr 24 18:12:32 2008 +0200
+++ b/src/raytracer.cc	Sun Apr 27 09:44:49 2008 +0200
@@ -74,7 +74,7 @@
 
 // calculate shader function
 // P is point of intersection, N normal in this point
-Colour PhongShader_ambient(Material &mat, Vector3 &P)
+Colour PhongShader_ambient(const Material &mat, const Vector3 &P)
 {
 	Colour col;
 	if (mat.texture)
@@ -92,8 +92,9 @@
  R direction of reflected ray,
  V direction to the viewer
 */
-Colour PhongShader_calculate(Material &mat, Vector3 &P, Vector3 &N, Vector3 &R, Vector3 &V,
-	Light &light)
+Colour PhongShader_calculate(const Material &mat,
+	const Vector3 &P, const Vector3 &N, const Vector3 &R, const Vector3 &V,
+	const Light &light)
 {
 	Colour I = Colour();
 	Vector3 L = light.pos - P;
@@ -116,7 +117,7 @@
 	return I;
 }
 
-Colour Raytracer::shader_evalulate(Ray &ray, int depth, Shape *origin_shape,
+Colour Raytracer::shader_evalulate(const Ray &ray, int depth, Shape *origin_shape,
 	Float nearest_distance, Shape *nearest_shape)
 {
 	Colour col = Colour();
@@ -236,21 +237,18 @@
 	Shape *nearest_shape = top->nearest_intersection(origin_shape, ray, nearest_distance);
 
 	if (nearest_shape == NULL)
-	{
 		return bg_colour;
-	}
 	else
-	{
 		return shader_evalulate(ray, depth, origin_shape, nearest_distance, nearest_shape);
-	}
 }
 
-void Raytracer::raytracePacket(Ray *rays, Colour *results)
+void Raytracer::raytracePacket(RayPacket &rays, Colour *results)
 {
 	Float nearest_distances[4] = {Inf,Inf,Inf,Inf};
 	Shape *nearest_shapes[4];
 	static const Shape *origin_shapes[4] = {NULL, NULL, NULL, NULL};
 	top->packet_intersection(origin_shapes, rays, nearest_distances, nearest_shapes);
+	Ray ray;
 
 	for (int i = 0; i < 4; i++)
 		if (nearest_shapes[i] == NULL)
@@ -267,7 +265,8 @@
 	Sample my_queue[my_queue_size];
 	Colour my_colours[my_queue_size];
 	int my_count;
-	Ray ray, rays[4];
+	Ray ray;
+	RayPacket rays;
 	const bool can_use_packets = (rt->use_packets && rt->sampler->packetableSamples());
 	for (;;)
 	{
diff -r e3a2a5b26abb -r 6f7fe14782c2 src/raytracermodule.cc
--- a/src/raytracermodule.cc	Thu Apr 24 18:12:32 2008 +0200
+++ b/src/raytracermodule.cc	Sun Apr 27 09:44:49 2008 +0200
@@ -28,7 +28,7 @@
 
 #include <vector>
 #include "raytracer.h"
-#include "octree.h"
+#include "kdtree.h"
 
 //=========================== Light Source Object ===========================
 
@@ -760,7 +760,7 @@
 	v->raytracer = new Raytracer();
 	v->children = new vector<PyObject*>();
 	v->raytracer->setCamera(new Camera());
-	v->raytracer->setTop(new Octree());
+	v->raytracer->setTop(new KdTree());
 
 	return (PyObject*)v;
 }
diff -r e3a2a5b26abb -r 6f7fe14782c2 tests/SConscript
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/tests/SConscript	Sun Apr 27 09:44:49 2008 +0200
@@ -0,0 +1,9 @@
+Import('env lib')
+myenv = env.Clone()
+myenv.Append(CPPPATH = ['.','#include'], LIBPATH='#build/lib')
+myenv.Prepend(LIBS=['pyrit'])
+
+l = []
+l.append( myenv.Program(['vector.cc']) )
+
+myenv.Alias('tests', l)
diff -r e3a2a5b26abb -r 6f7fe14782c2 tests/vector.cc
--- a/tests/vector.cc	Thu Apr 24 18:12:32 2008 +0200
+++ b/tests/vector.cc	Sun Apr 27 09:44:49 2008 +0200
@@ -1,12 +1,15 @@
+#include <xmmintrin.h>
 #include "vector.h"
 
 int main() {
+	{
+	/* Vector3 */
 	Vector3 a(1, 2, 3);
 	cout << "=== Vector3 test ===" << endl;
 	cout << "a = " << a << endl;
 	Vector3 b(2, 3, 2);
 	cout << "b = " << b << endl;
-	
+
 	cout << "a + b = " << a + b << endl;
 	cout << "b - a = " << b - a << endl;
 	cout << "dot(a,b) = " << dot(a,b) << endl;
@@ -16,8 +19,28 @@
 	cout << "-a = " << -a << endl;
 
 	cout << "a.mag() = " << a.mag() << endl;
-	cout << "a.unit() = " << a.unit() << endl;
-	cout << "a.unit().mag() = " << a.unit().mag() << endl;
+	cout << "normalize(a) = " << normalize(a) << endl;
+	cout << "normalize(a).mag() = " << normalize(a).mag() << endl;
+	}
+
+	{
+	/* VectorPacket */
+	VectorPacket a(_mm_set_ps(4,3,2,1), _mm_set_ps(8,7,6,5), _mm_set_ps(12,11,10,9));
+	VectorPacket b(_mm_set_ps(41,31,21,11), _mm_set_ps(42,32,22,12), _mm_set_ps(43,33,23,13));
+	cout << "=== VectorPacket test ===" << endl;
+	cout << "a = " << a << endl;
+	cout << "b = " << b << endl;
+
+	cout << "a + b = " << a + b << endl;
+	cout << "b - a = " << b - a << endl;
+/*	cout << "dot(a,b) = " << dot(a,b) << endl;
+	cout << "cross(a,b) = " << cross(a,b) << endl;
+	cout << "a * 2 = " << a * 2 << endl;
+	cout << "3 * b = " << 3 * b << endl;
+	cout << "-a = " << -a << endl;
+
+	cout << "normalize(a) = " << normalize(a) << endl;*/
+	}
 
 	return 0;
 }