diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 621087b62..a77b7564e 100644
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -75,7 +75,6 @@ jobs:
         arch: x64
 
     - name: Configure CMake
-      # enkiTS is failing ASAN on windows
       run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBOX2D_SANITIZE=ON -DBUILD_SHARED_LIBS=OFF
       # run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DBOX2D_SAMPLES=OFF -DBUILD_SHARED_LIBS=OFF
 
@@ -85,4 +84,22 @@ jobs:
     - name: Test
       working-directory: ${{github.workspace}}/build
       run: ./bin/${{env.BUILD_TYPE}}/test
+      
+  samples-windows:
+    name: windows
+    runs-on: windows-latest
+    steps:
+
+    - uses: actions/checkout@v4
+
+    - name: Setup MSVC dev command prompt
+      uses: TheMrMilchmann/setup-msvc-dev@v3
+      with:
+        arch: x64
+
+    - name: Configure CMake
+      run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=Release -DBOX2D_SAMPLES=ON -DBUILD_SHARED_LIBS=OFF -DBOX2D_UNIT_TESTS=OFF
+      
+    - name: Build
+      run: cmake --build ${{github.workspace}}/build --config Release
       
\ No newline at end of file
diff --git a/include/box2d/box2d.h b/include/box2d/box2d.h
index 7b65841b5..328129250 100644
--- a/include/box2d/box2d.h
+++ b/include/box2d/box2d.h
@@ -51,48 +51,53 @@ B2_API b2SensorEvents b2World_GetSensorEvents( b2WorldId worldId );
 B2_API b2ContactEvents b2World_GetContactEvents( b2WorldId worldId );
 
 /// Overlap test for all shapes that *potentially* overlap the provided AABB
-B2_API void b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context );
+B2_API b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn,
+											  void* context );
 
 /// Overlap test for for all shapes that overlap the provided circle
-B2_API void b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform, b2QueryFilter filter,
-								   b2OverlapResultFcn* fcn, void* context );
+B2_API b2TreeStats b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform,
+												b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context );
 
 /// Overlap test for all shapes that overlap the provided capsule
-B2_API void b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform transform, b2QueryFilter filter,
-									b2OverlapResultFcn* fcn, void* context );
+B2_API b2TreeStats b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform transform,
+												 b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context );
 
 /// Overlap test for all shapes that overlap the provided polygon
-B2_API void b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform transform, b2QueryFilter filter,
-									b2OverlapResultFcn* fcn, void* context );
+B2_API b2TreeStats b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform transform,
+												 b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context );
 
 /// Cast a ray into the world to collect shapes in the path of the ray.
 /// Your callback function controls whether you get the closest point, any point, or n-points.
 /// The ray-cast ignores shapes that contain the starting point.
+/// @note The callback function may receive shapes in any order
 /// @param worldId The world to cast the ray against
 /// @param origin The start point of the ray
 /// @param translation The translation of the ray from the start point to the end point
 /// @param filter Contains bit flags to filter unwanted shapes from the results
 /// @param fcn A user implemented callback function
 /// @param context A user context that is passed along to the callback function
-/// @note The callback function may receive shapes in any order
-B2_API void b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn,
-							 void* context );
+///	@return traversal performance counters
+B2_API b2TreeStats b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter,
+										  b2CastResultFcn* fcn, void* context );
 
 /// Cast a ray into the world to collect the closest hit. This is a convenience function.
 /// This is less general than b2World_CastRay() and does not allow for custom filtering.
 B2_API b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter );
 
 /// Cast a circle through the world. Similar to a cast ray except that a circle is cast instead of a point.
-B2_API void b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Transform originTransform, b2Vec2 translation,
-								b2QueryFilter filter, b2CastResultFcn* fcn, void* context );
+///	@see b2World_CastRay
+B2_API b2TreeStats b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Transform originTransform,
+											 b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn, void* context );
 
 /// Cast a capsule through the world. Similar to a cast ray except that a capsule is cast instead of a point.
-B2_API void b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform, b2Vec2 translation,
-								 b2QueryFilter filter, b2CastResultFcn* fcn, void* context );
+///	@see b2World_CastRay
+B2_API b2TreeStats b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform,
+											  b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn, void* context );
 
 /// Cast a polygon through the world. Similar to a cast ray except that a polygon is cast instead of a point.
-B2_API void b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation,
-								 b2QueryFilter filter, b2CastResultFcn* fcn, void* context );
+///	@see b2World_CastRay
+B2_API b2TreeStats b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform,
+											  b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn, void* context );
 
 /// Enable/disable sleep. If your application does not need sleeping, you can gain some performance
 /// by disabling sleep completely at the world level.
@@ -183,6 +188,9 @@ B2_API b2Counters b2World_GetCounters( b2WorldId worldId );
 /// Dump memory stats to box2d_memory.txt
 B2_API void b2World_DumpMemoryStats( b2WorldId worldId );
 
+/// todo testing
+B2_API void b2World_RebuildStaticTree( b2WorldId worldId );
+
 /** @} */
 
 /**
@@ -466,8 +474,10 @@ B2_API b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, c
 /// @return the shape id for accessing the shape
 B2_API b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon );
 
-/// Destroy a shape
-B2_API void b2DestroyShape( b2ShapeId shapeId );
+/// Destroy a shape. You may defer the body mass update which can improve performance if several shapes on a
+///	body are destroyed at once.
+///	@see b2Body_ApplyMassFromShapes
+B2_API void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass );
 
 /// Shape identifier validation. Provides validation for up to 64K allocations.
 B2_API bool b2Shape_IsValid( b2ShapeId id );
@@ -492,9 +502,9 @@ B2_API void b2Shape_SetUserData( b2ShapeId shapeId, void* userData );
 B2_API void* b2Shape_GetUserData( b2ShapeId shapeId );
 
 /// Set the mass density of a shape, typically in kg/m^2.
-/// This will not update the mass properties on the parent body.
+/// This will optionally update the mass properties on the parent body.
 /// @see b2ShapeDef::density, b2Body_ApplyMassFromShapes
-B2_API void b2Shape_SetDensity( b2ShapeId shapeId, float density );
+B2_API void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass );
 
 /// Get the density of a shape, typically in kg/m^2
 B2_API float b2Shape_GetDensity( b2ShapeId shapeId );
diff --git a/include/box2d/collision.h b/include/box2d/collision.h
index 0e55b9aa3..a9e39b80d 100644
--- a/include/box2d/collision.h
+++ b/include/box2d/collision.h
@@ -29,7 +29,7 @@ typedef struct b2Hull b2Hull;
 /// don't use more vertices.
 #define b2_maxPolygonVertices 8
 
-/// Low level ray-cast input data
+/// Low level ray cast input data
 typedef struct b2RayCastInput
 {
 	/// Start point of the ray cast
@@ -63,7 +63,7 @@ typedef struct b2ShapeCastInput
 	float maxFraction;
 } b2ShapeCastInput;
 
-/// Low level ray-cast or shape-cast output data
+/// Low level ray cast or shape-cast output data
 typedef struct b2CastOutput
 {
 	/// The surface normal at the hit point
@@ -566,16 +566,16 @@ B2_API b2Manifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, b2Trans
 											  b2Transform xfB );
 
 /// Compute the contact manifold between a chain segment and a circle
-B2_API b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA,
-												   const b2Circle* circleB, b2Transform xfB );
+B2_API b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA, const b2Circle* circleB,
+												  b2Transform xfB );
 
 /// Compute the contact manifold between a chain segment and a capsule
-B2_API b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA,
-													const b2Capsule* capsuleB, b2Transform xfB, b2DistanceCache* cache );
+B2_API b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA, const b2Capsule* capsuleB,
+												   b2Transform xfB, b2DistanceCache* cache );
 
 /// Compute the contact manifold between a chain segment and a rounded polygon
-B2_API b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA,
-													const b2Polygon* polygonB, b2Transform xfB, b2DistanceCache* cache );
+B2_API b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA, const b2Polygon* polygonB,
+												   b2Transform xfB, b2DistanceCache* cache );
 
 /**@}*/
 
@@ -602,8 +602,7 @@ B2_API b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segment
 /// The default category bit for a tree proxy. Used for collision filtering.
 #define b2_defaultCategoryBits ( 1 )
 
-/// Convenience mask bits to use when you don't need collision filtering and just want
-/// all results.
+/// Convenience mask bits to use when you don't need collision filtering and just want all results.
 #define b2_defaultMaskBits ( UINT64_MAX )
 
 /// A node in the dynamic tree. This is private data placed here for performance reasons.
@@ -617,31 +616,27 @@ typedef struct b2TreeNode
 
 	union
 	{
-		/// The node parent index
+		/// The node parent index (allocated node)
 		int32_t parent;
 
-		/// The node freelist next index
+		/// The node freelist next index (free node)
 		int32_t next;
 	}; // 4
 
-	/// Child 1 index
+	/// Child 1 index (internal node)
 	int32_t child1; // 4
 
-	/// Child 2 index
-	int32_t child2; // 4
-
-	/// User data
-	// todo could be union with child index
-	int32_t userData; // 4
-
-	/// Leaf = 0, free node = -1
-	int16_t height; // 2
+	union
+	{
+		/// Child 2 index (internal node)
+		int32_t child2;
 
-	/// Has the AABB been enlarged?
-	bool enlarged; // 1
+		/// User data (leaf node)
+		int32_t userData;
+	}; // 4
 
-	/// Padding for clarity
-	char pad[5];
+	uint16_t height; // 2
+	uint16_t flags;	 // 2
 } b2TreeNode;
 
 /// The dynamic tree structure. This should be considered private data.
@@ -682,6 +677,16 @@ typedef struct b2DynamicTree
 	int32_t rebuildCapacity;
 } b2DynamicTree;
 
+/// These are performance results returned by dynamic tree queries.
+typedef struct b2TreeStats
+{
+	/// Number of internal nodes visited during the query
+	int32_t nodeVisits;
+
+	/// Number of leaf nodes visited during the query
+	int32_t leafVisits;
+} b2TreeStats;
+
 /// Constructing the tree initializes the node pool.
 B2_API b2DynamicTree b2DynamicTree_Create( void );
 
@@ -705,49 +710,53 @@ B2_API void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int32_t proxyId, b2
 typedef bool b2TreeQueryCallbackFcn( int32_t proxyId, int32_t userData, void* context );
 
 /// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied AABB.
-B2_API void b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, b2TreeQueryCallbackFcn* callback,
-								 void* context );
+///	@return performance data
+B2_API b2TreeStats b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits,
+											  b2TreeQueryCallbackFcn* callback, void* context );
 
-/// This function receives clipped raycast input for a proxy. The function
+/// This function receives clipped ray cast input for a proxy. The function
 /// returns the new ray fraction.
 /// - return a value of 0 to terminate the ray cast
 /// - return a value less than input->maxFraction to clip the ray
 /// - return a value of input->maxFraction to continue the ray cast without clipping
 typedef float b2TreeRayCastCallbackFcn( const b2RayCastInput* input, int32_t proxyId, int32_t userData, void* context );
 
-/// Ray-cast against the proxies in the tree. This relies on the callback
-/// to perform a exact ray-cast in the case were the proxy contains a shape.
+/// Ray cast against the proxies in the tree. This relies on the callback
+/// to perform a exact ray cast in the case were the proxy contains a shape.
 /// The callback also performs the any collision filtering. This has performance
 /// roughly equal to k * log(n), where k is the number of collisions and n is the
 /// number of proxies in the tree.
 /// Bit-wise filtering using mask bits can greatly improve performance in some scenarios.
+///	However, this filtering may be approximate, so the user should still apply filtering to results.
 /// @param tree the dynamic tree to ray cast
-/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1)
-/// @param maskBits filter bits: `bool accept = (maskBits & node->categoryBits) != 0;`
+/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1)
+/// @param maskBits mask bit hint: `bool accept = (maskBits & node->categoryBits) != 0;`
 /// @param callback a callback class that is called for each proxy that is hit by the ray
 /// @param context user context that is passed to the callback
-B2_API void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits,
-								   b2TreeRayCastCallbackFcn* callback, void* context );
+///	@return performance data
+B2_API b2TreeStats b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits,
+												b2TreeRayCastCallbackFcn* callback, void* context );
 
-/// This function receives clipped ray-cast input for a proxy. The function
+/// This function receives clipped ray cast input for a proxy. The function
 /// returns the new ray fraction.
-/// - return a value of 0 to terminate the ray-cast
+/// - return a value of 0 to terminate the ray cast
 /// - return a value less than input->maxFraction to clip the ray
 /// - return a value of input->maxFraction to continue the ray cast without clipping
 typedef float b2TreeShapeCastCallbackFcn( const b2ShapeCastInput* input, int32_t proxyId, int32_t userData, void* context );
 
-/// Ray-cast against the proxies in the tree. This relies on the callback
-/// to perform a exact ray-cast in the case were the proxy contains a shape.
+/// Ray cast against the proxies in the tree. This relies on the callback
+/// to perform a exact ray cast in the case were the proxy contains a shape.
 /// The callback also performs the any collision filtering. This has performance
 /// roughly equal to k * log(n), where k is the number of collisions and n is the
 /// number of proxies in the tree.
 /// @param tree the dynamic tree to ray cast
-/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
+/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
 /// @param maskBits filter bits: `bool accept = (maskBits & node->categoryBits) != 0;`
 /// @param callback a callback class that is called for each proxy that is hit by the shape
 /// @param context user context that is passed to the callback
-B2_API void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits,
-									 b2TreeShapeCastCallbackFcn* callback, void* context );
+///	@return performance data
+B2_API b2TreeStats b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits,
+												  b2TreeShapeCastCallbackFcn* callback, void* context );
 
 /// Validate this tree. For testing.
 B2_API void b2DynamicTree_Validate( const b2DynamicTree* tree );
@@ -781,7 +790,6 @@ B2_API void b2DynamicTree_ShiftOrigin( b2DynamicTree* tree, b2Vec2 newOrigin );
 B2_API int b2DynamicTree_GetByteCount( const b2DynamicTree* tree );
 
 /// Get proxy user data
-/// @return the proxy user data or 0 if the id is invalid
 B2_INLINE int32_t b2DynamicTree_GetUserData( const b2DynamicTree* tree, int32_t proxyId )
 {
 	return tree->nodes[proxyId].userData;
diff --git a/include/box2d/types.h b/include/box2d/types.h
index 9faf6b84c..672e105b8 100644
--- a/include/box2d/types.h
+++ b/include/box2d/types.h
@@ -58,6 +58,8 @@ typedef struct b2RayResult
 	b2Vec2 point;
 	b2Vec2 normal;
 	float fraction;
+	int nodeVisits;
+	int leafVisits;
 	bool hit;
 } b2RayResult;
 
@@ -220,10 +222,6 @@ typedef struct b2BodyDef
 	/// Used to disable a body. A disabled body does not move or collide.
 	bool isEnabled;
 
-	/// Automatically compute mass and related properties on this body from shapes.
-	/// Triggers whenever a shape is add/removed/changed. Default is true.
-	bool automaticMass;
-
 	/// This allows this body to bypass rotational speed limits. Should only be used
 	/// for circular objects, like wheels.
 	bool allowFastRotation;
@@ -367,6 +365,9 @@ typedef struct b2ShapeDef
 	/// This is implicitly always true for sensors.
 	bool forceContactCreation;
 
+	/// Should the body update the mass properties when this shape is created. Default is true.
+	bool updateBodyMass;
+
 	/// Used internally to detect a valid definition. DO NOT SET.
 	int32_t internalValue;
 } b2ShapeDef;
diff --git a/samples/main.cpp b/samples/main.cpp
index d967f5664..9e4ad4133 100644
--- a/samples/main.cpp
+++ b/samples/main.cpp
@@ -68,7 +68,7 @@ void* AllocFcn( uint32_t size, int32_t alignment )
 	size_t sizeAligned = ( ( size - 1 ) | ( alignment - 1 ) ) + 1;
 	assert( ( sizeAligned & ( alignment - 1 ) ) == 0 );
 
-#if defined( _WIN64 )
+#if defined( _WIN64 ) || defined( _WIN32 )
 	void* ptr = _aligned_malloc( sizeAligned, alignment );
 #else
 	void* ptr = aligned_alloc( alignment, sizeAligned );
@@ -79,7 +79,7 @@ void* AllocFcn( uint32_t size, int32_t alignment )
 
 void FreeFcn( void* mem )
 {
-#if defined( _WIN64 )
+#if defined( _WIN64 ) || defined( _WIN32 )
 	_aligned_free( mem );
 #else
 	free( mem );
diff --git a/samples/sample_benchmark.cpp b/samples/sample_benchmark.cpp
index d761803b3..f349d3dc8 100644
--- a/samples/sample_benchmark.cpp
+++ b/samples/sample_benchmark.cpp
@@ -329,6 +329,16 @@ class BenchmarkTumbler : public Sample
 			polygon = b2MakeOffsetBox( 10.0f, 0.5f, { 0.0f, -10.0f }, b2Rot_identity );
 			b2CreatePolygonShape( bodyId, &shapeDef, &polygon );
 
+			shapeDef.customColor = b2_colorBlueViolet;
+			b2Circle circle = { { 5.0f, 5.0f }, 1.0f };
+			b2CreateCircleShape( bodyId, &shapeDef, &circle );
+			circle = { { 5.0f, -5.0f }, 1.0f };
+			b2CreateCircleShape( bodyId, &shapeDef, &circle );
+			circle = { { -5.0f, -5.0f }, 1.0f };
+			b2CreateCircleShape( bodyId, &shapeDef, &circle );
+			circle = { { -5.0f, 5.0f }, 1.0f };
+			b2CreateCircleShape( bodyId, &shapeDef, &circle );
+
 			// m_motorSpeed = 9.0f;
 			m_motorSpeed = 25.0f;
 
@@ -1436,8 +1446,8 @@ class BenchmarkCompound : public Sample
 			b2BodyDef bodyDef = b2DefaultBodyDef();
 			bodyDef.type = b2_dynamicBody;
 			// defer mass properties to avoid n-squared mass computations
-			bodyDef.automaticMass = false;
 			b2ShapeDef shapeDef = b2DefaultShapeDef();
+			shapeDef.updateBodyMass = false;
 
 			for ( int m = 0; m < count; ++m )
 			{
@@ -1498,13 +1508,14 @@ class BenchmarkKinematic : public Sample
 		b2BodyDef bodyDef = b2DefaultBodyDef();
 		bodyDef.type = b2_kinematicBody;
 		bodyDef.angularVelocity = 1.0f;
-		// defer mass properties to avoid n-squared mass computations
-		bodyDef.automaticMass = false;
 
 		b2ShapeDef shapeDef = b2DefaultShapeDef();
 		shapeDef.filter.categoryBits = 1;
 		shapeDef.filter.maskBits = 2;
 
+		// defer mass properties to avoid n-squared mass computations
+		shapeDef.updateBodyMass = false;
+
 		b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
 
 		for ( int i = -span; i < span; ++i )
@@ -1529,3 +1540,408 @@ class BenchmarkKinematic : public Sample
 };
 
 static int sampleKinematic = RegisterSample( "Benchmark", "Kinematic", BenchmarkKinematic::Create );
+
+enum QueryType
+{
+	e_rayCast,
+	e_circleCast,
+	e_overlap,
+};
+
+class BenchmarkCast : public Sample
+{
+public:
+
+	explicit BenchmarkCast( Settings& settings )
+		: Sample( settings )
+	{
+		if ( settings.restart == false )
+		{
+			g_camera.m_center = { 500.0f, 500.0f };
+			g_camera.m_zoom = 25.0f * 21.0f;
+			settings.drawShapes = g_sampleDebug;
+		}
+
+		m_queryType = e_circleCast;
+		m_ratio = 5.0f;
+		m_grid = 1.0f;
+		m_fill = 0.1f;
+		m_rowCount = g_sampleDebug ? 100 : 1000;
+		m_columnCount = g_sampleDebug ? 100 : 1000;
+		m_minTime = 1e6f;
+		m_drawIndex = 0;
+		m_topDown = false;
+		m_buildTime = 0.0f;
+		m_radius = 0.1f;
+
+		g_seed = 1234;
+		int sampleCount = g_sampleDebug ? 100 : 10000;
+		m_origins.resize( sampleCount );
+		m_translations.resize( sampleCount );
+		float extent = m_rowCount * m_grid;
+
+		// Pre-compute rays to avoid randomizer overhead
+		for ( int i = 0; i < sampleCount; ++i )
+		{
+			b2Vec2 rayStart = RandomVec2( 0.0f, extent );
+			b2Vec2 rayEnd = RandomVec2( 0.0f, extent );
+
+			m_origins[i] = rayStart;
+			m_translations[i] = rayEnd - rayStart;
+		}
+
+		BuildScene();
+	}
+
+	void BuildScene()
+	{
+		g_seed = 1234;
+		b2DestroyWorld( m_worldId );
+		b2WorldDef worldDef = b2DefaultWorldDef();
+		m_worldId = b2CreateWorld( &worldDef );
+
+		b2Timer timer = b2CreateTimer();
+
+		b2BodyDef bodyDef = b2DefaultBodyDef();
+		b2ShapeDef shapeDef = b2DefaultShapeDef();
+
+		float y = 0.0f;
+
+		for ( int i = 0; i < m_rowCount; ++i )
+		{
+			float x = 0.0f;
+
+			for ( int j = 0; j < m_columnCount; ++j )
+			{
+				float fillTest = RandomFloat( 0.0f, 1.0f );
+				if ( fillTest <= m_fill )
+				{
+					bodyDef.position = { x, y };
+					b2BodyId bodyId = b2CreateBody( m_worldId, &bodyDef );
+
+					float ratio = RandomFloat( 1.0f, m_ratio );
+					float halfWidth = RandomFloat( 0.05f, 0.25f );
+
+					b2Polygon box;
+					if ( RandomFloat() > 0.0f )
+					{
+						box = b2MakeBox( ratio * halfWidth, halfWidth );
+					}
+					else
+					{
+						box = b2MakeBox( halfWidth, ratio * halfWidth );
+					}
+
+					int category = RandomInt( 0, 2 );
+					shapeDef.filter.categoryBits = 1 << category;
+					if ( category == 0 )
+					{
+						shapeDef.customColor = b2_colorBox2DBlue;
+					}
+					else if ( category == 1 )
+					{
+						shapeDef.customColor = b2_colorBox2DYellow;
+					}
+					else
+					{
+						shapeDef.customColor = b2_colorBox2DGreen;
+					}
+
+					b2CreatePolygonShape( bodyId, &shapeDef, &box );
+				}
+
+				x += m_grid;
+			}
+
+			y += m_grid;
+		}
+
+		if (m_topDown)
+		{
+			b2World_RebuildStaticTree( m_worldId );
+		}
+
+		m_buildTime = b2GetMilliseconds( &timer );
+		m_minTime = 1e6f;
+	}
+
+	void UpdateUI() override
+	{
+		float height = 240.0f;
+		ImGui::SetNextWindowPos( ImVec2( 10.0f, g_camera.m_height - height - 50.0f ), ImGuiCond_Once );
+		ImGui::SetNextWindowSize( ImVec2( 200.0f, height ) );
+
+		ImGui::Begin( "Cast", nullptr, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize );
+
+		ImGui::PushItemWidth( 100.0f );
+
+		bool changed = false;
+
+		const char* queryTypes[] = { "Ray", "Circle", "Overlap" };
+		int queryType = int( m_queryType );
+		if (ImGui::Combo( "Query", &queryType, queryTypes, IM_ARRAYSIZE( queryTypes ) ))
+		{
+			m_queryType = QueryType( queryType );
+			if ( m_queryType == e_overlap )
+			{
+				m_radius = 5.0f;
+			}
+			else
+			{
+				m_radius = 0.1f;
+			}
+
+			changed = true;
+		}
+
+		if ( ImGui::SliderInt( "rows", &m_rowCount, 0, 1000, "%d" ) )
+		{
+			changed = true;
+		}
+
+		if ( ImGui::SliderInt( "columns", &m_columnCount, 0, 1000, "%d" ) )
+		{
+			changed = true;
+		}
+
+		if ( ImGui::SliderFloat( "fill", &m_fill, 0.0f, 1.0f, "%.2f" ) )
+		{
+			changed = true;
+		}
+
+		if ( ImGui::SliderFloat( "grid", &m_grid, 0.5f, 2.0f, "%.2f" ) )
+		{
+			changed = true;
+		}
+
+		if ( ImGui::SliderFloat( "ratio", &m_ratio, 1.0f, 10.0f, "%.2f" ) )
+		{
+			changed = true;
+		}
+
+		if ( ImGui::Checkbox( "top down", &m_topDown ) )
+		{
+			changed = true;
+		}
+
+		if ( ImGui::Button( "Draw Next" ) )
+		{
+			m_drawIndex = ( m_drawIndex + 1 ) % m_origins.size();
+		}
+
+		ImGui::PopItemWidth();
+		ImGui::End();
+
+		if ( changed )
+		{
+			BuildScene();
+		}
+	}
+
+	struct CastResult
+	{
+		b2Vec2 point;
+		float fraction;
+		bool hit;
+	};
+
+	static float CastCallback( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context )
+	{
+		CastResult* result = (CastResult*)context;
+		result->point = point;
+		result->fraction = fraction;
+		result->hit = true;
+		return fraction;
+	}
+
+	struct OverlapResult
+	{
+		b2Vec2 points[32];
+		int count;
+	};
+
+	static bool OverlapCallback( b2ShapeId shapeId, void* context )
+	{
+		OverlapResult* result = (OverlapResult*)context;
+		if (result->count < 32)
+		{
+			b2AABB aabb = b2Shape_GetAABB( shapeId );
+			result->points[result->count] = b2AABB_Center( aabb );
+			result->count += 1;
+		}
+
+		return true;
+	}
+
+	void Step( Settings& settings ) override
+	{
+		Sample::Step( settings );
+
+		b2QueryFilter filter = b2DefaultQueryFilter();
+		filter.maskBits = 1;
+		int hitCount = 0;
+		int nodeVisits = 0;
+		int leafVisits = 0;
+		float ms = 0.0f;
+		int sampleCount = m_origins.size();
+
+		if ( m_queryType == e_rayCast )
+		{
+			b2Timer timer = b2CreateTimer();
+
+			b2RayResult drawResult = {};
+
+			for ( int i = 0; i < sampleCount; ++i )
+			{
+				b2Vec2 origin = m_origins[i];
+				b2Vec2 translation = m_translations[i];
+
+				b2RayResult result = b2World_CastRayClosest( m_worldId, origin, translation, filter );
+
+				if (i == m_drawIndex)
+				{
+					drawResult = result;
+				}
+
+				nodeVisits += result.nodeVisits;
+				leafVisits += result.leafVisits;
+				hitCount += result.hit ? 1 : 0;
+			}
+
+			ms = b2GetMilliseconds( &timer );
+
+			m_minTime = b2MinFloat( m_minTime, ms );
+
+			b2Vec2 p1 = m_origins[m_drawIndex];
+			b2Vec2 p2 = p1 + m_translations[m_drawIndex];
+			g_draw.DrawSegment( p1, p2, b2_colorWhite );
+			g_draw.DrawPoint( p1, 5.0f, b2_colorGreen );
+			g_draw.DrawPoint( p2, 5.0f, b2_colorRed );
+			if (drawResult.hit)
+			{
+				g_draw.DrawPoint( drawResult.point, 5.0f, b2_colorWhite );
+			}
+		}
+		else if ( m_queryType == e_circleCast )
+		{
+			b2Timer timer = b2CreateTimer();
+
+			b2Circle circle = { { 0.0f, 0.0f }, m_radius };
+			CastResult drawResult = {};
+
+			for ( int i = 0; i < sampleCount; ++i )
+			{
+				b2Transform origin = { m_origins[i], { 1.0f, 0.0f } };
+				b2Vec2 translation = m_translations[i];
+
+				CastResult result;
+				b2TreeStats traversalResult =
+					b2World_CastCircle( m_worldId, &circle, origin, translation, filter, CastCallback, &result );
+
+				if (i == m_drawIndex)
+				{
+					drawResult = result;
+				}
+
+				nodeVisits += traversalResult.nodeVisits;
+				leafVisits += traversalResult.leafVisits;
+				hitCount += result.hit ? 1 : 0;
+			}
+
+			ms = b2GetMilliseconds( &timer );
+
+			m_minTime = b2MinFloat( m_minTime, ms );
+
+			b2Vec2 p1 = m_origins[m_drawIndex];
+			b2Vec2 p2 = p1 + m_translations[m_drawIndex];
+			g_draw.DrawSegment( p1, p2, b2_colorWhite );
+			g_draw.DrawPoint( p1, 5.0f, b2_colorGreen );
+			g_draw.DrawPoint( p2, 5.0f, b2_colorRed );
+			if (drawResult.hit)
+			{
+				b2Vec2 t = b2Lerp( p1, p2, drawResult.fraction );
+				g_draw.DrawCircle( t, m_radius, b2_colorWhite );
+				g_draw.DrawPoint( drawResult.point, 5.0f, b2_colorWhite );
+			}
+		}
+		else if ( m_queryType == e_overlap )
+		{
+			b2Timer timer = b2CreateTimer();
+
+			OverlapResult drawResult = {};
+			b2Vec2 extent = { m_radius, m_radius };
+			OverlapResult result = {};
+
+			for ( int i = 0; i < sampleCount; ++i )
+			{
+				b2Vec2 origin = m_origins[i];
+				b2AABB aabb = { origin - extent, origin + extent };	
+
+				result.count = 0;
+				b2TreeStats traversalResult = b2World_OverlapAABB( m_worldId, aabb, filter, OverlapCallback, &result );
+
+				if (i == m_drawIndex)
+				{
+					drawResult = result;
+				}
+
+				nodeVisits += traversalResult.nodeVisits;
+				leafVisits += traversalResult.leafVisits;
+				hitCount += result.count;
+			}
+
+			ms = b2GetMilliseconds( &timer );
+
+			m_minTime = b2MinFloat( m_minTime, ms );
+
+			b2Vec2 origin = m_origins[m_drawIndex];
+			b2AABB aabb = { origin - extent, origin + extent };	
+
+			g_draw.DrawAABB( aabb, b2_colorWhite );
+
+			for (int i = 0; i < drawResult.count; ++i)
+			{
+				g_draw.DrawPoint( drawResult.points[i], 5.0f, b2_colorHotPink );
+			}
+		}
+
+		g_draw.DrawString( 5, m_textLine, "build time ms = %g", m_buildTime );
+		m_textLine += m_textIncrement;
+
+		g_draw.DrawString( 5, m_textLine, "hit count = %d, node visits = %d, leaf visits = %d", hitCount, nodeVisits, leafVisits );
+		m_textLine += m_textIncrement;
+
+		g_draw.DrawString( 5, m_textLine, "total ms = %.3f", ms );
+		m_textLine += m_textIncrement;
+
+		g_draw.DrawString( 5, m_textLine, "min total ms = %.3f", m_minTime );
+		m_textLine += m_textIncrement;
+
+		float aveRayCost = 1000.0f * m_minTime / float( sampleCount );
+		g_draw.DrawString( 5, m_textLine, "average us = %.2f", aveRayCost );
+		m_textLine += m_textIncrement;
+	}
+
+	static Sample* Create( Settings& settings )
+	{
+		return new BenchmarkCast( settings );
+	}
+
+	QueryType m_queryType;
+
+	std::vector<b2Vec2> m_origins;
+	std::vector<b2Vec2> m_translations;
+	float m_minTime;
+	float m_buildTime;
+
+	int m_rowCount, m_columnCount;
+	int m_updateType;
+	int m_drawIndex;
+	float m_radius;
+	float m_fill;
+	float m_ratio;
+	float m_grid;
+	bool m_topDown;
+};
+
+static int sampleCast = RegisterSample( "Benchmark", "Cast", BenchmarkCast::Create );
diff --git a/samples/sample_collision.cpp b/samples/sample_collision.cpp
index 9241152e4..8128f185b 100644
--- a/samples/sample_collision.cpp
+++ b/samples/sample_collision.cpp
@@ -691,11 +691,14 @@ class DynamicTree : public Sample
 		if ( m_rayDrag )
 		{
 			b2RayCastInput input = { m_startPoint, b2Sub( m_endPoint, m_startPoint ), 1.0f };
-			b2DynamicTree_RayCast( &m_tree, &input, b2_defaultMaskBits, RayCallback, this );
+			b2TreeStats result = b2DynamicTree_RayCast( &m_tree, &input, b2_defaultMaskBits, RayCallback, this );
 
 			g_draw.DrawSegment( m_startPoint, m_endPoint, b2_colorWhite );
 			g_draw.DrawPoint( m_startPoint, 5.0f, b2_colorGreen );
 			g_draw.DrawPoint( m_endPoint, 5.0f, b2_colorRed );
+
+			g_draw.DrawString( 5, m_textLine, "node visits = %d, leaf visits = %d", result.nodeVisits, result.leafVisits );
+			m_textLine += m_textIncrement;
 		}
 
 		b2HexColor c = b2_colorBlue;
diff --git a/samples/sample_events.cpp b/samples/sample_events.cpp
index 94d03e5a6..8e87121aa 100644
--- a/samples/sample_events.cpp
+++ b/samples/sample_events.cpp
@@ -672,9 +672,17 @@ class ContactEvent : public Sample
 			m_debrisIds[index] = b2_nullBodyId;
 		}
 
+
 		for ( int i = 0; i < destroyCount; ++i )
 		{
-			b2DestroyShape( shapesToDestroy[i] );
+			bool updateMass = false;
+			b2DestroyShape( shapesToDestroy[i], updateMass );
+		}
+
+		if (destroyCount > 0)
+		{
+			// Update mass just once
+			b2Body_ApplyMassFromShapes( m_playerId );
 		}
 
 		if ( settings.hertz > 0.0f && settings.pause == false )
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 695fff850..0bc507726 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -135,6 +135,9 @@ if (MSVC)
 	# Atomics are still considered experimental in Visual Studio 17.8
 	target_compile_options(box2d PRIVATE /experimental:c11atomics)
 
+	# All warnings
+	target_compile_options(box2d PRIVATE /W4)
+
 	if (BOX2D_AVX2)
 		message(STATUS "Box2D using AVX2")	
 		target_compile_definitions(box2d PRIVATE BOX2D_AVX2)
diff --git a/src/body.c b/src/body.c
index 639914258..7dc6f469b 100644
--- a/src/body.c
+++ b/src/body.c
@@ -294,7 +294,6 @@ b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def )
 	body->fixedRotation = def->fixedRotation;
 	body->isSpeedCapped = false;
 	body->isMarked = false;
-	body->automaticMass = def->automaticMass;
 
 	// dynamic and kinematic bodies that are enabled need a island
 	if ( setId >= b2_awakeSet )
@@ -310,6 +309,7 @@ b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def )
 
 bool b2IsBodyAwake( b2World* world, b2Body* body )
 {
+	B2_MAYBE_UNUSED( world );
 	return body->setIndex == b2_awakeSet;
 }
 
diff --git a/src/broad_phase.c b/src/broad_phase.c
index 7502c2f9f..23a3a4057 100644
--- a/src/broad_phase.c
+++ b/src/broad_phase.c
@@ -479,12 +479,7 @@ void b2ValidateNoEnlarged( const b2BroadPhase* bp )
 				continue;
 			}
 
-			if ( node->enlarged == true )
-			{
-				capacity += 0;
-			}
-
-			B2_ASSERT( node->enlarged == false );
+			B2_ASSERT( (node->flags & b2_enlargedNode) == 0 );
 		}
 	}
 #else
diff --git a/src/contact_solver.c b/src/contact_solver.c
index 9c9d547fb..b89bca967 100644
--- a/src/contact_solver.c
+++ b/src/contact_solver.c
@@ -667,12 +667,26 @@ static inline void b2StoreW( float32_t* data, b2FloatW a )
 
 static inline b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b )
 {
+#if defined( __aarch64__ )
 	return vzip1q_f32( a, b );
+#else
+	float32x2_t a1 = vget_low_f32( a );
+	float32x2_t b1 = vget_low_f32( b );
+	float32x2x2_t result = vzip_f32( a1, b1 );
+	return vcombine_f32( result.val[0], result.val[1] );
+#endif
 }
 
 static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b )
 {
+#if defined( __aarch64__ )
 	return vzip2q_f32( a, b );
+#else
+	float32x2_t a1 = vget_high_f32( a );
+	float32x2_t b1 = vget_high_f32( b );
+	float32x2x2_t result = vzip_f32( a1, b1 );
+	return vcombine_f32( result.val[0], result.val[1] );
+#endif
 }
 
 #elif defined( B2_SIMD_SSE2 )
diff --git a/src/core.h b/src/core.h
index a37ec42a0..c7d755e9e 100644
--- a/src/core.h
+++ b/src/core.h
@@ -25,7 +25,7 @@
 #endif
 
 // Define platform
-#if defined( _WIN64 )
+#if defined(_WIN32) || defined(_WIN64)
 	#define B2_PLATFORM_WINDOWS
 #elif defined( __ANDROID__ )
 	#define B2_PLATFORM_ANDROID
@@ -178,6 +178,13 @@ extern float b2_lengthUnitsPerMeter;
 
 #define b2CheckDef( DEF ) B2_ASSERT( DEF->internalValue == B2_SECRET_COOKIE )
 
+enum b2TreeNodeFlags
+{
+	b2_allocatedNode = 0x0001,
+	b2_enlargedNode = 0x0002,
+	b2_leafNode = 0x0004,
+};
+
 void* b2Alloc( int size );
 void b2Free( void* mem, int size );
 void* b2GrowAlloc( void* oldMem, int oldSize, int newSize );
diff --git a/src/dynamic_tree.c b/src/dynamic_tree.c
index 322b5221c..05ba86272 100644
--- a/src/dynamic_tree.c
+++ b/src/dynamic_tree.c
@@ -12,27 +12,28 @@
 
 #define b2_treeStackSize 1024
 
-// TODO_ERIN
-// - try incrementally sorting internal nodes by height for better cache efficiency during depth first traversal.
-
 static b2TreeNode b2_defaultTreeNode = {
-	{ { 0.0f, 0.0f }, { 0.0f, 0.0f } }, 0, { B2_NULL_INDEX }, B2_NULL_INDEX, B2_NULL_INDEX, -1, -2, false,
-	{ 0, 0, 0, 0, 0 } };
+	.aabb = { { 0.0f, 0.0f }, { 0.0f, 0.0f } },
+	.categoryBits = b2_defaultCategoryBits,
+	.parent = B2_NULL_INDEX,
+	.child1 = B2_NULL_INDEX,
+	.child2 = B2_NULL_INDEX,
+	.height = 0,
+	.flags = b2_allocatedNode,
+};
 
 static inline bool b2IsLeaf( const b2TreeNode* node )
 {
-	return node->height == 0;
+	return node->flags & b2_leafNode;
 }
 
-static inline int16_t b2MaxInt16( int16_t a, int16_t b )
+static inline uint16_t b2MaxUInt16( uint16_t a, uint16_t b )
 {
 	return a > b ? a : b;
 }
 
 b2DynamicTree b2DynamicTree_Create( void )
 {
-	_Static_assert( ( sizeof( b2TreeNode ) & 0xF ) == 0, "tree node size not a multiple of 16" );
-
 	b2DynamicTree tree;
 	tree.root = B2_NULL_INDEX;
 
@@ -45,10 +46,9 @@ b2DynamicTree b2DynamicTree_Create( void )
 	for ( int32_t i = 0; i < tree.nodeCapacity - 1; ++i )
 	{
 		tree.nodes[i].next = i + 1;
-		tree.nodes[i].height = -1;
 	}
+
 	tree.nodes[tree.nodeCapacity - 1].next = B2_NULL_INDEX;
-	tree.nodes[tree.nodeCapacity - 1].height = -1;
 	tree.freeList = 0;
 
 	tree.proxyCount = 0;
@@ -95,10 +95,9 @@ static int32_t b2AllocateNode( b2DynamicTree* tree )
 		for ( int32_t i = tree->nodeCount; i < tree->nodeCapacity - 1; ++i )
 		{
 			tree->nodes[i].next = i + 1;
-			tree->nodes[i].height = -1;
 		}
+
 		tree->nodes[tree->nodeCapacity - 1].next = B2_NULL_INDEX;
-		tree->nodes[tree->nodeCapacity - 1].height = -1;
 		tree->freeList = tree->nodeCount;
 	}
 
@@ -117,7 +116,7 @@ static void b2FreeNode( b2DynamicTree* tree, int32_t nodeId )
 	B2_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity );
 	B2_ASSERT( 0 < tree->nodeCount );
 	tree->nodes[nodeId].next = tree->freeList;
-	tree->nodes[nodeId].height = -1;
+	tree->nodes[nodeId].flags = 0;
 	tree->freeList = nodeId;
 	--tree->nodeCount;
 }
@@ -134,7 +133,7 @@ static void b2FreeNode( b2DynamicTree* tree, int32_t nodeId )
 // All of these have a clear cost except when B or C is an internal node. Hence we need to be greedy.
 
 // The cost for cases 1, 2a, and 3a can be computed using the sibling cost formula.
-// cost of sibling H = area(union(H, D)) + increased are of ancestors
+// cost of sibling H = area(union(H, D)) + increased area of ancestors
 
 // Suppose B (or C) is an internal node, then the lowest cost would be one of two cases:
 // case1: D becomes a sibling of B
@@ -353,12 +352,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 
 			C->aabb = aabbBG;
 
-			C->height = 1 + b2MaxInt16( B->height, G->height );
-			A->height = 1 + b2MaxInt16( C->height, F->height );
+			C->height = 1 + b2MaxUInt16( B->height, G->height );
+			A->height = 1 + b2MaxUInt16( C->height, F->height );
 			C->categoryBits = B->categoryBits | G->categoryBits;
 			A->categoryBits = C->categoryBits | F->categoryBits;
-			C->enlarged = B->enlarged || G->enlarged;
-			A->enlarged = C->enlarged || F->enlarged;
+			C->flags |= ( B->flags | G->flags ) & b2_enlargedNode;
+			A->flags |= ( C->flags | F->flags ) & b2_enlargedNode;
 		}
 		else
 		{
@@ -371,12 +370,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 
 			C->aabb = aabbBF;
 
-			C->height = 1 + b2MaxInt16( B->height, F->height );
-			A->height = 1 + b2MaxInt16( C->height, G->height );
+			C->height = 1 + b2MaxUInt16( B->height, F->height );
+			A->height = 1 + b2MaxUInt16( C->height, G->height );
 			C->categoryBits = B->categoryBits | F->categoryBits;
 			A->categoryBits = C->categoryBits | G->categoryBits;
-			C->enlarged = B->enlarged || F->enlarged;
-			A->enlarged = C->enlarged || G->enlarged;
+			C->flags |= ( B->flags | F->flags ) & b2_enlargedNode;
+			A->flags |= ( C->flags | G->flags ) & b2_enlargedNode;
 		}
 	}
 	else if ( C->height == 0 )
@@ -419,12 +418,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 
 			B->aabb = aabbCE;
 
-			B->height = 1 + b2MaxInt16( C->height, E->height );
-			A->height = 1 + b2MaxInt16( B->height, D->height );
+			B->height = 1 + b2MaxUInt16( C->height, E->height );
+			A->height = 1 + b2MaxUInt16( B->height, D->height );
 			B->categoryBits = C->categoryBits | E->categoryBits;
 			A->categoryBits = B->categoryBits | D->categoryBits;
-			B->enlarged = C->enlarged || E->enlarged;
-			A->enlarged = B->enlarged || D->enlarged;
+			B->flags |= ( C->flags | E->flags ) & b2_enlargedNode;
+			A->flags |= ( B->flags | D->flags ) & b2_enlargedNode;
 		}
 		else
 		{
@@ -436,12 +435,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 			E->parent = iA;
 
 			B->aabb = aabbCD;
-			B->height = 1 + b2MaxInt16( C->height, D->height );
-			A->height = 1 + b2MaxInt16( B->height, E->height );
+			B->height = 1 + b2MaxUInt16( C->height, D->height );
+			A->height = 1 + b2MaxUInt16( B->height, E->height );
 			B->categoryBits = C->categoryBits | D->categoryBits;
 			A->categoryBits = B->categoryBits | E->categoryBits;
-			B->enlarged = C->enlarged || D->enlarged;
-			A->enlarged = B->enlarged || E->enlarged;
+			B->flags |= ( C->flags | D->flags ) & b2_enlargedNode;
+			A->flags |= ( B->flags | E->flags ) & b2_enlargedNode;
 		}
 	}
 	else
@@ -517,12 +516,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 				F->parent = iA;
 
 				C->aabb = aabbBG;
-				C->height = 1 + b2MaxInt16( B->height, G->height );
-				A->height = 1 + b2MaxInt16( C->height, F->height );
+				C->height = 1 + b2MaxUInt16( B->height, G->height );
+				A->height = 1 + b2MaxUInt16( C->height, F->height );
 				C->categoryBits = B->categoryBits | G->categoryBits;
 				A->categoryBits = C->categoryBits | F->categoryBits;
-				C->enlarged = B->enlarged || G->enlarged;
-				A->enlarged = C->enlarged || F->enlarged;
+				C->flags |= ( B->flags | G->flags ) & b2_enlargedNode;
+				A->flags |= ( C->flags | F->flags ) & b2_enlargedNode;
 				break;
 
 			case b2_rotateBG:
@@ -533,12 +532,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 				G->parent = iA;
 
 				C->aabb = aabbBF;
-				C->height = 1 + b2MaxInt16( B->height, F->height );
-				A->height = 1 + b2MaxInt16( C->height, G->height );
+				C->height = 1 + b2MaxUInt16( B->height, F->height );
+				A->height = 1 + b2MaxUInt16( C->height, G->height );
 				C->categoryBits = B->categoryBits | F->categoryBits;
 				A->categoryBits = C->categoryBits | G->categoryBits;
-				C->enlarged = B->enlarged || F->enlarged;
-				A->enlarged = C->enlarged || G->enlarged;
+				C->flags |= ( B->flags | F->flags ) & b2_enlargedNode;
+				A->flags |= ( C->flags | G->flags ) & b2_enlargedNode;
 				break;
 
 			case b2_rotateCD:
@@ -549,12 +548,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 				D->parent = iA;
 
 				B->aabb = aabbCE;
-				B->height = 1 + b2MaxInt16( C->height, E->height );
-				A->height = 1 + b2MaxInt16( B->height, D->height );
+				B->height = 1 + b2MaxUInt16( C->height, E->height );
+				A->height = 1 + b2MaxUInt16( B->height, D->height );
 				B->categoryBits = C->categoryBits | E->categoryBits;
 				A->categoryBits = B->categoryBits | D->categoryBits;
-				B->enlarged = C->enlarged || E->enlarged;
-				A->enlarged = B->enlarged || D->enlarged;
+				B->flags |= ( C->flags | E->flags ) & b2_enlargedNode;
+				A->flags |= ( B->flags | D->flags ) & b2_enlargedNode;
 				break;
 
 			case b2_rotateCE:
@@ -565,12 +564,12 @@ static void b2RotateNodes( b2DynamicTree* tree, int32_t iA )
 				E->parent = iA;
 
 				B->aabb = aabbCD;
-				B->height = 1 + b2MaxInt16( C->height, D->height );
-				A->height = 1 + b2MaxInt16( B->height, E->height );
+				B->height = 1 + b2MaxUInt16( C->height, D->height );
+				A->height = 1 + b2MaxUInt16( B->height, E->height );
 				B->categoryBits = C->categoryBits | D->categoryBits;
 				A->categoryBits = B->categoryBits | E->categoryBits;
-				B->enlarged = C->enlarged || D->enlarged;
-				A->enlarged = B->enlarged || E->enlarged;
+				B->flags |= ( C->flags | D->flags ) & b2_enlargedNode;
+				A->flags |= ( B->flags | E->flags ) & b2_enlargedNode;
 				break;
 
 			default:
@@ -644,8 +643,8 @@ static void b2InsertLeaf( b2DynamicTree* tree, int32_t leaf, bool shouldRotate )
 
 		nodes[index].aabb = b2AABB_Union( nodes[child1].aabb, nodes[child2].aabb );
 		nodes[index].categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits;
-		nodes[index].height = 1 + b2MaxInt16( nodes[child1].height, nodes[child2].height );
-		nodes[index].enlarged = nodes[child1].enlarged || nodes[child2].enlarged;
+		nodes[index].height = 1 + b2MaxUInt16( nodes[child1].height, nodes[child2].height );
+		nodes[index].flags |= ( nodes[child1].flags | nodes[child2].flags ) & b2_enlargedNode;
 
 		if ( shouldRotate )
 		{
@@ -710,7 +709,7 @@ static void b2RemoveLeaf( b2DynamicTree* tree, int32_t leaf )
 
 			node->aabb = b2AABB_Union( child1->aabb, child2->aabb );
 			node->categoryBits = child1->categoryBits | child2->categoryBits;
-			node->height = 1 + b2MaxInt16( child1->height, child2->height );
+			node->height = 1 + b2MaxUInt16( child1->height, child2->height );
 
 			index = node->parent;
 		}
@@ -739,6 +738,7 @@ int32_t b2DynamicTree_CreateProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t ca
 	node->userData = userData;
 	node->categoryBits = categoryBits;
 	node->height = 0;
+	node->flags = b2_leafNode;
 
 	bool shouldRotate = true;
 	b2InsertLeaf( tree, proxyId, shouldRotate );
@@ -800,7 +800,7 @@ void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aa
 	while ( parentIndex != B2_NULL_INDEX )
 	{
 		bool changed = b2EnlargeAABB( &nodes[parentIndex].aabb, aabb );
-		nodes[parentIndex].enlarged = true;
+		nodes[parentIndex].flags |= b2_enlargedNode;
 		parentIndex = nodes[parentIndex].parent;
 
 		if ( changed == false )
@@ -811,13 +811,13 @@ void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int32_t proxyId, b2AABB aa
 
 	while ( parentIndex != B2_NULL_INDEX )
 	{
-		if ( nodes[parentIndex].enlarged == true )
+		if ( nodes[parentIndex].flags & b2_enlargedNode )
 		{
 			// early out because this ancestor was previously ascended and marked as enlarged
 			break;
 		}
 
-		nodes[parentIndex].enlarged = true;
+		nodes[parentIndex].flags |= b2_enlargedNode;
 		parentIndex = nodes[parentIndex].parent;
 	}
 }
@@ -871,7 +871,7 @@ static int b2ComputeHeight( const b2DynamicTree* tree, int32_t nodeId )
 
 	int32_t height1 = b2ComputeHeight( tree, node->child1 );
 	int32_t height2 = b2ComputeHeight( tree, node->child2 );
-	return 1 + b2MaxInt16( height1, height2 );
+	return 1 + b2MaxInt( height1, height2 );
 }
 
 int b2DynamicTree_ComputeHeight( const b2DynamicTree* tree )
@@ -895,26 +895,24 @@ static void b2ValidateStructure( const b2DynamicTree* tree, int32_t index )
 
 	const b2TreeNode* node = tree->nodes + index;
 
-	int32_t child1 = node->child1;
-	int32_t child2 = node->child2;
-
 	if ( b2IsLeaf( node ) )
 	{
-		B2_ASSERT( child1 == B2_NULL_INDEX );
-		B2_ASSERT( child2 == B2_NULL_INDEX );
 		B2_ASSERT( node->height == 0 );
 		return;
 	}
 
+	int32_t child1 = node->child1;
+	int32_t child2 = node->child2;
+
 	B2_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity );
 	B2_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity );
 
 	B2_ASSERT( tree->nodes[child1].parent == index );
 	B2_ASSERT( tree->nodes[child2].parent == index );
 
-	if ( tree->nodes[child1].enlarged || tree->nodes[child2].enlarged )
+	if ( ( tree->nodes[child1].flags | tree->nodes[child2].flags ) & b2_enlargedNode )
 	{
-		B2_ASSERT( node->enlarged == true );
+		B2_ASSERT( node->flags & b2_enlargedNode );
 	}
 
 	b2ValidateStructure( tree, child1 );
@@ -930,24 +928,21 @@ static void b2ValidateMetrics( const b2DynamicTree* tree, int32_t index )
 
 	const b2TreeNode* node = tree->nodes + index;
 
-	int32_t child1 = node->child1;
-	int32_t child2 = node->child2;
-
 	if ( b2IsLeaf( node ) )
 	{
-		B2_ASSERT( child1 == B2_NULL_INDEX );
-		B2_ASSERT( child2 == B2_NULL_INDEX );
 		B2_ASSERT( node->height == 0 );
 		return;
 	}
 
+	int child1 = node->child1;
+	int child2 = node->child2;
+
 	B2_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity );
 	B2_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity );
 
-	int32_t height1 = tree->nodes[child1].height;
-	int32_t height2 = tree->nodes[child2].height;
-	int32_t height;
-	height = 1 + b2MaxInt16( height1, height2 );
+	int height1 = tree->nodes[child1].height;
+	int height2 = tree->nodes[child2].height;
+	int height = 1 + b2MaxInt( height1, height2 );
 	B2_ASSERT( node->height == height );
 
 	// b2AABB aabb = b2AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb);
@@ -1000,8 +995,8 @@ void b2DynamicTree_Validate( const b2DynamicTree* tree )
 
 int32_t b2DynamicTree_GetMaxBalance( const b2DynamicTree* tree )
 {
-	int32_t maxBalance = 0;
-	for ( int32_t i = 0; i < tree->nodeCapacity; ++i )
+	int maxBalance = 0;
+	for ( int i = 0; i < tree->nodeCapacity; ++i )
 	{
 		const b2TreeNode* node = tree->nodes + i;
 		if ( node->height <= 1 )
@@ -1011,9 +1006,9 @@ int32_t b2DynamicTree_GetMaxBalance( const b2DynamicTree* tree )
 
 		B2_ASSERT( b2IsLeaf( node ) == false );
 
-		int32_t child1 = node->child1;
-		int32_t child2 = node->child2;
-		int32_t balance = b2AbsFloat( tree->nodes[child2].height - tree->nodes[child1].height );
+		int child1 = node->child1;
+		int child2 = node->child2;
+		int balance = b2AbsInt( tree->nodes[child2].height - tree->nodes[child1].height );
 		maxBalance = b2MaxInt( maxBalance, balance );
 	}
 
@@ -1022,11 +1017,11 @@ int32_t b2DynamicTree_GetMaxBalance( const b2DynamicTree* tree )
 
 void b2DynamicTree_RebuildBottomUp( b2DynamicTree* tree )
 {
-	int32_t* nodes = b2Alloc( tree->nodeCount * sizeof( int32_t ) );
-	int32_t count = 0;
+	int* nodes = b2Alloc( tree->nodeCount * sizeof( int ) );
+	int count = 0;
 
 	// Build array of leaves. Free the rest.
-	for ( int32_t i = 0; i < tree->nodeCapacity; ++i )
+	for ( int i = 0; i < tree->nodeCapacity; ++i )
 	{
 		if ( tree->nodes[i].height < 0 )
 		{
@@ -1079,7 +1074,7 @@ void b2DynamicTree_RebuildBottomUp( b2DynamicTree* tree )
 		parent->child2 = index2;
 		parent->aabb = b2AABB_Union( child1->aabb, child2->aabb );
 		parent->categoryBits = child1->categoryBits | child2->categoryBits;
-		parent->height = 1 + b2MaxInt16( child1->height, child2->height );
+		parent->height = 1 + b2MaxUInt16( child1->height, child2->height );
 		parent->parent = B2_NULL_INDEX;
 
 		child1->parent = parentIndex;
@@ -1117,9 +1112,16 @@ int b2DynamicTree_GetByteCount( const b2DynamicTree* tree )
 	return (int)size;
 }
 
-void b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, b2TreeQueryCallbackFcn* callback,
-						  void* context )
+b2TreeStats b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits,
+									   b2TreeQueryCallbackFcn* callback, void* context )
 {
+	b2TreeStats result = { 0 };
+
+	if (tree->nodeCount == 0)
+	{
+		return result;
+	}
+
 	int32_t stack[b2_treeStackSize];
 	int32_t stackCount = 0;
 	stack[stackCount++] = tree->root;
@@ -1129,10 +1131,13 @@ void b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskB
 		int32_t nodeId = stack[--stackCount];
 		if ( nodeId == B2_NULL_INDEX )
 		{
+			// todo huh?
+			B2_ASSERT( false );
 			continue;
 		}
 
 		const b2TreeNode* node = tree->nodes + nodeId;
+		result.nodeVisits += 1;
 
 		if ( b2AABB_Overlaps( node->aabb, aabb ) && ( node->categoryBits & maskBits ) != 0 )
 		{
@@ -1140,9 +1145,11 @@ void b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskB
 			{
 				// callback to user code with proxy id
 				bool proceed = callback( nodeId, node->userData, context );
+				result.leafVisits += 1;
+
 				if ( proceed == false )
 				{
-					return;
+					return result;
 				}
 			}
 			else
@@ -1156,11 +1163,121 @@ void b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskB
 			}
 		}
 	}
+
+	return result;
+}
+
+#define B2_DIRK_RECURSE 0
+
+#if B2_DIRK_RECURSE == 1
+b2TraversalResult b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits,
+										 b2TreeRayCastCallbackFcn* callback, void* context )
+{
+	b2TraversalResult result = { 0 };
+
+	b2Vec2 p1 = input->origin;
+	b2Vec2 d = input->translation;
+
+	b2Vec2 r = b2Normalize( d );
+
+	// v is perpendicular to the segment.
+	b2Vec2 v = b2CrossSV( 1.0f, r );
+	b2Vec2 abs_v = b2Abs( v );
+
+	// Separating axis for segment (Gino, p80).
+	// |dot(v, p1 - c)| > dot(|v|, h)
+
+	float maxFraction = input->maxFraction;
+
+	b2Vec2 p2 = b2MulAdd( p1, maxFraction, d );
+
+	// Build a bounding box for the segment.
+	b2AABB segmentAABB = { b2Min( p1, p2 ), b2Max( p1, p2 ) };
+
+	int32_t stack[b2_treeStackSize];
+	int32_t stackCount = 0;
+	int32_t nodeId = tree->root;
+
+	b2RayCastInput subInput = *input;
+
+	while ( true )
+	{
+		const b2TreeNode* node = tree->nodes + nodeId;
+		result.nodeVisits += 1;
+
+		b2AABB nodeAABB = node->aabb;
+
+		if ( ( node->categoryBits & maskBits ) != 0 && b2AABB_Overlaps( nodeAABB, segmentAABB ) )
+		{
+			// Separating axis for segment (Gino, p80).
+			// |dot(v, p1 - c)| > dot(|v|, h)
+			// radius extension is added to the node in this case
+			b2Vec2 c = b2AABB_Center( nodeAABB );
+			b2Vec2 h = b2AABB_Extents( nodeAABB );
+			float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) );
+			float term2 = b2Dot( abs_v, h );
+			if ( term2 >= term1 )
+			{
+				if ( b2IsLeaf( node ) )
+				{
+					subInput.maxFraction = maxFraction;
+
+					float value = callback( &subInput, nodeId, node->userData, context );
+					result.leafVisits += 1;
+
+					if ( value == 0.0f )
+					{
+						// The client has terminated the ray cast.
+						return result;
+					}
+
+					if ( 0.0f < value && value < maxFraction )
+					{
+						// Update segment bounding box.
+						maxFraction = value;
+						p2 = b2MulAdd( p1, maxFraction, d );
+						segmentAABB.lowerBound = b2Min( p1, p2 );
+						segmentAABB.upperBound = b2Max( p1, p2 );
+					}
+				}
+				else
+				{
+					B2_ASSERT( stackCount < b2_treeStackSize - 1 );
+					if ( stackCount < b2_treeStackSize - 1 )
+					{
+						stack[stackCount++] = node->child2;
+					}
+
+					nodeId = node->child1;
+
+					continue;
+				}
+			}
+		}
+
+		if ( stackCount == 0 )
+		{
+			break;
+		}
+
+		nodeId = stack[--stackCount];
+	}
+
+	return result;
 }
 
-void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits,
-							b2TreeRayCastCallbackFcn* callback, void* context )
+#else
+
+b2TreeStats b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits,
+										 b2TreeRayCastCallbackFcn* callback, void* context )
 {
+	b2TreeStats result = { 0 };
+
+	if (tree->nodeCount == 0)
+	{
+		return result;
+	}
+
 	b2Vec2 p1 = input->origin;
 	b2Vec2 d = input->translation;
 
@@ -1184,6 +1301,8 @@ void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* inp
 	int32_t stackCount = 0;
 	stack[stackCount++] = tree->root;
 
+	const b2TreeNode* nodes = tree->nodes;
+
 	b2RayCastInput subInput = *input;
 
 	while ( stackCount > 0 )
@@ -1191,11 +1310,17 @@ void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* inp
 		int32_t nodeId = stack[--stackCount];
 		if ( nodeId == B2_NULL_INDEX )
 		{
+			// todo is this possible?
+			B2_ASSERT( false );
 			continue;
 		}
 
-		const b2TreeNode* node = tree->nodes + nodeId;
-		if ( b2AABB_Overlaps( node->aabb, segmentAABB ) == false || ( node->categoryBits & maskBits ) == 0 )
+		const b2TreeNode* node = nodes + nodeId;
+		result.nodeVisits += 1;
+
+		b2AABB nodeAABB = node->aabb;
+
+		if ( ( node->categoryBits & maskBits ) == 0 || b2AABB_Overlaps( nodeAABB, segmentAABB ) == false )
 		{
 			continue;
 		}
@@ -1203,8 +1328,8 @@ void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* inp
 		// Separating axis for segment (Gino, p80).
 		// |dot(v, p1 - c)| > dot(|v|, h)
 		// radius extension is added to the node in this case
-		b2Vec2 c = b2AABB_Center( node->aabb );
-		b2Vec2 h = b2AABB_Extents( node->aabb );
+		b2Vec2 c = b2AABB_Center( nodeAABB );
+		b2Vec2 h = b2AABB_Extents( nodeAABB );
 		float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) );
 		float term2 = b2Dot( abs_v, h );
 		if ( term2 < term1 )
@@ -1217,11 +1342,12 @@ void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* inp
 			subInput.maxFraction = maxFraction;
 
 			float value = callback( &subInput, nodeId, node->userData, context );
+			result.leafVisits += 1;
 
 			if ( value == 0.0f )
 			{
 				// The client has terminated the ray cast.
-				return;
+				return result;
 			}
 
 			if ( 0.0f < value && value < maxFraction )
@@ -1238,21 +1364,35 @@ void b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* inp
 			B2_ASSERT( stackCount < b2_treeStackSize - 1 );
 			if ( stackCount < b2_treeStackSize - 1 )
 			{
-				// TODO_ERIN just put one node on the stack, continue on a child node
-				// TODO_ERIN test ordering children by nearest to ray origin
-				stack[stackCount++] = node->child1;
-				stack[stackCount++] = node->child2;
+				b2Vec2 c1 = b2AABB_Center( nodes[node->child1].aabb );
+				b2Vec2 c2 = b2AABB_Center( nodes[node->child2].aabb );
+				if ( b2DistanceSquared( c1, p1 ) < b2DistanceSquared( c2, p1 ) )
+				{
+					stack[stackCount++] = node->child2;
+					stack[stackCount++] = node->child1;
+				}
+				else
+				{
+					stack[stackCount++] = node->child1;
+					stack[stackCount++] = node->child2;
+				}
 			}
 		}
 	}
+
+	return result;
 }
 
-void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits,
-							  b2TreeShapeCastCallbackFcn* callback, void* context )
+#endif
+
+b2TreeStats b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits,
+										   b2TreeShapeCastCallbackFcn* callback, void* context )
 {
-	if ( input->count == 0 )
+	b2TreeStats stats = { 0 };
+
+	if ( tree->nodeCount == 0 || input->count == 0 )
 	{
-		return;
+		return stats;
 	}
 
 	b2AABB originAABB = { input->points[0], input->points[0] };
@@ -1288,6 +1428,7 @@ void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput*
 	};
 
 	b2ShapeCastInput subInput = *input;
+	const b2TreeNode* nodes = tree->nodes;
 
 	int32_t stack[b2_treeStackSize];
 	int32_t stackCount = 0;
@@ -1298,11 +1439,15 @@ void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput*
 		int32_t nodeId = stack[--stackCount];
 		if ( nodeId == B2_NULL_INDEX )
 		{
+			// todo is this possible?
+			B2_ASSERT( false );
 			continue;
 		}
 
-		const b2TreeNode* node = tree->nodes + nodeId;
-		if ( b2AABB_Overlaps( node->aabb, totalAABB ) == false || ( node->categoryBits & maskBits ) == 0 )
+		const b2TreeNode* node = nodes + nodeId;
+		stats.nodeVisits += 1;
+
+		if ( ( node->categoryBits & maskBits ) == 0 || b2AABB_Overlaps( node->aabb, totalAABB ) == false )
 		{
 			continue;
 		}
@@ -1324,11 +1469,12 @@ void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput*
 			subInput.maxFraction = maxFraction;
 
 			float value = callback( &subInput, nodeId, node->userData, context );
+			stats.leafVisits += 1;
 
 			if ( value == 0.0f )
 			{
 				// The client has terminated the ray cast.
-				return;
+				return stats;
 			}
 
 			if ( 0.0f < value && value < maxFraction )
@@ -1345,13 +1491,23 @@ void b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput*
 			B2_ASSERT( stackCount < b2_treeStackSize - 1 );
 			if ( stackCount < b2_treeStackSize - 1 )
 			{
-				// TODO_ERIN just put one node on the stack, continue on a child node
-				// TODO_ERIN test ordering children by nearest to ray origin
-				stack[stackCount++] = node->child1;
-				stack[stackCount++] = node->child2;
+				b2Vec2 c1 = b2AABB_Center( nodes[node->child1].aabb );
+				b2Vec2 c2 = b2AABB_Center( nodes[node->child2].aabb );
+				if ( b2DistanceSquared( c1, p1 ) < b2DistanceSquared( c2, p1 ) )
+				{
+					stack[stackCount++] = node->child2;
+					stack[stackCount++] = node->child1;
+				}
+				else
+				{
+					stack[stackCount++] = node->child1;
+					stack[stackCount++] = node->child2;
+				}
 			}
 		}
 	}
+
+	return stats;
 }
 
 // Median split == 0, Surface area heuristic == 1
@@ -1368,7 +1524,6 @@ static int32_t b2PartitionMid( int32_t* indices, b2Vec2* centers, int32_t count
 		return count / 2;
 	}
 
-	// todo SIMD?
 	b2Vec2 lowerBound = centers[0];
 	b2Vec2 upperBound = centers[0];
 
@@ -1465,15 +1620,13 @@ static int32_t b2PartitionMid( int32_t* indices, b2Vec2* centers, int32_t count
 	{
 		return i1;
 	}
-	else
-	{
-		return count / 2;
-	}
+
+	return count / 2;
 }
 
 #else
 
-	#define B2_BIN_COUNT 8
+#define B2_BIN_COUNT 64
 
 typedef struct b2TreeBin
 {
@@ -1727,7 +1880,7 @@ static int32_t b2BuildTree( b2DynamicTree* tree, int32_t leafCount )
 			b2TreeNode* child2 = nodes + node->child2;
 
 			node->aabb = b2AABB_Union( child1->aabb, child2->aabb );
-			node->height = 1 + b2MaxInt16( child1->height, child2->height );
+			node->height = 1 + b2MaxUInt16( child1->height, child2->height );
 			node->categoryBits = child1->categoryBits | child2->categoryBits;
 
 			// Pop stack
@@ -1802,7 +1955,7 @@ static int32_t b2BuildTree( b2DynamicTree* tree, int32_t leafCount )
 	b2TreeNode* child2 = nodes + rootNode->child2;
 
 	rootNode->aabb = b2AABB_Union( child1->aabb, child2->aabb );
-	rootNode->height = 1 + b2MaxInt16( child1->height, child2->height );
+	rootNode->height = 1 + b2MaxUInt16( child1->height, child2->height );
 	rootNode->categoryBits = child1->categoryBits | child2->categoryBits;
 
 	return stack[0].nodeIndex;
@@ -1862,7 +2015,7 @@ int32_t b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild )
 	// this should be weighed against b2_aabbMargin
 	while ( true )
 	{
-		if ( node->height == 0 || ( node->enlarged == false && fullBuild == false ) )
+		if ( node->height == 0 || ( ( node->flags & b2_enlargedNode ) == 0 && fullBuild == false ) )
 		{
 			leafIndices[leafCount] = nodeIndex;
 #if B2_TREE_HEURISTIC == 0
@@ -1909,9 +2062,9 @@ int32_t b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild )
 	int32_t capacity = tree->nodeCapacity;
 	for ( int32_t i = 0; i < capacity; ++i )
 	{
-		if ( nodes[i].height >= 0 )
+		if ( nodes[i].flags & b2_allocatedNode )
 		{
-			B2_ASSERT( nodes[i].enlarged == false );
+			B2_ASSERT( ( nodes[i].flags & b2_enlargedNode ) == 0 );
 		}
 	}
 #endif
diff --git a/src/hull.c b/src/hull.c
index 41fd76de1..eb0f4c5c7 100644
--- a/src/hull.c
+++ b/src/hull.c
@@ -94,7 +94,7 @@ b2Hull b2ComputeHull( const b2Vec2* points, int count )
 		return hull;
 	}
 
-	count = b2MinFloat( count, b2_maxPolygonVertices );
+	count = b2MinInt( count, b2_maxPolygonVertices );
 
 	b2AABB aabb = { { FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX } };
 
diff --git a/src/joint.h b/src/joint.h
index 80ac04bfb..441e4394c 100644
--- a/src/joint.h
+++ b/src/joint.h
@@ -47,13 +47,14 @@ typedef struct b2Joint
 	int islandPrev;
 	int islandNext;
 
-	// This is monotonically advanced when a body is allocated in this slot
-	// Used to check for invalid b2JointId
-	int revision;
-
 	float drawSize;
 
 	b2JointType type;
+
+	// This is monotonically advanced when a body is allocated in this slot
+	// Used to check for invalid b2JointId
+	uint16_t revision;
+
 	bool isMarked;
 	bool collideConnected;
 
diff --git a/src/manifold.c b/src/manifold.c
index b66785d28..38f7c94d2 100644
--- a/src/manifold.c
+++ b/src/manifold.c
@@ -328,7 +328,7 @@ b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const
 		return manifold;
 	}
 
-	float distance = sqrt( distanceSquared );
+	float distance = sqrtf( distanceSquared );
 
 	float length1, length2;
 	b2Vec2 u1 = b2GetLengthAndNormalize( &length1, d1 );
diff --git a/src/motor_joint.c b/src/motor_joint.c
index b41f685a8..721e98974 100644
--- a/src/motor_joint.c
+++ b/src/motor_joint.c
@@ -186,6 +186,7 @@ void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context )
 
 void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias )
 {
+	B2_MAYBE_UNUSED( useBias );
 	B2_ASSERT( base->type == b2_motorJoint );
 
 	float mA = base->invMassA;
diff --git a/src/shape.c b/src/shape.c
index b63602253..f2f3900ad 100644
--- a/src/shape.c
+++ b/src/shape.c
@@ -165,7 +165,7 @@ b2ShapeId b2CreateShape( b2BodyId bodyId, const b2ShapeDef* def, const void* geo
 
 	b2Shape* shape = b2CreateShapeInternal( world, body, transform, def, geometry, shapeType );
 
-	if ( body->automaticMass == true )
+	if ( def->updateBodyMass == true )
 	{
 		b2UpdateBodyMassData( world, body );
 	}
@@ -262,7 +262,7 @@ void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body, bool
 	b2ValidateSolverSets( world );
 }
 
-void b2DestroyShape( b2ShapeId shapeId )
+void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass )
 {
 	b2World* world = b2GetWorldLocked( shapeId.world0 );
 
@@ -274,7 +274,7 @@ void b2DestroyShape( b2ShapeId shapeId )
 	b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId );
 	b2DestroyShapeInternal( world, shape, body, wakeBodies );
 
-	if ( body->automaticMass == true )
+	if ( updateBodyMass == true )
 	{
 		b2UpdateBodyMassData( world, body );
 	}
@@ -911,7 +911,7 @@ b2CastOutput b2Shape_RayCast( b2ShapeId shapeId, const b2RayCastInput* input )
 	return output;
 }
 
-void b2Shape_SetDensity( b2ShapeId shapeId, float density )
+void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass )
 {
 	B2_ASSERT( b2IsValid( density ) && density >= 0.0f );
 
@@ -929,6 +929,12 @@ void b2Shape_SetDensity( b2ShapeId shapeId, float density )
 	}
 
 	shape->density = density;
+
+	if (updateBodyMass == true)
+	{
+		b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId );
+		b2UpdateBodyMassData( world, body );
+	}
 }
 
 float b2Shape_GetDensity( b2ShapeId shapeId )
diff --git a/src/solver.c b/src/solver.c
index bd1a81eef..0603f2a31 100644
--- a/src/solver.c
+++ b/src/solver.c
@@ -1515,8 +1515,7 @@ void b2Solve( b2World* world, b2StepContext* stepContext )
 			}
 		}
 
-		ptrdiff_t blockDiff = baseGraphBlock - graphBlocks;
-		B2_ASSERT( blockDiff == graphBlockCount );
+		B2_ASSERT( (ptrdiff_t)(baseGraphBlock - graphBlocks) == graphBlockCount );
 
 		b2SolverStage* stage = stages;
 
diff --git a/src/timer.c b/src/timer.c
index 0aecd57ba..fe2c53d6f 100644
--- a/src/timer.c
+++ b/src/timer.c
@@ -193,7 +193,7 @@ void b2Yield()
 uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count )
 {
 	uint32_t result = hash;
-	for ( size_t i = 0; i < count; i++ )
+	for ( int i = 0; i < count; i++ )
 	{
 		result = ( result << 5 ) + result + data[i];
 	}
diff --git a/src/types.c b/src/types.c
index 42556d4fe..5dd8e45a9 100644
--- a/src/types.c
+++ b/src/types.c
@@ -37,7 +37,6 @@ b2BodyDef b2DefaultBodyDef( void )
 	def.enableSleep = true;
 	def.isAwake = true;
 	def.isEnabled = true;
-	def.automaticMass = true;
 	def.internalValue = B2_SECRET_COOKIE;
 	return def;
 }
@@ -62,6 +61,7 @@ b2ShapeDef b2DefaultShapeDef( void )
 	def.filter = b2DefaultFilter();
 	def.enableSensorEvents = true;
 	def.enableContactEvents = true;
+	def.updateBodyMass = true;
 	def.internalValue = B2_SECRET_COOKIE;
 	return def;
 }
diff --git a/src/weld_joint.c b/src/weld_joint.c
index e772fde22..965a5210f 100644
--- a/src/weld_joint.c
+++ b/src/weld_joint.c
@@ -150,8 +150,6 @@ void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context )
 	float ka = iA + iB;
 	joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f;
 
-	const float h = context->dt;
-
 	if ( joint->linearHertz == 0.0f )
 	{
 		joint->linearSoftness = context->jointSoftness;
diff --git a/src/wheel_joint.c b/src/wheel_joint.c
index e4d96b401..b2a971dfc 100644
--- a/src/wheel_joint.c
+++ b/src/wheel_joint.c
@@ -315,9 +315,6 @@ void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias )
 
 	b2WheelJoint* joint = &base->wheelJoint;
 
-	// This is a dummy body to represent a static body since static bodies don't have a solver body.
-	b2BodyState dummyBody = { 0 };
-
 	b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA;
 	b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB;
 
@@ -378,8 +375,6 @@ void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias )
 
 	if ( joint->enableLimit )
 	{
-		float translation = b2Dot( axisA, d );
-
 		// Lower limit
 		{
 			float C = translation - joint->lowerTranslation;
diff --git a/src/world.c b/src/world.c
index 68be3286e..f9226810c 100644
--- a/src/world.c
+++ b/src/world.c
@@ -294,7 +294,7 @@ void b2DestroyWorld( b2WorldId worldId )
 	// Wipe world but preserve revision
 	uint16_t revision = world->revision;
 	*world = ( b2World ){ 0 };
-	world->worldId = B2_NULL_INDEX;
+	world->worldId = 0;
 	world->revision = revision + 1;
 }
 
@@ -304,7 +304,7 @@ static void b2CollideTask( int startIndex, int endIndex, uint32_t threadIndex, v
 
 	b2StepContext* stepContext = context;
 	b2World* world = stepContext->world;
-	B2_ASSERT( threadIndex < world->workerCount );
+	B2_ASSERT( (int)threadIndex < world->workerCount );
 	b2TaskContext* taskContext = world->taskContexts.data + threadIndex;
 	b2ContactSim** contactSims = stepContext->contacts;
 	b2Shape* shapes = world->shapes.data;
@@ -1884,13 +1884,16 @@ static bool TreeQueryCallback( int proxyId, int shapeId, void* context )
 	return result;
 }
 
-void b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context )
+b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn,
+									   void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2AABB_IsValid( aabb ) );
@@ -1899,8 +1902,14 @@ void b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter,
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeQueryCallback, &worldContext );
+		b2TreeStats treeResult =
+			b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeQueryCallback, &worldContext );
+
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 	}
+
+	return treeStats;
 }
 
 typedef struct WorldOverlapContext
@@ -1953,14 +1962,16 @@ static bool TreeOverlapCallback( int proxyId, int shapeId, void* context )
 	return result;
 }
 
-void b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform, b2QueryFilter filter,
+b2TreeStats b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transform transform, b2QueryFilter filter,
 							b2OverlapResultFcn* fcn, void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2Vec2_IsValid( transform.p ) );
@@ -1973,18 +1984,26 @@ void b2World_OverlapCircle( b2WorldId worldId, const b2Circle* circle, b2Transfo
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext );
+		b2TreeStats treeResult =
+			b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext );
+
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 	}
+
+	return treeStats;
 }
 
-void b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform transform, b2QueryFilter filter,
+b2TreeStats b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform transform, b2QueryFilter filter,
 							 b2OverlapResultFcn* fcn, void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2Vec2_IsValid( transform.p ) );
@@ -1997,18 +2016,26 @@ void b2World_OverlapCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Tran
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext );
+		b2TreeStats treeResult =
+			b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext );
+
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 	}
+
+	return treeStats;
 }
 
-void b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform transform, b2QueryFilter filter,
+b2TreeStats b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform transform, b2QueryFilter filter,
 							 b2OverlapResultFcn* fcn, void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2Vec2_IsValid( transform.p ) );
@@ -2021,8 +2048,14 @@ void b2World_OverlapPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Tran
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext );
+		b2TreeStats treeResult =
+			b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext );
+
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 	}
+
+	return treeStats;
 }
 
 typedef struct WorldRayCastContext
@@ -2065,14 +2098,16 @@ static float RayCastCallback( const b2RayCastInput* input, int proxyId, int shap
 	return input->maxFraction;
 }
 
-void b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn,
+b2TreeStats b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn,
 					  void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2Vec2_IsValid( origin ) );
@@ -2084,15 +2119,19 @@ void b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2Qu
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext );
+		b2TreeStats treeResult = b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext );
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 
 		if ( worldContext.fraction == 0.0f )
 		{
-			return;
+			return treeStats;
 		}
 
 		input.maxFraction = worldContext.fraction;
 	}
+
+	return treeStats;
 }
 
 // This callback finds the closest hit. This is the most common callback used in games.
@@ -2126,7 +2165,9 @@ b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 tra
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext );
+		b2TreeStats treeResult = b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext );
+		result.nodeVisits += treeResult.nodeVisits;
+		result.leafVisits += treeResult.leafVisits;
 
 		if ( worldContext.fraction == 0.0f )
 		{
@@ -2157,6 +2198,7 @@ static float ShapeCastCallback( const b2ShapeCastInput* input, int proxyId, int
 
 	b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId );
 	b2Transform transform = b2GetBodyTransformQuick( world, body );
+
 	b2CastOutput output = b2ShapeCastShape( input, shape, transform );
 
 	if ( output.hit )
@@ -2170,14 +2212,16 @@ static float ShapeCastCallback( const b2ShapeCastInput* input, int proxyId, int
 	return input->maxFraction;
 }
 
-void b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Transform originTransform, b2Vec2 translation,
+b2TreeStats b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Transform originTransform, b2Vec2 translation,
 						 b2QueryFilter filter, b2CastResultFcn* fcn, void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2Vec2_IsValid( originTransform.p ) );
@@ -2195,25 +2239,33 @@ void b2World_CastCircle( b2WorldId worldId, const b2Circle* circle, b2Transform
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext );
+		b2TreeStats treeResult =
+			b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext );
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 
 		if ( worldContext.fraction == 0.0f )
 		{
-			return;
+			return treeStats;
 		}
 
 		input.maxFraction = worldContext.fraction;
 	}
+
+	return treeStats;
 }
 
-void b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform, b2Vec2 translation,
+b2TreeStats b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transform originTransform,
+									   b2Vec2 translation,
 						  b2QueryFilter filter, b2CastResultFcn* fcn, void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2Vec2_IsValid( originTransform.p ) );
@@ -2232,25 +2284,33 @@ void b2World_CastCapsule( b2WorldId worldId, const b2Capsule* capsule, b2Transfo
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext );
+		b2TreeStats treeResult =
+			b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext );
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 
 		if ( worldContext.fraction == 0.0f )
 		{
-			return;
+			return treeStats;
 		}
 
 		input.maxFraction = worldContext.fraction;
 	}
+
+	return treeStats;
 }
 
-void b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform, b2Vec2 translation,
+b2TreeStats b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transform originTransform,
+									   b2Vec2 translation,
 						  b2QueryFilter filter, b2CastResultFcn* fcn, void* context )
 {
+	b2TreeStats treeStats = { 0 };
+
 	b2World* world = b2GetWorldFromId( worldId );
 	B2_ASSERT( world->locked == false );
 	if ( world->locked )
 	{
-		return;
+		return treeStats;
 	}
 
 	B2_ASSERT( b2Vec2_IsValid( originTransform.p ) );
@@ -2271,15 +2331,20 @@ void b2World_CastPolygon( b2WorldId worldId, const b2Polygon* polygon, b2Transfo
 
 	for ( int i = 0; i < b2_bodyTypeCount; ++i )
 	{
-		b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext );
+		b2TreeStats treeResult =
+			b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext );
+		treeStats.nodeVisits += treeResult.nodeVisits;
+		treeStats.leafVisits += treeResult.leafVisits;
 
 		if ( worldContext.fraction == 0.0f )
 		{
-			return;
+			return treeStats;
 		}
 
 		input.maxFraction = worldContext.fraction;
 	}
+
+	return treeStats;
 }
 
 #if 0
@@ -2516,6 +2581,20 @@ void b2World_Explode( b2WorldId worldId, const b2ExplosionDef* explosionDef )
 						 &explosionContext );
 }
 
+void b2World_RebuildStaticTree(b2WorldId worldId)
+{
+	b2World* world = b2GetWorldFromId( worldId );
+	B2_ASSERT( world->locked == false );
+	if ( world->locked )
+	{
+		return;
+	}
+
+	b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody;
+	b2DynamicTree_Rebuild( staticTree, true );
+}
+
+
 #if B2_VALIDATE
 // When validating islands ids I have to compare the root island
 // ids because islands are not merged until the next time step.
@@ -2891,7 +2970,7 @@ void b2ValidateSolverSets( b2World* world )
 
 	int contactIdCount = b2GetIdCount( &world->contactIdPool );
 	B2_ASSERT( totalContactCount == contactIdCount );
-	B2_ASSERT( totalContactCount == world->broadPhase.pairSet.count );
+	B2_ASSERT( totalContactCount == (int)world->broadPhase.pairSet.count );
 
 	int jointIdCount = b2GetIdCount( &world->jointIdPool );
 	B2_ASSERT( totalJointCount == jointIdCount );