add raycast accelerator for btHeightfieldTerrainShape, thanks to Marc Zylann, see https://github.com/bulletphysics/bullet3/pull/2062

it can be disabled by setting the flag cb.m_flags |= btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator;
acceleration is disabled for z axis up.
add btHeightfieldTerrainShape example to example browser

12 files changed, 1702 insertions, 38 deletions

diff --git a/examples/Benchmarks/BenchmarkDemo.cpp b/examples/Benchmarks/BenchmarkDemo.cpp
index 110a35eab..995275916 100644
--- a/examples/Benchmarks/BenchmarkDemo.cpp
+++ b/examples/Benchmarks/BenchmarkDemo.cpp
@@ -313,35 +313,6 @@ public:
 			m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0], lineColor, points.size(), sizeof(btVector3FloatData), &indices[0], indices.size(), 1);
 		}
 
-#if 0
-		glDisable (GL_LIGHTING);
-		glColor3f (0.0, 1.0, 0.0);
-		glBegin (GL_LINES);
-		int i;
-
-		for (i = 0; i < NUMRAYS; i++)
-		{
-			glVertex3f (source[i][0], source[i][1], source[i][2]);
-			glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
-		}
-		glEnd ();
-		glColor3f (1.0, 1.0, 1.0);
-		glBegin (GL_LINES);
-		for (i = 0; i < NUMRAYS; i++)
-		{
-			glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
-			glVertex3f (hit[i][0] + normal[i][0], hit[i][1] + normal[i][1], hit[i][2] + normal[i][2]);
-		}
-		glEnd ();
-		glColor3f (0.0, 1.0, 1.0);
-		glBegin (GL_POINTS);
-		for ( i = 0; i < NUMRAYS; i++)
-		{
-			glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
-		}
-		glEnd ();
-		glEnable (GL_LIGHTING);
-#endif  //USE_GRAPHICAL_BENCHMARK
 	}
 };
 
diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h
index 09f8b57d3..fea34aec0 100644
--- a/examples/CommonInterfaces/CommonRigidBodyBase.h
+++ b/examples/CommonInterfaces/CommonRigidBodyBase.h
@@ -439,12 +439,15 @@ struct CommonRigidBodyBase : public CommonExampleInterface
 
 	virtual void renderScene()
 	{
+		if (m_dynamicsWorld)
 		{
-			m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
-		}
+			{
+				m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
+			}
 
-		{
-			m_guiHelper->render(m_dynamicsWorld);
+			{
+				m_guiHelper->render(m_dynamicsWorld);
+			}
 		}
 	}
 };
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index ceb6d9590..89531eb77 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -207,6 +207,8 @@ SET(BulletExampleBrowser_SRCS
 	../MultiThreadedDemo/MultiThreadedDemo.h
 	../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
 	../MultiThreadedDemo/CommonRigidBodyMTBase.h
+	../Heightfield/HeightfieldExample.cpp
+	../Heightfield/HeightfieldExample.h
 	../BlockSolver/btBlockSolver.cpp
 	../BlockSolver/btBlockSolver.h
 	../BlockSolver/BlockSolverExample.cpp
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index f2e3901b7..c21fa8618 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -5,6 +5,7 @@
 #include "../BlockSolver/RigidBodyBoxes.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "EmptyExample.h"
+#include "../Heightfield/HeightfieldExample.h"
 #include "../RenderingExamples/RenderInstancingDemo.h"
 #include "../RenderingExamples/CoordinateSystemDemo.h"
 #include "../RenderingExamples/RaytracerSetup.h"
@@ -157,8 +158,8 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS),
 		ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG),
 		ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK),
-		ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI),
-		ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_BLOCK),
+		//ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI),
+		//ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_BLOCK),
 
 		ExampleEntry(0, "Inverse Dynamics"),
 		ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF),
@@ -234,6 +235,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Convex vs Mesh", "Benchmark the performance and stability of rigid bodies using convex hull collision shapes (btConvexHullShape), resting on a triangle mesh, btBvhTriangleMeshShape.", BenchmarkCreateFunc, 6),
 		ExampleEntry(1, "Raycast", "Benchmark the performance of the btCollisionWorld::rayTest. Note that currently the rays are not rendered.", BenchmarkCreateFunc, 7),
 		ExampleEntry(1, "Convex Pack", "Benchmark the performance of the convex hull primitive.", BenchmarkCreateFunc, 8),
+		ExampleEntry(1, "Heightfield", "Raycast against a btHeightfieldTerrainShape", HeightfieldExampleCreateFunc),
 		//#endif
 
 		ExampleEntry(0, "Importers"),
diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
index bb8671d48..93369d8a0 100644
--- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp
+++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp
@@ -1,7 +1,7 @@
 #include "OpenGLGuiHelper.h"
 
 #include "btBulletDynamicsCommon.h"
-
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
 #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
 #include "../CommonInterfaces/CommonRenderInterface.h"
 #include "Bullet3Common/b3Scalar.h"
@@ -255,6 +255,38 @@ void OpenGLGuiHelper::createRigidBodyGraphicsObject(btRigidBody* body, const btV
 	createCollisionObjectGraphicsObject(body, color);
 }
 
+
+class MyTriangleCollector2 : public btTriangleCallback
+{
+public:
+	btAlignedObjectArray<GLInstanceVertex>* m_pVerticesOut;
+	btAlignedObjectArray<int>* m_pIndicesOut;
+
+	MyTriangleCollector2()
+	{
+		m_pVerticesOut = 0;
+		m_pIndicesOut = 0;
+	}
+
+	virtual void processTriangle(btVector3* tris, int partId, int triangleIndex)
+	{
+		for (int k = 0; k < 3; k++)
+		{
+			GLInstanceVertex v;
+			v.xyzw[3] = 0;
+			v.uv[0] = v.uv[1] = 0.5f;
+			btVector3 normal = (tris[0] - tris[1]).cross(tris[0] - tris[2]);
+			normal.safeNormalize();
+			for (int l = 0; l < 3; l++)
+			{
+				v.xyzw[l] = tris[k][l];
+				v.normal[l] = normal[l];
+			}
+			m_pIndicesOut->push_back(m_pVerticesOut->size());
+			m_pVerticesOut->push_back(v);
+		}
+	}
+};
 void OpenGLGuiHelper::createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color)
 {
 	if (body->getUserIndex() < 0)
@@ -409,6 +441,30 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
 	//if (collisionShape->getShapeType()==BOX_SHAPE_PROXYTYPE)
 	{
 	}
+
+
+	if (collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE)
+	{
+		const btHeightfieldTerrainShape* heightField = static_cast<const btHeightfieldTerrainShape*>(collisionShape);
+		MyTriangleCollector2  col;
+		col.m_pVerticesOut = &gfxVertices;
+		col.m_pIndicesOut = &indices;
+		btVector3 aabbMin, aabbMax;
+		for (int k = 0; k < 3; k++)
+		{
+			aabbMin[k] = -BT_LARGE_FLOAT;
+			aabbMax[k] = BT_LARGE_FLOAT;
+		}
+		heightField->processAllTriangles(&col, aabbMin, aabbMax);
+		if (gfxVertices.size() && indices.size())
+		{
+			int shapeId = m_data->m_glApp->m_renderer->registerShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size());
+			collisionShape->setUserIndex(shapeId);
+		}
+		return;
+	}
+
+
 	if (collisionShape->getShapeType() == SOFTBODY_SHAPE_PROXYTYPE)
 	{
 		computeSoftBodyVertices(collisionShape, gfxVertices, indices);
diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua
index 067eb2c64..d23c782e5 100644
--- a/examples/ExampleBrowser/premake4.lua
+++ b/examples/ExampleBrowser/premake4.lua
@@ -169,6 +169,7 @@ project "App_BulletExampleBrowser"
 		"../Collision/Internal/*",
 		"../Benchmarks/*",
 		"../MultiThreadedDemo/*",
+		"../Heightfield/HeightfieldExample.*",
 		"../CommonInterfaces/*.h",
 		"../ForkLift/ForkLiftDemo.*",
 		"../Importers/**",
diff --git a/examples/Heightfield/HeightfieldExample.cpp b/examples/Heightfield/HeightfieldExample.cpp
new file mode 100644
index 000000000..1f65d17fd
--- /dev/null
+++ b/examples/Heightfield/HeightfieldExample.cpp
@@ -0,0 +1,1116 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006,2008 Erwin Coumans  http://bulletphysics.org
+
+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 removed or altered from any source distribution.
+*/
+
+#include "HeightfieldExample.h"		// always include our own header first!
+
+#include "btBulletDynamicsCommon.h"
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../MultiThreadedDemo/CommonRigidBodyMTBase.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+
+// constants -------------------------------------------------------------------
+static const btScalar s_gravity = 9.8;		// 9.8 m/s^2
+
+static const int s_gridSize = 128 + 1;  // must be (2^N) + 1
+static const btScalar s_gridSpacing = 0.5;
+
+static const btScalar s_gridHeightScale = 0.02;
+
+// the singularity at the center of the radial model means we need a lot of
+//   finely-spaced time steps to get the physics right.
+// These numbers are probably too aggressive for a real game!
+static const int s_requestedHz = 180;
+static const float s_engineTimeStep = 1.0 / s_requestedHz;
+
+// delta phase: radians per second
+static const btScalar s_deltaPhase = 0.25 * 2.0 * SIMD_PI;
+
+// what type of terrain is generated?
+enum eTerrainModel {
+	eRadial = 0,	// deterministic
+	eFractal = 1	// random
+};
+
+
+typedef unsigned char byte_t;
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//
+//	static helper methods
+//
+//	Only used within this file (helpers and terrain generation, etc)
+//
+////////////////////////////////////////////////////////////////////////////////
+
+static const char *
+getTerrainTypeName
+(
+	eTerrainModel model
+)
+{
+	switch (model) {
+	case eRadial:
+		return "Radial";
+
+	case eFractal:
+		return "Fractal";
+
+	default:
+		btAssert(!"bad terrain model type");
+	}
+
+	return NULL;
+}
+
+
+
+static const char *
+getDataTypeName
+(
+	PHY_ScalarType type
+)
+{
+	switch (type) {
+	case PHY_UCHAR:
+		return "UnsignedChar";
+
+	case PHY_SHORT:
+		return "Short";
+
+	case PHY_FLOAT:
+		return "Float";
+
+	default:
+		btAssert(!"bad heightfield data type");
+	}
+
+	return NULL;
+}
+
+
+
+static const char *
+getUpAxisName
+(
+	int axis
+)
+{
+	switch (axis) {
+	case 0:
+		return "X";
+
+	case 1:
+		return "Y";
+
+	case 2:
+		return "Z";
+
+	default:
+		btAssert(!"bad up axis");
+	}
+
+	return NULL;
+}
+
+
+
+static btVector3
+getUpVector
+(
+	int upAxis,
+	btScalar regularValue,
+	btScalar upValue
+)
+{
+	btAssert(upAxis >= 0 && upAxis <= 2 && "bad up axis");
+
+	btVector3 v(regularValue, regularValue, regularValue);
+	v[upAxis] = upValue;
+
+	return v;
+}
+
+
+
+// TODO: it would probably cleaner to have a struct per data type, so
+// 	you could lookup byte sizes, conversion functions, etc.
+static int getByteSize
+(
+	PHY_ScalarType type
+)
+{
+	int size = 0;
+
+	switch (type) {
+	case PHY_FLOAT:
+		size = sizeof(btScalar);
+		break;
+
+	case PHY_UCHAR:
+		size = sizeof(unsigned char);
+		break;
+
+	case PHY_SHORT:
+		size = sizeof(short);
+		break;
+
+	default:
+		btAssert(!"Bad heightfield data type");
+	}
+
+	return size;
+}
+
+
+
+static btScalar
+convertToFloat
+(
+	const byte_t * p,
+	PHY_ScalarType type
+)
+{
+	btAssert(p);
+
+	switch (type) {
+	case PHY_FLOAT:
+	{
+		btScalar * pf = (btScalar *)p;
+		return *pf;
+	}
+
+	case PHY_UCHAR:
+	{
+		unsigned char * pu = (unsigned char *)p;
+		return ((*pu) * s_gridHeightScale);
+	}
+
+	case PHY_SHORT:
+	{
+		short * ps = (short *)p;
+		return ((*ps) * s_gridHeightScale);
+	}
+
+	default:
+		btAssert(!"bad type");
+	}
+
+	return 0;
+}
+
+
+
+static btScalar
+getGridHeight
+(
+	byte_t * grid,
+	int i,
+	int j,
+	PHY_ScalarType type
+)
+{
+	btAssert(grid);
+	btAssert(i >= 0 && i < s_gridSize);
+	btAssert(j >= 0 && j < s_gridSize);
+
+	int bpe = getByteSize(type);
+	btAssert(bpe > 0 && "bad bytes per element");
+
+	int idx = (j * s_gridSize) + i;
+	long offset = ((long)bpe) * idx;
+
+	byte_t * p = grid + offset;
+
+	return convertToFloat(p, type);
+}
+
+
+
+static void
+convertFromFloat
+(
+	byte_t * p,
+	btScalar value,
+	PHY_ScalarType type
+)
+{
+	btAssert(p && "null");
+
+	switch (type) {
+	case PHY_FLOAT:
+	{
+		btScalar * pf = (btScalar *)p;
+		*pf = value;
+	}
+	break;
+
+	case PHY_UCHAR:
+	{
+		unsigned char * pu = (unsigned char *)p;
+		*pu = (unsigned char)(value / s_gridHeightScale);
+	}
+	break;
+
+	case PHY_SHORT:
+	{
+		short * ps = (short *)p;
+		*ps = (short)(value / s_gridHeightScale);
+	}
+	break;
+
+	default:
+		btAssert(!"bad type");
+	}
+}
+
+
+
+// creates a radially-varying heightfield
+static void
+setRadial
+(
+	byte_t * grid,
+	int bytesPerElement,
+	PHY_ScalarType type,
+	btScalar phase = 0.0
+)
+{
+	btAssert(grid);
+	btAssert(bytesPerElement > 0);
+
+	// min/max
+	btScalar period = 0.5 / s_gridSpacing;
+	btScalar floor = 0.0;
+	btScalar min_r = 3.0 * btSqrt(s_gridSpacing);
+	btScalar magnitude = 5.0 * btSqrt(s_gridSpacing);
+
+	// pick a base_phase such that phase = 0 results in max height
+	//   (this way, if you create a heightfield with phase = 0,
+	//    you can rely on the min/max heights that result)
+	btScalar base_phase = (0.5 * SIMD_PI) - (period * min_r);
+	phase += base_phase;
+
+	// center of grid
+	btScalar cx = 0.5 * s_gridSize * s_gridSpacing;
+	btScalar cy = cx;		// assume square grid
+	byte_t * p = grid;
+	for (int i = 0; i < s_gridSize; ++i) {
+		float x = i * s_gridSpacing;
+		for (int j = 0; j < s_gridSize; ++j) {
+			float y = j * s_gridSpacing;
+
+			float dx = x - cx;
+			float dy = y - cy;
+
+			float r = sqrt((dx * dx) + (dy * dy));
+
+			float z = period;
+			if (r < min_r) {
+				r = min_r;
+			}
+			z = (1.0 / r) * sin(period * r + phase);
+			if (z > period) {
+				z = period;
+			}
+			else if (z < -period) {
+				z = -period;
+			}
+			z = floor + magnitude * z;
+
+			convertFromFloat(p, z, type);
+			p += bytesPerElement;
+		}
+	}
+}
+
+
+
+static float
+randomHeight
+(
+	int step
+)
+{
+	return (0.33 * s_gridSpacing * s_gridSize * step * (rand() - (0.5 * RAND_MAX))) / (1.0 * RAND_MAX * s_gridSize);
+}
+
+
+
+static void
+dumpGrid
+(
+	const byte_t * grid,
+	int bytesPerElement,
+	PHY_ScalarType type,
+	int max
+)
+{
+	//std::cerr << "Grid:\n";
+
+	char buffer[32];
+
+	for (int j = 0; j < max; ++j) {
+		for (int i = 0; i < max; ++i) {
+			long offset = j * s_gridSize + i;
+			float z = convertToFloat(grid + offset * bytesPerElement, type);
+			sprintf(buffer, "%6.2f", z);
+			//std::cerr << "  " << buffer;
+		}
+		//std::cerr << "\n";
+	}
+}
+
+
+
+static void
+updateHeight
+(
+	byte_t * p,
+	btScalar new_val,
+	PHY_ScalarType type
+)
+{
+	btScalar old_val = convertToFloat(p, type);
+	if (!old_val) {
+		convertFromFloat(p, new_val, type);
+	}
+}
+
+
+
+// creates a random, fractal heightfield
+static void
+setFractal
+(
+	byte_t * grid,
+	int bytesPerElement,
+	PHY_ScalarType type,
+	int step
+)
+{
+	btAssert(grid);
+	btAssert(bytesPerElement > 0);
+	btAssert(step > 0);
+	btAssert(step < s_gridSize);
+
+	int newStep = step / 2;
+	//	std::cerr << "Computing grid with step = " << step << ": before\n";
+	//	dumpGrid(grid, bytesPerElement, type, step + 1);
+
+	// special case: starting (must set four corners)
+	if (s_gridSize - 1 == step) {
+		// pick a non-zero (possibly negative) base elevation for testing
+		btScalar base = randomHeight(step / 2);
+
+		convertFromFloat(grid, base, type);
+		convertFromFloat(grid + step * bytesPerElement, base, type);
+		convertFromFloat(grid + step * s_gridSize * bytesPerElement, base, type);
+		convertFromFloat(grid + (step * s_gridSize + step) * bytesPerElement, base, type);
+	}
+
+	// determine elevation of each corner
+	btScalar c00 = convertToFloat(grid, type);
+	btScalar c01 = convertToFloat(grid + step * bytesPerElement, type);
+	btScalar c10 = convertToFloat(grid + (step * s_gridSize) * bytesPerElement, type);
+	btScalar c11 = convertToFloat(grid + (step * s_gridSize + step) * bytesPerElement, type);
+
+	// set top middle
+	updateHeight(grid + newStep * bytesPerElement, 0.5 * (c00 + c01) + randomHeight(step), type);
+
+	// set left middle
+	updateHeight(grid + (newStep * s_gridSize) * bytesPerElement, 0.5 * (c00 + c10) + randomHeight(step), type);
+
+	// set right middle
+	updateHeight(grid + (newStep * s_gridSize + step) * bytesPerElement, 0.5 * (c01 + c11) + randomHeight(step), type);
+
+	// set bottom middle
+	updateHeight(grid + (step * s_gridSize + newStep) * bytesPerElement, 0.5 * (c10 + c11) + randomHeight(step), type);
+
+	// set middle
+	updateHeight(grid + (newStep * s_gridSize + newStep) * bytesPerElement, 0.25 * (c00 + c01 + c10 + c11) + randomHeight(step), type);
+
+	//	std::cerr << "Computing grid with step = " << step << ": after\n";
+	//	dumpGrid(grid, bytesPerElement, type, step + 1);
+
+	// terminate?
+	if (newStep < 2) {
+		return;
+	}
+
+	// recurse
+	setFractal(grid, bytesPerElement, type, newStep);
+	setFractal(grid + newStep * bytesPerElement, bytesPerElement, type, newStep);
+	setFractal(grid + (newStep * s_gridSize) * bytesPerElement, bytesPerElement, type, newStep);
+	setFractal(grid + ((newStep * s_gridSize) + newStep) * bytesPerElement, bytesPerElement, type, newStep);
+}
+
+
+
+static byte_t *
+getRawHeightfieldData
+(
+	eTerrainModel model,
+	PHY_ScalarType type,
+	btScalar& minHeight,
+	btScalar& maxHeight
+)
+{
+	//	std::cerr << "\nRegenerating terrain\n";
+	//	std::cerr << "  model = " << model << "\n";
+	//	std::cerr << "  type = " << type << "\n";
+
+	long nElements = ((long)s_gridSize) * s_gridSize;
+	//	std::cerr << "  nElements = " << nElements << "\n";
+
+	int bytesPerElement = getByteSize(type);
+	//	std::cerr << "  bytesPerElement = " << bytesPerElement << "\n";
+	btAssert(bytesPerElement > 0 && "bad bytes per element");
+
+	long nBytes = nElements * bytesPerElement;
+	//	std::cerr << "  nBytes = " << nBytes << "\n";
+	byte_t * raw = new byte_t[nBytes];
+	btAssert(raw && "out of memory");
+
+	// reseed randomization every 30 seconds
+	//	srand(time(NULL) / 30);
+
+	// populate based on model
+	switch (model) {
+	case eRadial:
+		setRadial(raw, bytesPerElement, type);
+		break;
+
+	case eFractal:
+		for (int i = 0; i < nBytes; i++)
+		{
+			raw[i] = 0;
+		}
+		setFractal(raw, bytesPerElement, type, s_gridSize - 1);
+		break;
+
+	default:
+		btAssert(!"bad model type");
+	}
+
+	if (0) {
+		// inside if(0) so it keeps compiling but isn't
+		// 	exercised and doesn't cause warnings
+		//		std::cerr << "final grid:\n";
+		dumpGrid(raw, bytesPerElement, type, s_gridSize - 1);
+	}
+
+	// find min/max
+	for (int i = 0; i < s_gridSize; ++i) {
+		for (int j = 0; j < s_gridSize; ++j) {
+			btScalar z = getGridHeight(raw, i, j, type);
+			//			std::cerr << "i=" << i << ", j=" << j << ": z=" << z << "\n";
+
+			// update min/max
+			if (!i && !j) {
+				minHeight = z;
+				maxHeight = z;
+			}
+			else {
+				if (z < minHeight) {
+					minHeight = z;
+				}
+				if (z > maxHeight) {
+					maxHeight = z;
+				}
+			}
+		}
+	}
+
+	if (maxHeight < -minHeight) {
+		maxHeight = -minHeight;
+	}
+	if (minHeight > -maxHeight) {
+		minHeight = -maxHeight;
+	}
+
+	//	std::cerr << "  minHeight = " << minHeight << "\n";
+	//	std::cerr << "  maxHeight = " << maxHeight << "\n";
+
+	return raw;
+}
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//
+//	TerrainDemo class
+//
+////////////////////////////////////////////////////////////////////////////////
+
+/// class that demonstrates the btHeightfieldTerrainShape object
+class HeightfieldExample : public CommonRigidBodyMTBase//CommonRigidBodyBase
+{
+public:
+	// constructor, destructor ---------------------------------------------
+	HeightfieldExample(struct GUIHelperInterface* helper);
+	virtual ~HeightfieldExample();
+
+	virtual void initPhysics();
+
+	// public class methods ------------------------------------------------
+	
+	void castRays();
+
+	void stepSimulation(float deltaTime);
+
+	void resetCamera()
+	{
+		float dist = 15;
+		float pitch = -32;
+		float yaw = 35;
+		float targetPos[3] = { 0, 0, 0 };
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+
+private:
+	// private helper methods ----------------------------------------------
+	void resetPhysics(void);
+	void clearWorld(void);
+
+	// private data members ------------------------------------------------
+	int					m_upAxis;
+	PHY_ScalarType				m_type;
+	eTerrainModel				m_model;
+	byte_t *				m_rawHeightfieldData;
+	btScalar				m_minHeight;
+	btScalar				m_maxHeight;
+	float					m_phase;	// for dynamics
+	bool					m_isDynamic;
+	btHeightfieldTerrainShape * m_heightfieldShape;
+};
+
+
+#define HEIGHTFIELD_TYPE_COUNT 2
+eTerrainModel gHeightfieldType = eRadial;
+
+void setHeightfieldTypeComboBoxCallback(int combobox, const char* item, void* userPointer)
+{
+	const char** items = static_cast<const char**>(userPointer);
+	for (int i = 0; i < HEIGHTFIELD_TYPE_COUNT; ++i)
+	{
+		if (strcmp(item, items[i]) == 0)
+		{
+			gHeightfieldType = static_cast<eTerrainModel>(i);
+			break;
+		}
+	}
+}
+
+
+
+HeightfieldExample::HeightfieldExample(struct GUIHelperInterface* helper)
+	: CommonRigidBodyMTBase(helper),
+	m_upAxis(1),
+	m_type(PHY_FLOAT),
+	m_model(eFractal),
+	m_rawHeightfieldData(NULL),
+	m_phase(0.0),
+	m_isDynamic(true),
+	m_heightfieldShape(0)
+{
+	{
+		// create a combo box for selecting the solver type
+		static const char* sHeightfieldTypeComboBoxItems[HEIGHTFIELD_TYPE_COUNT];
+		for (int i = 0; i < HEIGHTFIELD_TYPE_COUNT; ++i)
+		{
+			eTerrainModel heightfieldType = static_cast<eTerrainModel>(i);
+			sHeightfieldTypeComboBoxItems[i] = getTerrainTypeName(heightfieldType);
+		}
+		ComboBoxParams comboParams;
+		comboParams.m_userPointer = sHeightfieldTypeComboBoxItems;
+		comboParams.m_numItems = HEIGHTFIELD_TYPE_COUNT;
+		comboParams.m_startItem = gHeightfieldType;
+		comboParams.m_items = sHeightfieldTypeComboBoxItems;
+		comboParams.m_callback = setHeightfieldTypeComboBoxCallback;
+		m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+	}
+}
+
+
+
+HeightfieldExample::~HeightfieldExample(void)
+{
+	clearWorld();
+
+	
+}
+
+
+class MyTriangleCollector3 : public btTriangleCallback
+{
+public:
+	btAlignedObjectArray<GLInstanceVertex>* m_pVerticesOut;
+	btAlignedObjectArray<int>* m_pIndicesOut;
+
+	MyTriangleCollector3()
+	{
+		m_pVerticesOut = 0;
+		m_pIndicesOut = 0;
+	}
+
+	virtual void processTriangle(btVector3* tris, int partId, int triangleIndex)
+	{
+		for (int k = 0; k < 3; k++)
+		{
+			GLInstanceVertex v;
+			v.xyzw[3] = 0;
+			v.uv[0] = v.uv[1] = 0.5f;
+			btVector3 normal = (tris[0] - tris[1]).cross(tris[0] - tris[2]);
+			normal.safeNormalize();
+			for (int l = 0; l < 3; l++)
+			{
+				v.xyzw[l] = tris[k][l];
+				v.normal[l] = normal[l];
+			}
+			m_pIndicesOut->push_back(m_pVerticesOut->size());
+			m_pVerticesOut->push_back(v);
+		}
+	}
+};
+
+
+#define NUMRAYS2 500
+#define USE_PARALLEL_RAYCASTS 1
+
+class btRaycastBar3
+{
+public:
+	btVector3 source[NUMRAYS2];
+	btVector3 dest[NUMRAYS2];
+	btVector3 direction[NUMRAYS2];
+	btVector3 hit[NUMRAYS2];
+	btVector3 normal[NUMRAYS2];
+	struct GUIHelperInterface* m_guiHelper;
+
+	int frame_counter;
+	int ms;
+	int sum_ms;
+	int sum_ms_samples;
+	int min_ms;
+	int max_ms;
+
+#ifdef USE_BT_CLOCK
+	btClock frame_timer;
+#endif  //USE_BT_CLOCK
+
+	btScalar dx;
+	btScalar min_x;
+	btScalar max_x;
+	btScalar max_y;
+	btScalar sign;
+
+	btRaycastBar3()
+	{
+		m_guiHelper = 0;
+		ms = 0;
+		max_ms = 0;
+		min_ms = 9999;
+		sum_ms_samples = 0;
+		sum_ms = 0;
+	}
+
+	btRaycastBar3(btScalar ray_length, btScalar z, btScalar max_y, struct GUIHelperInterface* guiHelper)
+	{
+		m_guiHelper = guiHelper;
+		frame_counter = 0;
+		ms = 0;
+		max_ms = 0;
+		min_ms = 9999;
+		sum_ms_samples = 0;
+		sum_ms = 0;
+		dx = 10.0;
+		min_x = 0;
+		max_x = 0;
+		this->max_y = max_y;
+		sign = 1.0;
+		btScalar dalpha = 2 * SIMD_2_PI / NUMRAYS2;
+		for (int i = 0; i < NUMRAYS2; i++)
+		{
+			btScalar alpha = dalpha * i;
+			// rotate around by alpha degrees y
+			btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha);
+			direction[i] = btVector3(1.0, 0.0, 0.0);
+			direction[i] = quatRotate(q, direction[i]);
+			direction[i] = direction[i] * ray_length;
+
+			source[i] = btVector3(min_x, max_y, z);
+			dest[i] = source[i] + direction[i];
+			dest[i][1] = -1000;
+			normal[i] = btVector3(1.0, 0.0, 0.0);
+		}
+	}
+
+	void move(btScalar dt)
+	{
+		if (dt > btScalar(1.0 / 60.0))
+			dt = btScalar(1.0 / 60.0);
+		for (int i = 0; i < NUMRAYS2; i++)
+		{
+			source[i][0] += dx * dt * sign;
+			dest[i][0] += dx * dt * sign;
+		}
+		if (source[0][0] < min_x)
+			sign = 1.0;
+		else if (source[0][0] > max_x)
+			sign = -1.0;
+	}
+
+	void castRays(btCollisionWorld* cw, int iBegin, int iEnd)
+	{
+		if (m_guiHelper==0)
+			return;
+
+		for (int i = iBegin; i < iEnd; ++i)
+		{
+			btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]);
+
+			{
+				BT_PROFILE("cw->rayTest");
+				//to disable raycast accelerator, uncomment next line 
+				//cb.m_flags |= btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator;
+				cw->rayTest(source[i], dest[i], cb);
+			}
+			if (cb.hasHit())
+			{
+				hit[i] = cb.m_hitPointWorld;
+				normal[i] = cb.m_hitNormalWorld;
+				normal[i].normalize();
+			}
+			else
+			{
+				hit[i] = dest[i];
+				normal[i] = btVector3(1.0, 0.0, 0.0);
+			}
+		}
+	}
+
+	struct CastRaysLoopBody : public btIParallelForBody
+	{
+		btCollisionWorld* mWorld;
+		btRaycastBar3* mRaycasts;
+
+		CastRaysLoopBody(btCollisionWorld* cw, btRaycastBar3* rb) : mWorld(cw), mRaycasts(rb) {}
+
+		void forLoop(int iBegin, int iEnd) const
+		{
+			mRaycasts->castRays(mWorld, iBegin, iEnd);
+		}
+	};
+
+	void cast(btCollisionWorld* cw, bool multiThreading = false)
+	{
+		BT_PROFILE("cast");
+
+#ifdef USE_BT_CLOCK
+		frame_timer.reset();
+#endif  //USE_BT_CLOCK
+
+#ifdef BATCH_RAYCASTER
+		if (!gBatchRaycaster)
+			return;
+
+		gBatchRaycaster->clearRays();
+		for (int i = 0; i < NUMRAYS; i++)
+		{
+			gBatchRaycaster->addRay(source[i], dest[i]);
+		}
+		gBatchRaycaster->performBatchRaycast();
+		for (int i = 0; i < gBatchRaycaster->getNumRays(); i++)
+		{
+			const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
+			hit[i].setInterpolate3(source[i], dest[i], out.hitFraction);
+			normal[i] = out.hitNormal;
+			normal[i].normalize();
+		}
+#else
+#if USE_PARALLEL_RAYCASTS
+		if (multiThreading)
+		{
+			CastRaysLoopBody rayLooper(cw, this);
+			int grainSize = 20;  // number of raycasts per task
+			btParallelFor(0, NUMRAYS2, grainSize, rayLooper);
+		}
+		else
+#endif  // USE_PARALLEL_RAYCASTS
+		{
+			// single threaded
+			castRays(cw, 0, NUMRAYS2);
+		}
+#ifdef USE_BT_CLOCK
+		ms += frame_timer.getTimeMilliseconds();
+#endif  //USE_BT_CLOCK
+		frame_counter++;
+		if (frame_counter > 50)
+		{
+			min_ms = ms < min_ms ? ms : min_ms;
+			max_ms = ms > max_ms ? ms : max_ms;
+			sum_ms += ms;
+			sum_ms_samples++;
+			btScalar mean_ms = (btScalar)sum_ms / (btScalar)sum_ms_samples;
+			printf("%d rays in %d ms %d %d %f\n", NUMRAYS2 * frame_counter, ms, min_ms, max_ms, mean_ms);
+			ms = 0;
+			frame_counter = 0;
+		}
+#endif
+	}
+
+	void draw()
+	{
+		if (m_guiHelper)
+		{
+			btAlignedObjectArray<unsigned int> indices;
+			btAlignedObjectArray<btVector3FloatData> points;
+
+			float lineColor[4] = { 1, 0.4, .4, 1 };
+
+			for (int i = 0; i < NUMRAYS2; i++)
+			{
+				btVector3FloatData s, h;
+				for (int w = 0; w < 4; w++)
+				{
+					s.m_floats[w] = source[i][w];
+					h.m_floats[w] = hit[i][w];
+				}
+
+				points.push_back(s);
+				points.push_back(h);
+				indices.push_back(indices.size());
+				indices.push_back(indices.size());
+			}
+
+			m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0], lineColor, points.size(), sizeof(btVector3FloatData), &indices[0], indices.size(), 1);
+		}
+
+	}
+};
+
+static btRaycastBar3 raycastBar;
+
+void HeightfieldExample::castRays()
+{
+#ifdef BT_THREADSAFE
+	raycastBar.cast(m_dynamicsWorld, true);
+#else
+	raycastBar.cast(m_dynamicsWorld, false);
+#endif
+}
+
+void HeightfieldExample::stepSimulation(float deltaTime)
+{
+	castRays();
+
+	raycastBar.draw();
+
+	// if dynamic and radial, go ahead and update the field
+	if (m_rawHeightfieldData && m_isDynamic && eRadial == m_model && m_heightfieldShape)
+	{
+		btAlignedObjectArray<GLInstanceVertex> gfxVertices;
+		btAlignedObjectArray<int> indices;
+		int strideInBytes = 9 * sizeof(float);
+
+		m_phase += s_deltaPhase * deltaTime;
+		if (m_phase > 2.0 * SIMD_PI) {
+			m_phase -= 2.0 * SIMD_PI;
+		}
+		int bpe = getByteSize(m_type);
+		btAssert(bpe > 0 && "Bad bytes per element");
+		setRadial(m_rawHeightfieldData, bpe, m_type, m_phase);
+
+		MyTriangleCollector3  col;
+		col.m_pVerticesOut = &gfxVertices;
+		col.m_pIndicesOut = &indices;
+		btVector3 aabbMin, aabbMax;
+		for (int k = 0; k < 3; k++)
+		{
+			aabbMin[k] = -BT_LARGE_FLOAT;
+			aabbMax[k] = BT_LARGE_FLOAT;
+		}
+		m_heightfieldShape->processAllTriangles(&col, aabbMin, aabbMax);
+		if (gfxVertices.size() && indices.size())
+		{
+			m_guiHelper->getRenderInterface()->updateShape(m_heightfieldShape->getUserIndex(), &gfxVertices[0].xyzw[0]);
+		}
+	}
+
+	if (m_model != gHeightfieldType)
+	{
+		m_model = gHeightfieldType;
+		resetPhysics();
+	}
+	CommonRigidBodyMTBase::stepSimulation(deltaTime);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+//
+//	TerrainDemo -- public class methods
+//
+////////////////////////////////////////////////////////////////////////////////
+
+/// one-time class and physics initialization
+void HeightfieldExample::initPhysics()
+{
+	//	std::cerr << "initializing...\n";
+	
+	createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	raycastBar = btRaycastBar3(2500.0, 0, 2.0, m_guiHelper);
+	// set up basic state
+	m_upAxis = 1;		// start with Y-axis as "up"
+
+
+	m_type = PHY_FLOAT;// SHORT;
+	m_model = gHeightfieldType;
+	m_isDynamic = true;
+
+	// set up the physics world
+	
+	// initialize axis- or type-dependent physics from here
+	this->resetPhysics();
+	
+}
+
+
+
+
+static PHY_ScalarType nextType (PHY_ScalarType type)
+{
+	switch (type)
+	{
+	case PHY_FLOAT:
+		return PHY_SHORT;
+		break;
+	case PHY_SHORT:
+		return PHY_UCHAR;
+		break;
+	case PHY_UCHAR:
+		return PHY_FLOAT;
+		break;
+	}
+	btAssert (0);
+	return PHY_FLOAT;
+}
+
+
+
+static void doPrint(int x, int& y, int dy, const char * text)
+{
+	//GLDebugDrawString(x, y, text);
+	y += dy;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+//
+//	TerrainDemo -- private helper methods
+//
+////////////////////////////////////////////////////////////////////////////////
+
+/// called whenever key terrain attribute is changed
+void HeightfieldExample::resetPhysics(void)
+{
+	m_guiHelper->removeAllGraphicsInstances();
+
+	// remove old heightfield
+	clearWorld();
+
+	// reset gravity to point in appropriate direction
+	m_dynamicsWorld->setGravity(getUpVector(m_upAxis, 0.0, -s_gravity));
+
+	// get new heightfield of appropriate type
+	m_rawHeightfieldData =
+		getRawHeightfieldData(m_model, m_type, m_minHeight, m_maxHeight);
+	btAssert(m_rawHeightfieldData && "failed to create raw heightfield");
+
+	bool flipQuadEdges = false;
+	m_heightfieldShape =
+		new btHeightfieldTerrainShape(s_gridSize, s_gridSize,
+			m_rawHeightfieldData,
+			s_gridHeightScale,
+			m_minHeight, m_maxHeight,
+			m_upAxis, m_type, flipQuadEdges);
+	btAssert(m_heightfieldShape && "null heightfield");
+
+	//buildAccelerator is optional, it may not support all features.
+	m_heightfieldShape->buildAccelerator();
+
+	// scale the shape
+	btVector3 localScaling = getUpVector(m_upAxis, s_gridSpacing, 1.0);
+	m_heightfieldShape->setLocalScaling(localScaling);
+
+	// stash this shape away
+	m_collisionShapes.push_back(m_heightfieldShape);
+
+	// set origin to middle of heightfield
+	btTransform tr;
+	tr.setIdentity();
+	tr.setOrigin(btVector3(0, 0, 0));
+
+	// create ground object
+	float mass = 0.0;
+	createRigidBody(mass, tr, m_heightfieldShape);
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+
+/// removes all objects and shapes from the world
+void HeightfieldExample::clearWorld(void)
+{
+	if (m_dynamicsWorld)
+	{
+		//remove the rigidbodies from the dynamics world and delete them
+		int i;
+		for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+		{
+			btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+			btRigidBody* body = btRigidBody::upcast(obj);
+			if (body && body->getMotionState())
+			{
+				delete body->getMotionState();
+			}
+			m_dynamicsWorld->removeCollisionObject(obj);
+			delete obj;
+		}
+
+		//delete collision shapes
+		for (int j = 0; j < m_collisionShapes.size(); j++)
+		{
+			btCollisionShape* shape = m_collisionShapes[j];
+			delete shape;
+		}
+		m_collisionShapes.clear();
+
+		// delete raw heightfield data
+		delete m_rawHeightfieldData;
+		m_rawHeightfieldData = NULL;
+	}
+}
+
+CommonExampleInterface* HeightfieldExampleCreateFunc(CommonExampleOptions& options)
+{
+	return new HeightfieldExample(options.m_guiHelper);
+}
+
+B3_STANDALONE_EXAMPLE(HeightfieldExampleCreateFunc)
+
diff --git a/examples/Heightfield/HeightfieldExample.h b/examples/Heightfield/HeightfieldExample.h
new file mode 100644
index 000000000..258e7f701
--- /dev/null
+++ b/examples/Heightfield/HeightfieldExample.h
@@ -0,0 +1,21 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+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 removed or altered from any source distribution.
+*/
+
+#ifndef HEIGHTFIELD_EXAMPLE_H
+#define HEIGHTFIELD_EXAMPLE_H
+
+class CommonExampleInterface* HeightfieldExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //HEIGHTFIELD_EXAMPLE_H
\ No newline at end of file
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
index 782e9efaf..eea18339d 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
@@ -22,6 +22,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionShapes/btSphereShape.h"                 //for raycasting
 #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"        //for raycasting
 #include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"  //for raycasting
+#include "BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h"     //for raycasting
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
@@ -413,6 +414,21 @@ void btCollisionWorld::rayTestSingleInternal(const btTransform& rayFromTrans, co
 				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
 				triangleMesh->performRaycast(&rcb, rayFromLocalScaled, rayToLocalScaled);
 			}
+			else if (((resultCallback.m_flags&btTriangleRaycastCallback::kF_DisableHeightfieldAccelerator)==0) 
+				&& collisionShape->getShapeType() == TERRAIN_SHAPE_PROXYTYPE && 
+				(((btHeightfieldTerrainShape*)collisionShape)->getUpAxis()==1)//accelerator only supports Y axis at the moment
+				)
+			{
+				///optimized version for btHeightfieldTerrainShape
+				btHeightfieldTerrainShape* heightField = (btHeightfieldTerrainShape*)collisionShape;
+				btTransform worldTocollisionObject = colObjWorldTransform.inverse();
+				btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin();
+				btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin();
+
+				BridgeTriangleRaycastCallback rcb(rayFromLocal, rayToLocal, &resultCallback, collisionObjectWrap->getCollisionObject(), heightField, colObjWorldTransform);
+				rcb.m_hitFraction = resultCallback.m_closestHitFraction;
+				heightField->performRaycast(&rcb, rayFromLocal, rayToLocal);
+			}
 			else
 			{
 				//generic (slower) case
diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
index c85ce2498..456418ef0 100644
--- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
+++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp
@@ -73,6 +73,10 @@ void btHeightfieldTerrainShape::initialize(
 	m_useZigzagSubdivision = false;
 	m_upAxis = upAxis;
 	m_localScaling.setValue(btScalar(1.), btScalar(1.), btScalar(1.));
+	
+	m_vboundsChunkSize = 0;
+	m_vboundsGridWidth = 0;
+	m_vboundsGridLength = 0;
 
 	// determine min/max axis-aligned bounding box (aabb) values
 	switch (m_upAxis)
@@ -108,6 +112,7 @@ void btHeightfieldTerrainShape::initialize(
 
 btHeightfieldTerrainShape::~btHeightfieldTerrainShape()
 {
+	clearAccelerator();
 }
 
 void btHeightfieldTerrainShape::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
@@ -323,6 +328,8 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback
 		}
 	}
 
+	// TODO If m_vboundsGrid is available, use it to determine if we really need to process this area
+
 	for (int j = startJ; j < endJ; j++)
 	{
 		for (int x = startX; x < endX; x++)
@@ -373,3 +380,447 @@ const btVector3& btHeightfieldTerrainShape::getLocalScaling() const
 {
 	return m_localScaling;
 }
+
+namespace
+{
+	struct GridRaycastState
+	{
+		int x;  // Next quad coords
+		int z;
+		int prev_x;  // Previous quad coords
+		int prev_z;
+		btScalar param;      // Exit param for previous quad
+		btScalar prevParam;  // Enter param for previous quad
+		btScalar maxDistanceFlat;
+		btScalar maxDistance3d;
+	};
+}
+
+// TODO Does it really need to take 3D vectors?
+/// Iterates through a virtual 2D grid of unit-sized square cells,
+/// and executes an action on each cell intersecting the given segment, ordered from begin to end.
+/// Initially inspired by http://www.cse.yorku.ca/~amana/research/grid.pdf
+template <typename Action_T>
+void gridRaycast(Action_T& quadAction, const btVector3& beginPos, const btVector3& endPos)
+{
+	GridRaycastState rs;
+	rs.maxDistance3d = beginPos.distance(endPos);
+	if (rs.maxDistance3d < 0.0001)
+	{
+		// Consider the ray is too small to hit anything
+		return;
+	}
+
+	btScalar rayDirectionFlatX = endPos[0] - beginPos[0];
+	btScalar rayDirectionFlatZ = endPos[2] - beginPos[2];
+	rs.maxDistanceFlat = btSqrt(rayDirectionFlatX * rayDirectionFlatX + rayDirectionFlatZ * rayDirectionFlatZ);
+
+	if (rs.maxDistanceFlat < 0.0001)
+	{
+		// Consider the ray vertical
+		rayDirectionFlatX = 0;
+		rayDirectionFlatZ = 0;
+	}
+	else
+	{
+		rayDirectionFlatX /= rs.maxDistanceFlat;
+		rayDirectionFlatZ /= rs.maxDistanceFlat;
+	}
+
+	const int xiStep = rayDirectionFlatX > 0 ? 1 : rayDirectionFlatX < 0 ? -1 : 0;
+	const int ziStep = rayDirectionFlatZ > 0 ? 1 : rayDirectionFlatZ < 0 ? -1 : 0;
+
+	const float infinite = 9999999;
+	const btScalar paramDeltaX = xiStep != 0 ? 1.f / btFabs(rayDirectionFlatX) : infinite;
+	const btScalar paramDeltaZ = ziStep != 0 ? 1.f / btFabs(rayDirectionFlatZ) : infinite;
+
+	// pos = param * dir
+	btScalar paramCrossX;  // At which value of `param` we will cross a x-axis lane?
+	btScalar paramCrossZ;  // At which value of `param` we will cross a z-axis lane?
+
+	// paramCrossX and paramCrossZ are initialized as being the first cross
+	// X initialization
+	if (xiStep != 0)
+	{
+		if (xiStep == 1)
+		{
+			paramCrossX = (ceil(beginPos[0]) - beginPos[0]) * paramDeltaX;
+		}
+		else
+		{
+			paramCrossX = (beginPos[0] - floor(beginPos[0])) * paramDeltaX;
+		}
+	}
+	else
+	{
+		paramCrossX = infinite;  // Will never cross on X
+	}
+
+	// Z initialization
+	if (ziStep != 0)
+	{
+		if (ziStep == 1)
+		{
+			paramCrossZ = (ceil(beginPos[2]) - beginPos[2]) * paramDeltaZ;
+		}
+		else
+		{
+			paramCrossZ = (beginPos[2] - floor(beginPos[2])) * paramDeltaZ;
+		}
+	}
+	else
+	{
+		paramCrossZ = infinite;  // Will never cross on Z
+	}
+
+	rs.x = static_cast<int>(floor(beginPos[0]));
+	rs.z = static_cast<int>(floor(beginPos[2]));
+
+	// Workaround cases where the ray starts at an integer position
+	if (paramCrossX == 0.0)
+	{
+		paramCrossX += paramDeltaX;
+		// If going backwards, we should ignore the position we would get by the above flooring,
+		// because the ray is not heading in that direction
+		if (xiStep == -1)
+		{
+			rs.x -= 1;
+		}
+	}
+
+	if (paramCrossZ == 0.0)
+	{
+		paramCrossZ += paramDeltaZ;
+		if (ziStep == -1)
+			rs.z -= 1;
+	}
+
+	rs.prev_x = rs.x;
+	rs.prev_z = rs.z;
+	rs.param = 0;
+
+	while (true)
+	{
+		rs.prev_x = rs.x;
+		rs.prev_z = rs.z;
+		rs.prevParam = rs.param;
+
+		if (paramCrossX < paramCrossZ)
+		{
+			// X lane
+			rs.x += xiStep;
+			// Assign before advancing the param,
+			// to be in sync with the initialization step
+			rs.param = paramCrossX;
+			paramCrossX += paramDeltaX;
+		}
+		else
+		{
+			// Z lane
+			rs.z += ziStep;
+			rs.param = paramCrossZ;
+			paramCrossZ += paramDeltaZ;
+		}
+
+		if (rs.param > rs.maxDistanceFlat)
+		{
+			rs.param = rs.maxDistanceFlat;
+			quadAction(rs);
+			break;
+		}
+		else
+		{
+			quadAction(rs);
+		}
+	}
+}
+
+struct ProcessTrianglesAction
+{
+	const btHeightfieldTerrainShape* shape;
+	bool flipQuadEdges;
+	bool useDiamondSubdivision;
+	int width;
+	int length;
+	btTriangleCallback* callback;
+
+	void exec(int x, int z) const
+	{
+		if (x < 0 || z < 0 || x >= width || z >= length)
+		{
+			return;
+		}
+
+		btVector3 vertices[3];
+
+		// TODO Since this is for raycasts, we could greatly benefit from an early exit on the first hit
+
+		// Check quad
+		if (flipQuadEdges || (useDiamondSubdivision && (((z + x) & 1) > 0)))
+		{
+			// First triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x + 1, z, vertices[1]);
+			shape->getVertex(x + 1, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+
+			// Second triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x + 1, z + 1, vertices[1]);
+			shape->getVertex(x, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+		}
+		else
+		{
+			// First triangle
+			shape->getVertex(x, z, vertices[0]);
+			shape->getVertex(x, z + 1, vertices[1]);
+			shape->getVertex(x + 1, z, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+
+			// Second triangle
+			shape->getVertex(x + 1, z, vertices[0]);
+			shape->getVertex(x, z + 1, vertices[1]);
+			shape->getVertex(x + 1, z + 1, vertices[2]);
+			callback->processTriangle(vertices, x, z);
+		}
+	}
+
+	void operator()(const GridRaycastState& bs) const
+	{
+		exec(bs.prev_x, bs.prev_z);
+	}
+};
+
+struct ProcessVBoundsAction
+{
+	const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& vbounds;
+	int width;
+	int length;
+	int chunkSize;
+
+	btVector3 rayBegin;
+	btVector3 rayEnd;
+	btVector3 rayDir;
+
+	ProcessTrianglesAction processTriangles;
+
+	ProcessVBoundsAction(const btAlignedObjectArray<btHeightfieldTerrainShape::Range>& bnd)
+		: vbounds(bnd)
+	{
+	}
+	void operator()(const GridRaycastState& rs) const
+	{
+		int x = rs.prev_x;
+		int z = rs.prev_z;
+
+		if (x < 0 || z < 0 || x >= width || z >= length)
+		{
+			return;
+		}
+
+		const btHeightfieldTerrainShape::Range chunk = vbounds[x + z * width];
+
+		btVector3 enterPos;
+		btVector3 exitPos;
+
+		if (rs.maxDistanceFlat > 0.0001)
+		{
+			btScalar flatTo3d = chunkSize * rs.maxDistance3d / rs.maxDistanceFlat;
+			btScalar enterParam3d = rs.prevParam * flatTo3d;
+			btScalar exitParam3d = rs.param * flatTo3d;
+			enterPos = rayBegin + rayDir * enterParam3d;
+			exitPos = rayBegin + rayDir * exitParam3d;
+
+			// We did enter the flat projection of the AABB,
+			// but we have to check if we intersect it on the vertical axis
+			if (enterPos[1] > chunk.max && exitPos[1] > chunk.max)
+			{
+				return;
+			}
+			if (enterPos[1] < chunk.min && exitPos[1] < chunk.min)
+			{
+				return;
+			}
+		}
+		else
+		{
+			// Consider the ray vertical
+			// (though we shouldn't reach this often because there is an early check up-front)
+			enterPos = rayBegin;
+			exitPos = rayEnd;
+		}
+
+		gridRaycast(processTriangles, enterPos, exitPos);
+		// Note: it could be possible to have more than one grid at different levels,
+		// to do this there would be a branch using a pointer to another ProcessVBoundsAction
+	}
+};
+
+// TODO How do I interrupt the ray when there is a hit? `callback` does not return any result
+/// Performs a raycast using a hierarchical Bresenham algorithm.
+/// Does not allocate any memory by itself.
+void btHeightfieldTerrainShape::performRaycast(btTriangleCallback* callback, const btVector3& raySource, const btVector3& rayTarget) const
+{
+	// Transform to cell-local
+	btVector3 beginPos = raySource / m_localScaling;
+	btVector3 endPos = rayTarget / m_localScaling;
+	beginPos += m_localOrigin;
+	endPos += m_localOrigin;
+
+	ProcessTrianglesAction processTriangles;
+	processTriangles.shape = this;
+	processTriangles.flipQuadEdges = m_flipQuadEdges;
+	processTriangles.useDiamondSubdivision = m_useDiamondSubdivision;
+	processTriangles.callback = callback;
+	processTriangles.width = m_heightStickWidth - 1;
+	processTriangles.length = m_heightStickLength - 1;
+
+	// TODO Transform vectors to account for m_upAxis
+	int iBeginX = static_cast<int>(floor(beginPos[0]));
+	int iBeginZ = static_cast<int>(floor(beginPos[2]));
+	int iEndX = static_cast<int>(floor(endPos[0]));
+	int iEndZ = static_cast<int>(floor(endPos[2]));
+
+	if (iBeginX == iEndX && iBeginZ == iEndZ)
+	{
+		// The ray will never cross quads within the plane,
+		// so directly process triangles within one quad
+		// (typically, vertical rays should end up here)
+		processTriangles.exec(iBeginX, iEndZ);
+		return;
+	}
+
+	if (m_vboundsGrid.size()==0)
+	{
+		// Process all quads intersecting the flat projection of the ray
+		gridRaycast(processTriangles, beginPos, endPos);
+	}
+	else
+	{
+		btVector3 rayDiff = endPos - beginPos;
+		btScalar flatDistance2 = rayDiff[0] * rayDiff[0] + rayDiff[2] * rayDiff[2];
+		if (flatDistance2 < m_vboundsChunkSize * m_vboundsChunkSize)
+		{
+			// Don't use chunks, the ray is too short in the plane
+			gridRaycast(processTriangles, beginPos, endPos);
+		}
+
+		ProcessVBoundsAction processVBounds(m_vboundsGrid);
+		processVBounds.width = m_vboundsGridWidth;
+		processVBounds.length = m_vboundsGridLength;
+		processVBounds.rayBegin = beginPos;
+		processVBounds.rayEnd = endPos;
+		processVBounds.rayDir = rayDiff.normalized();
+		processVBounds.processTriangles = processTriangles;
+		processVBounds.chunkSize = m_vboundsChunkSize;
+		// The ray is long, run raycast on a higher-level grid
+		gridRaycast(processVBounds, beginPos / m_vboundsChunkSize, endPos / m_vboundsChunkSize);
+	}
+}
+
+/// Builds a grid data structure storing the min and max heights of the terrain in chunks.
+/// if chunkSize is zero, that accelerator is removed.
+/// If you modify the heights, you need to rebuild this accelerator.
+void btHeightfieldTerrainShape::buildAccelerator(int chunkSize)
+{
+	if (chunkSize <= 0)
+	{
+		clearAccelerator();
+		return;
+	}
+
+	m_vboundsChunkSize = chunkSize;
+	int nChunksX = m_heightStickWidth / chunkSize;
+	int nChunksZ = m_heightStickLength / chunkSize;
+
+	if (m_heightStickWidth % chunkSize > 0)
+	{
+		++nChunksX;  // In case terrain size isn't dividable by chunk size
+	}
+	if (m_heightStickLength % chunkSize > 0)
+	{
+		++nChunksZ;
+	}
+
+	if (m_vboundsGridWidth != nChunksX || m_vboundsGridLength != nChunksZ)
+	{
+		clearAccelerator();
+		m_vboundsGridWidth = nChunksX;
+		m_vboundsGridLength = nChunksZ;
+	}
+
+	if (nChunksX == 0 || nChunksZ == 0)
+	{
+		return;
+	}
+
+	// This data structure is only reallocated if the required size changed
+	m_vboundsGrid.resize(nChunksX * nChunksZ);
+	
+	// Compute min and max height for all chunks
+	for (int cz = 0; cz < nChunksZ; ++cz)
+	{
+		int z0 = cz * chunkSize;
+
+		for (int cx = 0; cx < nChunksX; ++cx)
+		{
+			int x0 = cx * chunkSize;
+
+			Range r;
+
+			r.min = getRawHeightFieldValue(x0, z0);
+			r.max = r.min;
+
+			// Compute min and max height for this chunk.
+			// We have to include one extra cell to account for neighbors.
+			// Here is why:
+			// Say we have a flat terrain, and a plateau that fits a chunk perfectly.
+			//
+			//   Left        Right
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			// |   |   |   |   |   |
+			// 0---0---0---1---1---1
+			//           x
+			//
+			// If the AABB for the Left chunk did not share vertices with the Right,
+			// then we would fail collision tests at x due to a gap.
+			//
+			for (int z = z0; z < z0 + chunkSize + 1; ++z)
+			{
+				if (z >= m_heightStickLength)
+				{
+					continue;
+				}
+
+				for (int x = x0; x < x0 + chunkSize + 1; ++x)
+				{
+					if (x >= m_heightStickWidth)
+					{
+						continue;
+					}
+
+					btScalar height = getRawHeightFieldValue(x, z);
+
+					if (height < r.min)
+					{
+						r.min = height;
+					}
+					else if (height > r.max)
+					{
+						r.max = height;
+					}
+				}
+			}
+
+			m_vboundsGrid[cx + cz * nChunksX] = r;
+		}
+	}
+}
+
+void btHeightfieldTerrainShape::clearAccelerator()
+{
+	m_vboundsGrid.clear();
+}
\ No newline at end of file
diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
index 8a50a57e3..7e71d4f65 100644
--- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
+++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_HEIGHTFIELD_TERRAIN_SHAPE_H
 
 #include "btConcaveShape.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 ///btHeightfieldTerrainShape simulates a 2D heightfield terrain
 /**
@@ -71,6 +72,13 @@ subject to the following restrictions:
 ATTRIBUTE_ALIGNED16(class)
 btHeightfieldTerrainShape : public btConcaveShape
 {
+public:
+	struct Range
+	{
+		btScalar min;
+		btScalar max;
+	};
+
 protected:
 	btVector3 m_localAabbMin;
 	btVector3 m_localAabbMax;
@@ -100,9 +108,14 @@ protected:
 
 	btVector3 m_localScaling;
 
+	// Accelerator
+	btAlignedObjectArray<Range> m_vboundsGrid;
+	int m_vboundsGridWidth;
+	int m_vboundsGridLength;
+	int m_vboundsChunkSize;
+
 	virtual btScalar getRawHeightFieldValue(int x, int y) const;
 	void quantizeWithClamp(int* out, const btVector3& point, int isMax) const;
-	void getVertex(int x, int y, btVector3& vertex) const;
 
 	/// protected initialization
 	/**
@@ -155,8 +168,19 @@ public:
 
 	virtual const btVector3& getLocalScaling() const;
 
+	void getVertex(int x, int y, btVector3& vertex) const;
+
+	void performRaycast(btTriangleCallback * callback, const btVector3& raySource, const btVector3& rayTarget) const;
+
+	void buildAccelerator(int chunkSize = 16);
+	void clearAccelerator();
+
+	int getUpAxis() const
+	{
+		return m_upAxis;
+	}
 	//debugging
 	virtual const char* getName() const { return "HEIGHTFIELD"; }
 };
 
-#endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
+#endif  //BT_HEIGHTFIELD_TERRAIN_SHAPE_H
\ No newline at end of file
diff --git a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
index 2b2dfabec..2d0df718a 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h
@@ -37,6 +37,7 @@ public:
 													 ///SubSimplexConvexCastRaytest is the default, even if kF_None is set.
 		kF_UseSubSimplexConvexCastRaytest = 1 << 2,  // Uses an approximate but faster ray versus convex intersection algorithm
 		kF_UseGjkConvexCastRaytest = 1 << 3,
+		kF_DisableHeightfieldAccelerator  = 1 << 4, //don't use the heightfield raycast accelerator. See https://github.com/bulletphysics/bullet3/pull/2062
 		kF_Terminator = 0xFFFFFFFF
 	};
 	unsigned int m_flags;