add feature filtering for collision between spheres and meshes

include/bounce/collision/gjk/gjk_cache.h

@@ -46,18 +46,10 @@ b3GJKOutput b3GJK(const b3Transform& xfA, const b3GJKProxy& proxyA,
 // with the closest points.
 struct b3GJKFeaturePair
 {
-	enum Type
-	{
-		e_unknown = 0,
-		e_vertex = 1,
-		e_edge = 2,
-		e_face = 3
-	};
-
-	Type typeA; // number of vertices on proxy A
-	Type typeB; // number of vertices on proxy B
 	u32 indexA[3]; // vertices on proxy A
+	u32 countA; // number of vertices on proxy A
 	u32 indexB[3]; // vertices on proxy B
+	u32 countB; // number of vertices on proxy B
 };
 
 // Identify the vertices of the features that the closest points between two 

include/bounce/dynamics/contacts/mesh_contact.h

@@ -58,6 +58,8 @@ private:
 
 	void Collide();
 	
+	void CollideSphere();
+
 	void SynchronizeShapes();
 
 	bool MoveAABB(const b3AABB3& aabb, const b3Vec3& displacement);

include/testbed/tests/distance_test.h

@@ -54,13 +54,13 @@ public:
 		{
 			b3GJKFeaturePair featurePair = b3GetFeaturePair(m_cache);
 
-			for (u32 i = 0; i < (u32)featurePair.typeA; ++i)
+			for (u32 i = 0; i < featurePair.countA; ++i)
 			{
 				u32 index = featurePair.indexA[i];
 				g_debugDraw->DrawPoint(m_xfA * m_proxyA.GetVertex(index), b3Color(1.0f, 1.0f, 0.0f));
 			}
 
-			for (u32 i = 0; i < (u32)featurePair.typeB; ++i)
+			for (u32 i = 0; i < featurePair.countB; ++i)
 			{
 				u32 index = featurePair.indexB[i];
 				g_debugDraw->DrawPoint(m_xfB * m_proxyB.GetVertex(index), b3Color(1.0f, 1.0f, 0.0f));
@@ -133,4 +133,4 @@ public:
 	b3SimplexCache m_cache;
 };
 
-#endif
+#endif

src/bounce/collision/gjk/gjk_feature_pair.cpp

@@ -30,8 +30,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 	{
 		// VV 
 		b3GJKFeaturePair pair;
-		pair.typeA = b3GJKFeaturePair::e_vertex;
-		pair.typeB = b3GJKFeaturePair::e_vertex;
+		pair.countA = 1;
+		pair.countB = 1;
 		pair.indexA[0] = cache.indexA[0];
 		pair.indexB[0] = cache.indexB[0];
 		return pair;
@@ -43,8 +43,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// EE
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_edge;
-			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.countA = 2;
+			pair.countB = 2;
 			pair.indexA[0] = cache.indexA[0];
 			pair.indexA[1] = cache.indexA[1];
 			pair.indexB[0] = cache.indexB[0];
@@ -58,8 +58,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// VE
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_vertex;
-			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.countA = 1;
+			pair.countB = 2;
 			pair.indexA[0] = cache.indexA[0];
 			pair.indexB[0] = cache.indexB[0];
 			pair.indexB[1] = cache.indexB[1];
@@ -71,8 +71,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// EV
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_edge;
-			pair.typeB = b3GJKFeaturePair::e_vertex;
+			pair.countA = 2;
+			pair.countB = 1;
 			pair.indexA[0] = cache.indexA[0];
 			pair.indexA[1] = cache.indexA[1];
 			pair.indexB[0] = cache.indexB[0];
@@ -88,8 +88,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// VF
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_vertex;
-			pair.typeB = b3GJKFeaturePair::e_face;
+			pair.countA = 1;
+			pair.countB = 3;
 			pair.indexA[0] = cache.indexA[0];
 			pair.indexB[0] = cache.indexB[0];
 			pair.indexB[1] = cache.indexB[1];
@@ -104,8 +104,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// FV
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_face;
-			pair.typeB = b3GJKFeaturePair::e_vertex;
+			pair.countA = 3;
+			pair.countB = 1;
 			pair.indexA[0] = cache.indexA[0];
 			pair.indexA[1] = cache.indexA[1];
 			pair.indexA[2] = cache.indexA[2];
@@ -120,8 +120,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// EE
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_edge;
-			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.countA = 2;
+			pair.countB = 2;
 			pair.indexA[0] = cache.indexA[0];
 			pair.indexB[0] = cache.indexB[0];
 
@@ -151,8 +151,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// EF
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_edge;
-			pair.typeB = b3GJKFeaturePair::e_face;
+			pair.countA = 2;
+			pair.countB = 3;
 
 			pair.indexA[0] = cache.indexA[0];
 			if (cache.indexA[0] == cache.indexA[1])
@@ -179,8 +179,8 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 		{
 			// FE
 			b3GJKFeaturePair pair;
-			pair.typeA = b3GJKFeaturePair::e_face;
-			pair.typeB = b3GJKFeaturePair::e_edge;
+			pair.countA = 3;
+			pair.countB = 2;
 			pair.indexA[0] = cache.indexA[0];
 			pair.indexA[1] = cache.indexA[1];
 			pair.indexA[2] = cache.indexA[2];
@@ -208,7 +208,7 @@ b3GJKFeaturePair b3GetFeaturePair(const b3SimplexCache& cache)
 	B3_ASSERT(false);
 
 	b3GJKFeaturePair pair;
-	pair.typeA = b3GJKFeaturePair::e_unknown;
-	pair.typeB = b3GJKFeaturePair::e_unknown;
+	pair.countA = 0;
+	pair.countB = 0;
 	return pair;
 }

src/bounce/dynamics/contacts/collide/collide_sphere_mesh.cpp

@@ -0,0 +1,393 @@
+/*
+* Copyright (c) 2016-2016 Irlan Robson http://www.irlan.net
+*
+* This software is provided 'as-is', without any express or implied
+* warranty.  In no event will the authors be held liable for any damages
+* arising from the use of this software.
+* Permission is granted to anyone to use this software for any purpose,
+* including commercial applications, and to alter it and redistribute it
+* freely, subject to the following restrictions:
+* 1. The origin of this software must not be misrepresented; you must not
+* claim that you wrote the original software. If you use this software
+* in a product, an acknowledgment in the product documentation would be
+* appreciated but is not required.
+* 2. Altered source versions must be plainly marked as such, and must not be
+* misrepresented as being the original software.
+* 3. This notice may not be RemoveTriangled or altered from any source distribution.
+*/
+
+#include <bounce/dynamics/contacts/mesh_contact.h>
+#include <bounce/dynamics/shapes/sphere_shape.h>
+#include <bounce/dynamics/shapes/mesh_shape.h>
+#include <bounce/dynamics/contacts/contact_cluster.h>
+#include <bounce/dynamics/world.h>
+#include <bounce/dynamics/body.h>
+#include <bounce/collision/shapes/mesh.h>
+#include <bounce/collision/gjk/gjk_cache.h>
+#include <bounce/common/memory/stack_allocator.h>
+
+// Implementation based on Peter Tchernev's relevant quote 
+// in the Bullet forum.
+// The mesh must contain unique vertices for this collision
+// filtering algorithm to work correctly.
+// Otherwise we'll need to switch to comparisons 
+// using the binary representation of each vertex coordinate, 
+// which will be slight slower.
+// Therefore it is recommended for your resource compiler 
+// to enforce this restriction on each physics mesh.
+
+struct b3FeaturePoint
+{
+	u32 index; 
+	b3GJKOutput output;
+	b3GJKFeaturePair pair;
+};
+
+// This is used for fast sorting.
+struct b3SortKey
+{
+	bool operator<(const b3SortKey& b) const
+	{
+		return distance < b.distance;
+	}
+
+	u32 index; // original index to contact point
+	float32 distance; // distance between closest points
+};
+
+struct b3SMCollider
+{
+	b3SMCollider(b3StackAllocator* allocator,
+		b3TriangleCache* triangles, u32 triangleCount,
+		const b3Transform& xfA, const b3SphereShape* sA,
+		const b3Transform& xfB, const b3MeshShape* sB);
+
+	~b3SMCollider();
+
+	u32 Collide(b3Manifold manifolds[3]);
+	void CollideTriangle(u32 i);
+	void Filter();
+
+	bool ShouldCollide(u32 v) const;
+	bool ShouldCollide(u32 v1, u32 v2) const;
+	bool ShouldCollide(const b3FeaturePoint* p) const;
+
+	void AddPoint(u32 triangle, const b3GJKOutput& output, const b3GJKFeaturePair& pair);
+	void AddDelayedPoint(u32 triangle, const b3GJKOutput& output, const b3GJKFeaturePair& pair);
+	void AddValidPoint(u32 triangle, const b3GJKOutput& output, const b3GJKFeaturePair& pair);
+	void RemoveTriangle(const b3Triangle* triangle);
+
+	b3StackAllocator* m_alloc;
+
+	b3ShapeGJKProxy m_proxyA;
+	b3Vec3 m_localVertexA;
+	b3Vec3 m_vertexA;
+	float32 m_rA;
+	b3Transform m_xfA;
+
+	b3ShapeGJKProxy m_proxyB;
+	const b3MeshShape* m_meshB;
+	float32 m_rB;
+	b3Transform m_xfB;
+
+	float32 m_totalRadius;
+
+	b3TriangleCache* m_triangles;
+	u32 m_triangleCount;
+
+	b3Manifold* m_manifolds;
+	u32 m_manifoldCount;
+
+	// Triangles that cannot collide anymore.
+	b3Triangle* m_removeds;
+	u32 m_removedCount;
+
+	// Vertex or edge contact points that can collide.
+	b3FeaturePoint* m_delayeds;
+	b3SortKey* m_keys;
+	u32 m_delayedCount;
+};
+
+b3SMCollider::b3SMCollider(b3StackAllocator* allocator,
+	b3TriangleCache* triangles, u32 triangleCount,
+	const b3Transform& xfA, const b3SphereShape* sA,
+	const b3Transform& xfB, const b3MeshShape* sB)
+{
+	m_alloc = allocator;
+	m_triangles = triangles;
+	m_triangleCount = triangleCount;
+
+	m_xfA = xfA;
+	m_xfB = xfB;
+
+	m_proxyA.Set(sA, 0);
+	m_localVertexA = sA->m_center;
+	m_vertexA = b3Mul(m_xfA, m_localVertexA);
+	m_rA = sA->m_radius;
+
+	m_meshB = sB;
+	m_rB = sB->m_radius;
+
+	m_totalRadius = m_rA + m_rB;
+
+	m_delayeds = (b3FeaturePoint*)m_alloc->Allocate(m_triangleCount * sizeof(b3FeaturePoint));
+	m_keys = (b3SortKey*)m_alloc->Allocate(m_triangleCount * sizeof(b3SortKey));
+	m_delayedCount = 0;
+
+	m_removeds = (b3Triangle*)m_alloc->Allocate(m_triangleCount * sizeof(b3Triangle));
+	m_removedCount = 0;
+
+	m_manifolds = (b3Manifold*)m_alloc->Allocate(m_triangleCount * sizeof(b3Manifold));
+	m_manifoldCount = 0;
+}
+
+b3SMCollider::~b3SMCollider()
+{
+	m_alloc->Free(m_manifolds);
+	m_alloc->Free(m_removeds);
+	m_alloc->Free(m_keys);
+	m_alloc->Free(m_delayeds);
+}
+
+u32 b3SMCollider::Collide(b3Manifold manifolds[3])
+{
+	for (u32 i = 0; i < m_triangleCount; ++i)
+	{
+		CollideTriangle(i);
+	}
+
+	Filter();
+
+	u32 numOut = b3Clusterize(manifolds, m_manifolds, m_manifoldCount, m_xfA, m_rA, m_xfB, m_rB);
+	return numOut;
+}
+
+void b3SMCollider::CollideTriangle(u32 i)
+{
+	b3TriangleCache* t = m_triangles + i;
+	
+	// GJK
+	b3ShapeGJKProxy proxyB(m_meshB, t->index);
+	b3GJKOutput output = b3GJK(m_xfA, m_proxyA, m_xfB, proxyB, false, &t->cache.simplexCache);
+
+	if (output.distance > m_totalRadius)
+	{
+		return;
+	}
+
+	if (output.distance > B3_EPSILON)
+	{
+		b3GJKFeaturePair pair = b3GetFeaturePair(t->cache.simplexCache);
+		AddPoint(i, output, pair);
+		return;
+	}
+
+	// SAT
+	b3Triangle* triangle = m_meshB->m_mesh->triangles + t->index;
+	b3Vec3 A = b3Mul(m_xfB, m_meshB->m_mesh->vertices[triangle->v1]);
+	b3Vec3 B = b3Mul(m_xfB, m_meshB->m_mesh->vertices[triangle->v2]);
+	b3Vec3 C = b3Mul(m_xfB, m_meshB->m_mesh->vertices[triangle->v3]);
+
+	b3Plane plane(A, B, C);
+	float32 separation = b3Distance(m_vertexA, plane);
+	float32 sign = b3Sign(separation);
+
+	if (sign * separation > m_totalRadius)
+	{
+		return;
+	}
+
+	b3Vec3 normal = sign * plane.normal;
+
+	B3_ASSERT(m_manifoldCount < m_triangleCount);
+	b3Manifold* m = m_manifolds + m_manifoldCount;
+	m->GuessImpulses();
+	++m_manifoldCount;
+	
+	b3Vec3 p1 = m_vertexA;
+	b3Vec3 p2 = b3ClosestPointOnPlane(p1, plane);
+
+	// Ensure normal orientation to shape B.
+	normal = -normal;
+
+	m->pointCount = 1;
+	m->points[0].triangleKey = t->index;
+	m->points[0].key = 0;
+	m->points[0].localNormal = b3MulT(m_xfA.rotation, normal);
+	m->points[0].localPoint = m_localVertexA;
+	m->points[0].localPoint2 = p2;
+
+	m->center = 0.5f * (p1 + m_rA * normal + p2 - m_rB * normal);
+	m->normal = normal;
+	m->tangent1 = b3Perp(normal);
+	m->tangent2 = b3Cross(m->tangent1, normal);
+
+	RemoveTriangle(triangle);
+}
+
+void b3SMCollider::Filter()
+{
+	// Sort contact points according to distance to triangles.
+	std::sort(m_keys, m_keys + m_delayedCount);
+
+	for (u32 i = 0; i < m_delayedCount; ++i)
+	{
+		const b3SortKey* key = m_keys + i;
+		const b3FeaturePoint* p = m_delayeds + key->index;
+		const b3TriangleCache* tc = m_triangles + p->index;
+		const b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
+
+		bool ok = ShouldCollide(p);
+		if (ok)
+		{
+			AddValidPoint(p->index, p->output, p->pair);
+		}
+
+		// Now this triangle cannot collide anymore.
+		RemoveTriangle(t);
+	}
+}
+
+void b3SMCollider::RemoveTriangle(const b3Triangle* triangle)
+{
+	B3_ASSERT(m_removedCount < m_triangleCount);
+	m_removeds[m_removedCount] = *triangle;
+	++m_removedCount;
+}
+
+bool b3SMCollider::ShouldCollide(u32 v) const
+{
+	for (u32 i = 0; i < m_removedCount; ++i)
+	{
+		if (m_removeds[i].TestVertex(v))
+		{
+			return false;
+		}
+	}
+	return true;
+}
+
+bool b3SMCollider::ShouldCollide(u32 v1, u32 v2) const
+{
+	for (u32 i = 0; i < m_removedCount; ++i)
+	{
+		if (m_removeds[i].TestEdge(v1, v2))
+		{
+			return false;
+		}
+	}
+	
+	return true;
+}
+
+bool b3SMCollider::ShouldCollide(const b3FeaturePoint* p) const
+{
+	b3TriangleCache* tc = m_triangles + p->index;
+	b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
+	u32 is[3] = { t->v1, t->v2, t->v3 };
+
+	switch (p->pair.countB)
+	{
+	case 1:
+	{
+		u32 v1 = is[p->pair.indexB[0]];
+		return ShouldCollide(v1);
+	}
+	case 2:
+	{
+		u32 v1 = is[p->pair.indexB[0]];
+		u32 v2 = is[p->pair.indexB[1]];
+		return ShouldCollide(v1, v2);
+	}
+	case 3:
+	{
+		return true;
+	}
+	default:
+	{
+		B3_ASSERT(false);
+		return true;
+	}
+	}
+
+	return false;
+}
+
+void b3SMCollider::AddPoint(u32 i, const b3GJKOutput& output, const b3GJKFeaturePair& pair)
+{
+	if (pair.countB == 3)
+	{
+		b3TriangleCache* tc = m_triangles + i;
+		b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
+		
+		AddValidPoint(i, output, pair);
+		RemoveTriangle(t);
+	}
+	else
+	{
+		AddDelayedPoint(i, output, pair);
+	}
+}
+
+void b3SMCollider::AddDelayedPoint(u32 i, const b3GJKOutput& output, const b3GJKFeaturePair& pair)
+{
+	b3TriangleCache* tc = m_triangles + i;
+	b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
+
+	B3_ASSERT(m_delayedCount < m_triangleCount);
+	b3FeaturePoint* p = m_delayeds + m_delayedCount;
+	b3SortKey* k = m_keys + m_delayedCount;
+
+	p->index = i;
+	p->output = output;
+	p->pair = pair;
+	k->index = m_delayedCount;
+	k->distance = output.distance;
+
+	++m_delayedCount;
+}
+
+void b3SMCollider::AddValidPoint(u32 i, const b3GJKOutput& output, const b3GJKFeaturePair& pair)
+{
+	b3TriangleCache* tc = m_triangles + i;
+	b3Triangle* t = m_meshB->m_mesh->triangles + tc->index;
+	
+	B3_ASSERT(m_manifoldCount < m_triangleCount);
+	b3Manifold* m = m_manifolds + m_manifoldCount;
+	++m_manifoldCount;
+	
+	m->GuessImpulses();
+	
+	b3Vec3 p1 = output.pointA;
+	b3Vec3 p2 = output.pointB;
+	b3Vec3 n = (p2 - p1) / output.distance;
+
+	m->pointCount = 1;
+	m->points[0].triangleKey = tc->index;
+	m->points[0].key = 0;
+	m->points[0].localNormal = b3MulT(m_xfA.rotation, n);
+	m->points[0].localPoint = m_localVertexA;
+	m->points[0].localPoint2 = b3MulT(m_xfB, p2);
+
+	m->center = 0.5f * (p1 + m_rA * n + p2 - m_rB * n);
+	m->normal = n;
+	m->tangent1 = b3Perp(n);
+	m->tangent2 = b3Cross(m->tangent1, n);
+}
+
+void b3MeshContact::CollideSphere()
+{
+	b3SphereShape* sA = (b3SphereShape*)GetShapeA();
+	b3Body* bodyA = sA->GetBody();
+	b3Transform xfA = bodyA->GetTransform();
+
+	b3MeshShape* sB = (b3MeshShape*)GetShapeB();
+	b3Body* bodyB = sB->GetBody();
+	b3Transform xfB = bodyB->GetTransform();
+
+	b3World* world = bodyA->GetWorld();
+	b3StackAllocator* allocator = &world->m_stackAllocator;
+
+	b3SMCollider collider(allocator, m_triangles, m_triangleCount, xfA, sA, xfB, sB);
+	m_manifoldCount = collider.Collide(m_stackManifolds);
+}

src/bounce/dynamics/contacts/mesh_contact.cpp

@@ -231,6 +231,17 @@ void b3MeshContact::Collide()
 	b3MeshShape* meshShapeB = (b3MeshShape*)shapeB;
 	b3Transform xfB = bodyB->GetTransform();
 
+	// Remove this conditional inclusion if collisions 
+	// between spheres and the internal features of meshes 
+	// should be avoided.
+#if 0
+	if (shapeA->GetType() == e_sphereShape)
+	{
+		CollideSphere();
+		return;
+	}
+#endif
+
 	b3World* world = bodyA->GetWorld();
 	b3StackAllocator* allocator = &world->m_stackAllocator;
 

src/testbed/framework/test.cpp

@@ -101,74 +101,6 @@ Test::Test()
 		m_thinHull.SetTransform(xf);
 	}
 
-	{
-		const u32 w = 100;
-		const u32 h = 100;
-
-		b3Vec3 t;
-		t.x = -0.5f * float32(w);
-		t.y = 0.0f;
-		t.z = -0.5f * float32(h);
-
-		b3Mesh* mesh = m_meshes + e_gridMesh;
-
-		mesh->vertexCount = w * h;
-		mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3));
-
-		for (u32 i = 0; i < w; ++i)
-		{
-			for (u32 j = 0; j < h; ++j)
-			{
-				u32 v1 = i * w + j;
-
-				b3Vec3 v;
-				v.x = float32(i);
-				v.y = 0.0f;
-				v.z = float32(j);
-
-				v += t;
-
-				mesh->vertices[v1] = v;
-			}
-		}
-
-		// 2 triangles per quad
-		mesh->triangleCount = 2 * (w - 1) * (h - 1);
-		mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle));
-
-		u32 triangleCount = 0;
-		for (u32 i = 0; i < w - 1; ++i)
-		{
-			for (u32 j = 0; j < h - 1; ++j)
-			{
-				u32 v1 = i * w + j;
-				u32 v2 = (i + 1) * w + j;
-				u32 v3 = (i + 1) * w + (j + 1);
-				u32 v4 = i * w + (j + 1);
-
-				B3_ASSERT(triangleCount < mesh->triangleCount);
-				b3Triangle* t1 = mesh->triangles + triangleCount;
-				++triangleCount;
-
-				t1->v1 = v3;
-				t1->v2 = v2;
-				t1->v3 = v1;
-
-				B3_ASSERT(triangleCount < mesh->triangleCount);
-				b3Triangle* t2 = mesh->triangles + triangleCount;
-				++triangleCount;
-
-				t2->v1 = v1;
-				t2->v2 = v4;
-				t2->v3 = v3;
-			}
-		}
-
-		B3_ASSERT(triangleCount == mesh->triangleCount);
-
-		mesh->BuildTree();
-	}
-
 	{
 		const u32 w = 5;
 		const u32 h = 5;
@@ -253,8 +185,76 @@ Test::Test()
 	}
 
 	{
-		const u32 w = 100;
-		const u32 h = 100;
+		const u32 w = 50;
+		const u32 h = 50;
+
+		b3Vec3 t;
+		t.x = -0.5f * float32(w);
+		t.y = 0.0f;
+		t.z = -0.5f * float32(h);
+
+		b3Mesh* mesh = m_meshes + e_gridMesh;
+
+		mesh->vertexCount = w * h;
+		mesh->vertices = (b3Vec3*)b3Alloc(mesh->vertexCount * sizeof(b3Vec3));
+
+		for (u32 i = 0; i < w; ++i)
+		{
+			for (u32 j = 0; j < h; ++j)
+			{
+				u32 v1 = i * w + j;
+
+				b3Vec3 v;
+				v.x = float32(i);
+				v.y = 0.0f;
+				v.z = float32(j);
+
+				v += t;
+
+				mesh->vertices[v1] = v;
+			}
+		}
+
+		// 2 triangles per quad
+		mesh->triangleCount = 2 * (w - 1) * (h - 1);
+		mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle));
+
+		u32 triangleCount = 0;
+		for (u32 i = 0; i < w - 1; ++i)
+		{
+			for (u32 j = 0; j < h - 1; ++j)
+			{
+				u32 v1 = i * w + j;
+				u32 v2 = (i + 1) * w + j;
+				u32 v3 = (i + 1) * w + (j + 1);
+				u32 v4 = i * w + (j + 1);
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t1 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t1->v1 = v3;
+				t1->v2 = v2;
+				t1->v3 = v1;
+
+				B3_ASSERT(triangleCount < mesh->triangleCount);
+				b3Triangle* t2 = mesh->triangles + triangleCount;
+				++triangleCount;
+
+				t2->v1 = v1;
+				t2->v2 = v4;
+				t2->v3 = v3;
+			}
+		}
+
+		B3_ASSERT(triangleCount == mesh->triangleCount);
+
+		mesh->BuildTree();
+	}
+
+	{
+		const u32 w = 50;
+		const u32 h = 50;
 
 		b3Vec3 t;
 		t.x = -0.5f * float32(w);
@@ -273,9 +273,9 @@ Test::Test()
 				u32 v1 = i * w + j;
 				
 				b3Vec3 v;
-				v.x = float32(i);
+				v.x = 2.0f * float32(i);
 				v.y = RandomFloat(0.0f, 0.5f);
-				v.z = float32(j);
+				v.z = 2.0f *float32(j);
 
 				v += t;
 
@@ -283,7 +283,6 @@ Test::Test()
 			}
 		}
 
-		// 2 triangles per quad
 		mesh->triangleCount = 2 * (w - 1) * (h - 1);
 		mesh->triangles = (b3Triangle*)b3Alloc(mesh->triangleCount * sizeof(b3Triangle));