tests/box2d/Box2D/Collision/b2DynamicTree.h - external/github.com/kripken/emscripten - Git at Google

 /*
 * Copyright (c) 2009 Erin Catto http://www.box2d.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 B2_DYNAMIC_TREE_H
 #define B2_DYNAMIC_TREE_H

 #include <Box2D/Collision/b2Collision.h>
 #include <Box2D/Common/b2GrowableStack.h>

 #define b2_nullNode (-1)

 /// A node in the dynamic tree. The client does not interact with this directly.
 struct b2TreeNode
 {
 	bool IsLeaf() const
 	{
 		return child1 == b2_nullNode;
 	}

 	/// Enlarged AABB
 	b2AABB aabb;

 	void* userData;

 	union
 	{
 		int32 parent;
 		int32 next;
 	};

 	int32 child1;
 	int32 child2;

 	// leaf = 0, free node = -1
 	int32 height;
 };

 /// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
 /// A dynamic tree arranges data in a binary tree to accelerate
 /// queries such as volume queries and ray casts. Leafs are proxies
 /// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
 /// so that the proxy AABB is bigger than the client object. This allows the client
 /// object to move by small amounts without triggering a tree update.
 ///
 /// Nodes are pooled and relocatable, so we use node indices rather than pointers.
 class b2DynamicTree
 {
 public:
 	/// Constructing the tree initializes the node pool.
 	b2DynamicTree();

 	/// Destroy the tree, freeing the node pool.
 	~b2DynamicTree();

 	/// Create a proxy. Provide a tight fitting AABB and a userData pointer.
 	int32 CreateProxy(const b2AABB& aabb, void* userData);

 	/// Destroy a proxy. This asserts if the id is invalid.
 	void DestroyProxy(int32 proxyId);

 	/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
 	/// then the proxy is removed from the tree and re-inserted. Otherwise
 	/// the function returns immediately.
 	/// @return true if the proxy was re-inserted.
 	bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);

 	/// Get proxy user data.
 	/// @return the proxy user data or 0 if the id is invalid.
 	void* GetUserData(int32 proxyId) const;

 	/// Get the fat AABB for a proxy.
 	const b2AABB& GetFatAABB(int32 proxyId) const;

 	/// Query an AABB for overlapping proxies. The callback class
 	/// is called for each proxy that overlaps the supplied AABB.
 	template <typename T>
 	void Query(T* callback, const b2AABB& aabb) const;

 	/// 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 input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
 	/// @param callback a callback class that is called for each proxy that is hit by the ray.
 	template <typename T>
 	void RayCast(T* callback, const b2RayCastInput& input) const;

 	/// Validate this tree. For testing.
 	void Validate() const;

 	/// Compute the height of the binary tree in O(N) time. Should not be
 	/// called often.
 	int32 GetHeight() const;

 	/// Get the maximum balance of an node in the tree. The balance is the difference
 	/// in height of the two children of a node.
 	int32 GetMaxBalance() const;

 	/// Get the ratio of the sum of the node areas to the root area.
 	float32 GetAreaRatio() const;

 	/// Build an optimal tree. Very expensive. For testing.
 	void RebuildBottomUp();

 private:

 	int32 AllocateNode();
 	void FreeNode(int32 node);

 	void InsertLeaf(int32 node);
 	void RemoveLeaf(int32 node);

 	int32 Balance(int32 index);

 	int32 ComputeHeight() const;
 	int32 ComputeHeight(int32 nodeId) const;

 	void ValidateStructure(int32 index) const;
 	void ValidateMetrics(int32 index) const;

 	int32 m_root;

 	b2TreeNode* m_nodes;
 	int32 m_nodeCount;
 	int32 m_nodeCapacity;

 	int32 m_freeList;

 	/// This is used to incrementally traverse the tree for re-balancing.
 	uint32 m_path;

 	int32 m_insertionCount;
 };

 inline void* b2DynamicTree::GetUserData(int32 proxyId) const
 {
 	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
 	return m_nodes[proxyId].userData;
 }

 inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
 {
 	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
 	return m_nodes[proxyId].aabb;
 }

 template <typename T>
 inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
 {
 	b2GrowableStack<int32, 256> stack;
 	stack.Push(m_root);

 	while (stack.GetCount() > 0)
 	{
 		int32 nodeId = stack.Pop();
 		if (nodeId == b2_nullNode)
 		{
 			continue;
 		}

 		const b2TreeNode* node = m_nodes + nodeId;

 		if (b2TestOverlap(node->aabb, aabb))
 		{
 			if (node->IsLeaf())
 			{
 				bool proceed = callback->QueryCallback(nodeId);
 				if (proceed == false)
 				{
 					return;
 				}
 			}
 			else
 			{
 				stack.Push(node->child1);
 				stack.Push(node->child2);
 			}
 		}
 	}
 }

 template <typename T>
 inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
 {
 	b2Vec2 p1 = input.p1;
 	b2Vec2 p2 = input.p2;
 	b2Vec2 r = p2 - p1;
 	b2Assert(r.LengthSquared() > 0.0f);
 	r.Normalize();

 	// v is perpendicular to the segment.
 	b2Vec2 v = b2Cross(1.0f, r);
 	b2Vec2 abs_v = b2Abs(v);

 	// Separating axis for segment (Gino, p80).
 	// |dot(v, p1 - c)| > dot(|v|, h)

 	float32 maxFraction = input.maxFraction;

 	// Build a bounding box for the segment.
 	b2AABB segmentAABB;
 	{
 		b2Vec2 t = p1 + maxFraction * (p2 - p1);
 		segmentAABB.lowerBound = b2Min(p1, t);
 		segmentAABB.upperBound = b2Max(p1, t);
 	}

 	b2GrowableStack<int32, 256> stack;
 	stack.Push(m_root);

 	while (stack.GetCount() > 0)
 	{
 		int32 nodeId = stack.Pop();
 		if (nodeId == b2_nullNode)
 		{
 			continue;
 		}

 		const b2TreeNode* node = m_nodes + nodeId;

 		if (b2TestOverlap(node->aabb, segmentAABB) == false)
 		{
 			continue;
 		}

 		// Separating axis for segment (Gino, p80).
 		// |dot(v, p1 - c)| > dot(|v|, h)
 		b2Vec2 c = node->aabb.GetCenter();
 		b2Vec2 h = node->aabb.GetExtents();
 		float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
 		if (separation > 0.0f)
 		{
 			continue;
 		}

 		if (node->IsLeaf())
 		{
 			b2RayCastInput subInput;
 			subInput.p1 = input.p1;
 			subInput.p2 = input.p2;
 			subInput.maxFraction = maxFraction;

 			float32 value = callback->RayCastCallback(subInput, nodeId);

 			if (value == 0.0f)
 			{
 				// The client has terminated the ray cast.
 				return;
 			}

 			if (value > 0.0f)
 			{
 				// Update segment bounding box.
 				maxFraction = value;
 				b2Vec2 t = p1 + maxFraction * (p2 - p1);
 				segmentAABB.lowerBound = b2Min(p1, t);
 				segmentAABB.upperBound = b2Max(p1, t);
 			}
 		}
 		else
 		{
 			stack.Push(node->child1);
 			stack.Push(node->child2);
 		}
 	}
 }

 #endif
	/*
	* Copyright (c) 2009 Erin Catto http://www.box2d.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 B2_DYNAMIC_TREE_H
	#define B2_DYNAMIC_TREE_H

	#include <Box2D/Collision/b2Collision.h>
	#include <Box2D/Common/b2GrowableStack.h>

	#define b2_nullNode (-1)

	/// A node in the dynamic tree. The client does not interact with this directly.
	struct b2TreeNode
	{
	bool IsLeaf() const
	{
	return child1 == b2_nullNode;
	}

	/// Enlarged AABB
	b2AABB aabb;

	void* userData;

	union
	{
	int32 parent;
	int32 next;
	};

	int32 child1;
	int32 child2;

	// leaf = 0, free node = -1
	int32 height;
	};

	/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
	/// A dynamic tree arranges data in a binary tree to accelerate
	/// queries such as volume queries and ray casts. Leafs are proxies
	/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
	/// so that the proxy AABB is bigger than the client object. This allows the client
	/// object to move by small amounts without triggering a tree update.
	///
	/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
	class b2DynamicTree
	{
	public:
	/// Constructing the tree initializes the node pool.
	b2DynamicTree();

	/// Destroy the tree, freeing the node pool.
	~b2DynamicTree();

	/// Create a proxy. Provide a tight fitting AABB and a userData pointer.
	int32 CreateProxy(const b2AABB& aabb, void* userData);

	/// Destroy a proxy. This asserts if the id is invalid.
	void DestroyProxy(int32 proxyId);

	/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
	/// then the proxy is removed from the tree and re-inserted. Otherwise
	/// the function returns immediately.
	/// @return true if the proxy was re-inserted.
	bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);

	/// Get proxy user data.
	/// @return the proxy user data or 0 if the id is invalid.
	void* GetUserData(int32 proxyId) const;

	/// Get the fat AABB for a proxy.
	const b2AABB& GetFatAABB(int32 proxyId) const;

	/// Query an AABB for overlapping proxies. The callback class
	/// is called for each proxy that overlaps the supplied AABB.
	template <typename T>
	void Query(T* callback, const b2AABB& aabb) const;

	/// 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 input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
	/// @param callback a callback class that is called for each proxy that is hit by the ray.
	template <typename T>
	void RayCast(T* callback, const b2RayCastInput& input) const;

	/// Validate this tree. For testing.
	void Validate() const;

	/// Compute the height of the binary tree in O(N) time. Should not be
	/// called often.
	int32 GetHeight() const;

	/// Get the maximum balance of an node in the tree. The balance is the difference
	/// in height of the two children of a node.
	int32 GetMaxBalance() const;

	/// Get the ratio of the sum of the node areas to the root area.
	float32 GetAreaRatio() const;

	/// Build an optimal tree. Very expensive. For testing.
	void RebuildBottomUp();

	private:

	int32 AllocateNode();
	void FreeNode(int32 node);

	void InsertLeaf(int32 node);
	void RemoveLeaf(int32 node);

	int32 Balance(int32 index);

	int32 ComputeHeight() const;
	int32 ComputeHeight(int32 nodeId) const;

	void ValidateStructure(int32 index) const;
	void ValidateMetrics(int32 index) const;

	int32 m_root;

	b2TreeNode* m_nodes;
	int32 m_nodeCount;
	int32 m_nodeCapacity;

	int32 m_freeList;

	/// This is used to incrementally traverse the tree for re-balancing.
	uint32 m_path;

	int32 m_insertionCount;
	};

	inline void* b2DynamicTree::GetUserData(int32 proxyId) const
	{
	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
	return m_nodes[proxyId].userData;
	}

	inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
	{
	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
	return m_nodes[proxyId].aabb;
	}

	template <typename T>
	inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
	{
	b2GrowableStack<int32, 256> stack;
	stack.Push(m_root);

	while (stack.GetCount() > 0)
	{
	int32 nodeId = stack.Pop();
	if (nodeId == b2_nullNode)
	{
	continue;
	}

	const b2TreeNode* node = m_nodes + nodeId;

	if (b2TestOverlap(node->aabb, aabb))
	{
	if (node->IsLeaf())
	{
	bool proceed = callback->QueryCallback(nodeId);
	if (proceed == false)
	{
	return;
	}
	}
	else
	{
	stack.Push(node->child1);
	stack.Push(node->child2);
	}
	}
	}
	}

	template <typename T>
	inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
	{
	b2Vec2 p1 = input.p1;
	b2Vec2 p2 = input.p2;
	b2Vec2 r = p2 - p1;
	b2Assert(r.LengthSquared() > 0.0f);
	r.Normalize();

	// v is perpendicular to the segment.
	b2Vec2 v = b2Cross(1.0f, r);
	b2Vec2 abs_v = b2Abs(v);

	// Separating axis for segment (Gino, p80).
	// \|dot(v, p1 - c)\| > dot(\|v\|, h)

	float32 maxFraction = input.maxFraction;

	// Build a bounding box for the segment.
	b2AABB segmentAABB;
	{
	b2Vec2 t = p1 + maxFraction * (p2 - p1);
	segmentAABB.lowerBound = b2Min(p1, t);
	segmentAABB.upperBound = b2Max(p1, t);
	}

	b2GrowableStack<int32, 256> stack;
	stack.Push(m_root);

	while (stack.GetCount() > 0)
	{
	int32 nodeId = stack.Pop();
	if (nodeId == b2_nullNode)
	{
	continue;
	}

	const b2TreeNode* node = m_nodes + nodeId;

	if (b2TestOverlap(node->aabb, segmentAABB) == false)
	{
	continue;
	}

	// Separating axis for segment (Gino, p80).
	// \|dot(v, p1 - c)\| > dot(\|v\|, h)
	b2Vec2 c = node->aabb.GetCenter();
	b2Vec2 h = node->aabb.GetExtents();
	float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
	if (separation > 0.0f)
	{
	continue;
	}

	if (node->IsLeaf())
	{
	b2RayCastInput subInput;
	subInput.p1 = input.p1;
	subInput.p2 = input.p2;
	subInput.maxFraction = maxFraction;

	float32 value = callback->RayCastCallback(subInput, nodeId);

	if (value == 0.0f)
	{
	// The client has terminated the ray cast.
	return;
	}

	if (value > 0.0f)
	{
	// Update segment bounding box.
	maxFraction = value;
	b2Vec2 t = p1 + maxFraction * (p2 - p1);
	segmentAABB.lowerBound = b2Min(p1, t);
	segmentAABB.upperBound = b2Max(p1, t);
	}
	}
	else
	{
	stack.Push(node->child1);
	stack.Push(node->child2);
	}
	}
	}

	#endif