Box2d源码学习<六>动态树的实现

本系列博客是由扭曲45原创，欢迎转载，转载时注明出处，http://blog.csdn.net/cg0206/article/details/8293049

今天我们将学习碰撞模块（collision）部分，该部分主要有一下内容：

1、 形状，该部分包括的定义、实现。

2、 操作形状方法，该部分包括距离(Distance)、撞击时间(Time of Impact)等。

3、 算法辅助类，包括动态树和broad-phase算法。用来提高系统的碰撞检测的速度。

 

其中前两项均以算法辅助类为基础的，所以我们开始学习这一项，今天我们将学习动态树部分，动态树顾名思义就是“会动的树”，在Bx2d中动态树是由b2DynamicTree类来实现的，它主要是通过用户数据来操作轴对齐包围盒（axis-alignedbounding boxes,AABBs）来完成树的各种操作，并不需要知道形状是什么东东。(如有童鞋像我开始一样，不知道AABB是何物，请记得我们还有维基百科啦)。同时动态树继承自AABB树，树上的每一个节点都有两个孩子。叶节点是一个单独的用户AABB。即便是惰性输入，整个数也可以使用旋转保持平衡。

好了，我们开始上正餐了。先看b2DynameicTree.h文件：

//定义空节点
#define b2_nullNode (-1)
///一个动态树的子节点，不于外包直接交换
struct b2TreeNode
{
	//是否是叶子节点
	bool IsLeaf() const
	{
		return child1 == b2_nullNode;
	}

	/// Enlarged AABB
	//增大aabb变量
	b2AABB aabb;
	//用户数据
	void* userData;
	//父节点指针(索引)或孩子节点指针(索引)
	//因为此动态树是申请一块大的连续的空间，即含有n个元素的动态数组罢了
	//故用动态数组的索引值来模拟指针。为了统一，我们一下均使用指针
	union
	{
		int32 parent;
		int32 next;
	};
	//左右孩子指针
	int32 child1;
	int32 child2;
	//高度 叶子高度为0，空闲节点高度为-1
	int32 height;
};

//动态AABB树broad-phase,灵感来自于Nathanael Presson's btDbvt.
//一个动态树排列一个二叉树的数据来加速查询像体积查询和光线投射。叶子是若干个代理和轴对齐包围盒
//在树上我们用b2_fatAABBFactor扩展了aabb代理，这样aabb代理将比客户端对象大。这样允许客户端少量移动
//而不需更新一个树
//节点是汇集和浮动的，所以我们使用节点索引而不是指针
class b2DynamicTree
{
public:
	/**************************************************************************
	* 功能描述：构造一个树，并初始化节点内存池
	* 参数说明：(void)
	* 返 回 值：(void)
	***************************************************************************/
	b2DynamicTree();
	/**************************************************************************
	* 功能描述：销毁一个树，并释放节点内存池
	* 参数说明：(void)
	* 返 回 值：(void)
	***************************************************************************/
	~b2DynamicTree();
	/**************************************************************************
	* 功能描述：在树上创建一个叶子节点代理
	* 参数说明：aabb    ：aabb变量
	            user    : 数据
	* 返 回 值：节点的索引来替代指针,来增长我们的节点池
	***************************************************************************/
	int32 CreateProxy(const b2AABB& aabb, void* userData);
	/**************************************************************************
	* 功能描述：销毁一个代理，如果id无效则断言
	* 参数说明：poxyid :代理id
	* 返 回 值：节点的索引来替代指针,来增长我们的节点池
	***************************************************************************/
	void DestroyProxy(int32 proxyId);
	/**************************************************************************
	* 功能描述：移动一个代理并扫描AABB,如果代理移除了使它充实的AABB盒子
	            代理将会从树上移除并重新插入。否则，函数将立即返回
	* 参数说明：proxyId     ：叶子代理id
				aabb        : aabb变量
				displacement：移动坐标向量
	* 返 回 值： true :移动成功
	             false:移动失败
	***************************************************************************/
	bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
	/**************************************************************************
	* 功能描述：获取一个代理的userData
	* 参数说明：proxyId：叶子代理id
	* 返 回 值：id有效，则返回代理userData
	            id无效，则返0
	***************************************************************************/
	void* GetUserData(int32 proxyId) const;
	/**************************************************************************
	* 功能描述：获取从代理中宽大的AABB
	* 参数说明：proxyId：叶子代理id
	* 返 回 值：aabb对象
	***************************************************************************/
	const b2AABB& GetFatAABB(int32 proxyId) const;
	/**************************************************************************
	* 功能描述：查询一个aabb重叠代理，每个重叠提供AABB的代理都将回调回调类
	* 参数说明：callback ：回调对象
	            aabb     ：要查询的aabb
	* 返 回 值：aabb对象
	***************************************************************************/
	template <typename T>
	void Query(T* callback, const b2AABB& aabb) const;
	/**************************************************************************
	* 功能描述：光线投射到树上的每个代理上。
	            这依赖于回调去执行一个精确的光线投射到一个包含形状的代理上
				这个回调也执行任何碰撞过滤。
				性能几乎等于k * log(n),其中k是碰撞的次数，n是树上代理的数量
	* 参数说明：callback ：回调对象类，将会被每一个被光线照到的代理调用
	            input    ：光线投射输入数据，光线从 p1 + maxFraction * (p2 - p1)
	* 返 回 值：(void)
	***************************************************************************/
	template <typename T>
	void RayCast(T* callback, const b2RayCastInput& input) const;
	/**************************************************************************
	* 功能描述：验证这棵树，用于测试
	* 参数说明：(void)
	* 返 回 值：(void)
	***************************************************************************/
	void Validate() const;
	/**************************************************************************
	* 功能描述：在O(N)时间上计算二叉树的高度，要不经常调用
	* 参数说明：(void)
	* 返 回 值：二叉树的高度
	***************************************************************************/
	int32 GetHeight() const;
	/**************************************************************************
	* 功能描述：在树上获得节点之间最大的平衡。平衡是两个孩子节点最大的高度差
	* 参数说明：(void)
	* 返 回 值：平衡值
	***************************************************************************/
	int32 GetMaxBalance() const;
	/**************************************************************************
	* 功能描述：获得节点总数面积之和根面积的比
	* 参数说明：(void)
	* 返 回 值：节点总数面积之和根面积的比
	***************************************************************************/
	float32 GetAreaRatio() const;
	/**************************************************************************
	* 功能描述：构建一个最优的树，非常昂贵，用于测试
	* 参数说明：(void)
	* 返 回 值：节点总数面积之和根面积的比
	***************************************************************************/
	void RebuildBottomUp();

private:
	/**************************************************************************
	* 功能描述：从内存池中申请一个节点.如果必要增大内存池
	* 参数说明：(void)
	* 返 回 值：节点指针
	***************************************************************************/
	int32 AllocateNode();
	/**************************************************************************
	* 功能描述：释放节点
	* 参数说明：node :节点指针
	* 返 回 值：(void)
	***************************************************************************/
	void FreeNode(int32 node);
	/**************************************************************************
	* 功能描述：插入叶子节点
	* 参数说明：node：叶子节点指针
	* 返 回 值： (void)
	***************************************************************************/
	void InsertLeaf(int32 node);
	/**************************************************************************
	* 功能描述：移除叶子
	* 参数说明：node ：叶子节点指针
	* 返 回 值：(void)
	***************************************************************************/
	void RemoveLeaf(int32 node);
	/**************************************************************************
	* 功能描述：如果子树a不平衡，则执行一个向左或向右旋转
	* 参数说明：iA ：子树根节点指针
	* 返 回 值： 新的子树根指针
	***************************************************************************/
	int32 Balance(int32 index);
	/**************************************************************************
	* 功能描述：计算树的高度
	* 参数说明：(void)
	* 返 回 值：树的高度
	***************************************************************************/
	int32 ComputeHeight() const;
	/**************************************************************************
	* 功能描述：计算子树的高度
	* 参数说明：nodeid：子树的头指针
	* 返 回 值：子树的高度
	***************************************************************************/
	int32 ComputeHeight(int32 nodeId) const;
	/**************************************************************************
	* 功能描述：验证子树的结构
	* 参数说明：index:子树的头指针
	* 返 回 值：(void)
	***************************************************************************/
	void ValidateStructure(int32 index) const;
	/**************************************************************************
	* 功能描述：验证子树的度量
	* 参数说明：index:子树的头指针
	* 返 回 值：(void)
	***************************************************************************/
	void ValidateMetrics(int32 index) const;
	//树的根指针(也是索引)
	int32 m_root;
	//树的真正的头指针，也是一块连续的内存池的首地址
	b2TreeNode* m_nodes;
	//树节点的个数
	int32 m_nodeCount;
	//内存池中节点的总个数
	int32 m_nodeCapacity;
	//空闲链表指针
	int32 m_freeList;
	//  用于增量遍历树的调整
	uint32 m_path;
	//记录插入节点总数量
	int32 m_insertionCount;
};

我们可以看到该代码分为节点b2TreeNode定义和b2DynamicTree动态树类的定义，关于b2TreeNode结构体的定义就不多说了，而b2DynamicTree类主要还是先申请一块连续的内存用动态数组去模拟的（如果我们没记错的话，soa也是用动态数组实现的，两个栈的实现也是用的动态数组，好强大的动态数组），所以我们可以用索引模拟指针，直接去访问树上的节点。

 

我们再看看b2DynamicTree类有关内联函数的实现。

//根据代理id获取userData
inline void* b2DynamicTree::GetUserData(int32 proxyId) const
{
	//验证代理id的有效性
	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
	return m_nodes[proxyId].userData;
}
//根据代理id获得宽大的AABB
inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
{
	//验证代理id的有效性
	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
	return m_nodes[proxyId].aabb;
}

//查询一个aabb重叠代理，每个重叠提供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)
	{
		//获取节点id
		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垂直于向量r
	b2Vec2 v = b2Cross(1.0f, r);
	b2Vec2 abs_v = b2Abs(v);
	//分离轴 下面的书籍 p80 
	// 《Collision Detection in Interactive 3D Environments》 by Gino van den Bergen
	// 【从 http://download.csdn.net/detail/cg0206/4875309 下载】
	// 计算公式：|dot(v, p1 - c)| > dot(|v|, h)
	float32 maxFraction = input.maxFraction;
	// 构建一个绑定的盒子用于该线段
	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;
		//判断AABB
		if (b2TestOverlap(node->aabb, segmentAABB) == false)
		{
			continue;
		}

		//分离轴  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)
			{
				//客户端终止了光线投射
				return;
			}

			if (value > 0.0f)
			{
				// 更新线段的盒子边界
				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);
		}
	}
}

获取userData和AABB的，都是根据代理叶子id获取的节点信息。至于Query则是在树上查找每一个与AABB有重叠的叶子节点，并回调，效率比直接使用形状要高很多。

RayCast则用了大量的有关图形碰撞方面的知识和数学方面的计算，《CollisionDetection in Interactive 3D Environments》这本书有提到，本书目前没有找到中文版，对E文有兴趣的朋友可以从这里 下载。偶研究不深，也就不在这里误人子弟了。

 

我们再看看b2DynamicTree.ccp文件。

1、 构造、析构函数

/**************************************************************************
* 功能描述：动态树构造函数，初始化数据
* 参数说明：(void)
* 返 回 值：(void)
***************************************************************************/
b2DynamicTree::b2DynamicTree()
{
	m_root = b2_nullNode;

	m_nodeCapacity = 16;
	m_nodeCount = 0;
	//申请一块内存,创建m_nodeCapacity子节点,并清空内存中的内容
	m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
	memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
	// 创建一个空闲链表
	for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
	{
		m_nodes[i].next = i + 1;
		m_nodes[i].height = -1;
	}
	//链表的最后一个子节点的孩子指针、高度都置为初始值
	m_nodes[m_nodeCapacity-1].next = b2_nullNode;
	m_nodes[m_nodeCapacity-1].height = -1;
	m_freeList = 0;

	m_path = 0;

	m_insertionCount = 0;
}

/**************************************************************************
* 功能描述：析构函数
* 参数说明：(void)
* 返 回 值：(void)
***************************************************************************/
b2DynamicTree::~b2DynamicTree()
{
	//一次性释放整个内存池
	b2Free(m_nodes);
}

关于构造函数b2DynamicTree 主要是对类的成员初始化，并申请一块连续的内存，注意我们这次不是在自己实现的SOA（小型对象分配器）上面申请的，而是直接在内存中用b2Alloc()申请的。此块内存将作为节点内存池供树的节点使用。同时我们还看到有个for循环将所有的节点都遍历了一遍，又是干嘛呢？创建链表呗，看到注释的童鞋也许会这样回答，对的，那创建链表是干什么用的呢？嘿嘿……主要还是查找方便呗。关于析构函数主要是释放整个内存池。

 

2、对树中节点操作

/**************************************************************************
* 功能描述：从内存池中申请一个节点.如果必要增大内存池
* 参数说明：(void)
* 返 回 值：节点指针
***************************************************************************/
int32 b2DynamicTree::AllocateNode()
{
	// 如果需要扩大节点内存池
	if (m_freeList == b2_nullNode)
	{
		b2Assert(m_nodeCount == m_nodeCapacity);
		//空闲链表为空，重新创建一个更大的内存池
		b2TreeNode* oldNodes = m_nodes;
		m_nodeCapacity *= 2;
		m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
		memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
		b2Free(oldNodes);
		// 创建一个空闲链表。父节点成为下一个指针
		// 注意:这次是从m_nodeCount开始的
		for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
		{
			m_nodes[i].next = i + 1;
			m_nodes[i].height = -1;
		}

		m_nodes[m_nodeCapacity-1].next = b2_nullNode;
		m_nodes[m_nodeCapacity-1].height = -1;
		m_freeList = m_nodeCount;
	}
	//从空闲链表中去下一个节点，初始化该节点，
	//同时将空闲链表头指针m_freeList指向下一个
	int32 nodeId = m_freeList;
	m_freeList = m_nodes[nodeId].next;
	m_nodes[nodeId].parent = b2_nullNode;
	m_nodes[nodeId].child1 = b2_nullNode;
	m_nodes[nodeId].child2 = b2_nullNode;
	m_nodes[nodeId].height = 0;
	m_nodes[nodeId].userData = NULL;
	//增加节点数量
	++m_nodeCount;
	//返回节点id
	return nodeId;
}


//
/**************************************************************************
* 功能描述：从内存池中申请一个节点.根据nodeid将一个节点内存返回到节点池内
* 参数说明：nodeId：节点指针
* 返 回 值：(void)
***************************************************************************/
void b2DynamicTree::FreeNode(int32 nodeId)
{
	//验证nodeid的有效性
	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
	//验证是否有节点
	b2Assert(0 < m_nodeCount);
	//将当前节点以头插入的方式放到空闲链表中
	m_nodes[nodeId].next = m_freeList;
	m_nodes[nodeId].height = -1;
	m_freeList = nodeId;
	//将节点个数减1
	//注意此处并没有释放
	--m_nodeCount;
}

 

先看下面的图示:

动态数组m_nodes此时包含两部分，以m_root根的动态树部分用于管理动态树中的用户信息，和以m_freeList为头指针的空闲链表部分，用于管理内存池中未使用的节点。

对于AllocateNode函数，首先从空闲链表中获取其头部指针（索引）并返回，同时调整空闲链表的头指针到下一个节点。注意这里有一种情况，就是当空闲链表没有节点时，我们将重新申请一块是原来2倍内存空间的新的动态数组，并拷贝原来的信息到新的动态数组中，此处我们可以看到无论是m_root还是m_freeList，对其下属保存的都是索引，而非指针。这里拷贝到新的地方仍然有用，而指针则不然，在这里不得不佩服设计者的精妙之处。

对于FreeNode函数，我们将节点置空返回到空闲链表中，也并没有将其真正的释放，这也是很精妙的地方。

 

 

 

3、 对具有aabb代理的节点操作

/**************************************************************************
* 功能描述：在树上创建一个叶子节点代理
* 参数说明：aabb    ：aabb变量
            user    : 数据
* 返 回 值：节点的索引来替代指针,来增长我们的节点池
***************************************************************************/
int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
{
	//申请代理节点id
	int32 proxyId = AllocateNode();

	// Fatten the aabb.
	//填充aabb，为节点赋值
	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
	m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
	m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
	m_nodes[proxyId].userData = userData;
	m_nodes[proxyId].height = 0;
	//插入叶子节点
	InsertLeaf(proxyId);

	return proxyId;
}

/**************************************************************************
* 功能描述：销毁叶子代理
* 参数说明：proxyId     ：叶子代理id
* 返 回 值：(void)
***************************************************************************/
void b2DynamicTree::DestroyProxy(int32 proxyId)
{
	//验证proxyid的有效性
	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
	//验证是否是叶子节点
	b2Assert(m_nodes[proxyId].IsLeaf());
	//删除叶子节点
	RemoveLeaf(proxyId);
	//是否子节点
	FreeNode(proxyId);
}
/**************************************************************************
* 功能描述：移动叶子代理
* 参数说明：proxyId     ：叶子代理id
            aabb        : aabb变量
			displacement：移动坐标向量
* 返 回 值： (void)
***************************************************************************/
bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
{
	//验证proxyid的有效性
	b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
	//验证是否是叶子节点
	b2Assert(m_nodes[proxyId].IsLeaf());
	//检验aabb是否互相包含
	if (m_nodes[proxyId].aabb.Contains(aabb))
	{
		return false;
	}
	//根据proxyId移除叶子
	RemoveLeaf(proxyId);
	//扩大AABB
	b2AABB b = aabb;
	b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
	b.lowerBound = b.lowerBound - r;
	b.upperBound = b.upperBound + r;
	//预测aabb的位移
	b2Vec2 d = b2_aabbMultiplier * displacement;
	//扩大下限
	if (d.x < 0.0f)
	{
		b.lowerBound.x += d.x;
	}
	else
	{
		b.upperBound.x += d.x;
	}

	if (d.y < 0.0f)
	{
		b.lowerBound.y += d.y;
	}
	else
	{
		b.upperBound.y += d.y;
	}
	//重新设置aabb
	m_nodes[proxyId].aabb = b;
	//插入叶子节点
	InsertLeaf(proxyId);
	return true;
}

4、对叶子节点进行操作

/**************************************************************************
* 功能描述：插入叶子节点
* 参数说明：leaf    ：叶子节点指针
* 返 回 值： (void)
***************************************************************************/
void b2DynamicTree::InsertLeaf(int32 leaf)
{
	//插入节点总数量自增
	++m_insertionCount;
	//判断该树是否为空
	if (m_root == b2_nullNode)
	{
		m_root = leaf;
		m_nodes[m_root].parent = b2_nullNode;
		return;
	}
	//为该节点找到最好的兄弟（姐妹）节点
	//获取leaf的aabb
	b2AABB leafAABB = m_nodes[leaf].aabb;
	//获取根节点
	int32 index = m_root;
	//不是叶子节点
	while (m_nodes[index].IsLeaf() == false)
	{
		int32 child1 = m_nodes[index].child1;
		int32 child2 = m_nodes[index].child2;
		//获取aabb的周长
		float32 area = m_nodes[index].aabb.GetPerimeter();
		//获取
		b2AABB combinedAABB;
		combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
		float32 combinedArea = combinedAABB.GetPerimeter();
		//为该节点创建一个新的父节点【一个新叶子节点】
		float32 cost = 2.0f * combinedArea;
		// 最小cost推动叶子进一步下降的树的下一层
		float32 inheritanceCost = 2.0f * (combinedArea - area);
		//降低cost到child1
		float32 cost1;
		//左孩子是叶子节点
		if (m_nodes[child1].IsLeaf())
		{
			//获取cost1
			b2AABB aabb;
			aabb.Combine(leafAABB, m_nodes[child1].aabb);
			cost1 = aabb.GetPerimeter() + inheritanceCost;
		}
		else
		{
			//获取cost1
			b2AABB aabb;
			aabb.Combine(leafAABB, m_nodes[child1].aabb);
			float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
			float32 newArea = aabb.GetPerimeter();
			cost1 = (newArea - oldArea) + inheritanceCost;
		}
		//降低cost到child2
		float32 cost2;
		if (m_nodes[child2].IsLeaf())
		{
			//获取cost2
			b2AABB aabb;
			aabb.Combine(leafAABB, m_nodes[child2].aabb);
			cost2 = aabb.GetPerimeter() + inheritanceCost;
		}
		else
		{
			//获取cost2
			b2AABB aabb;
			aabb.Combine(leafAABB, m_nodes[child2].aabb);
			float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
			float32 newArea = aabb.GetPerimeter();
			cost2 = newArea - oldArea + inheritanceCost;
		}
		// 获取最小成本
		if (cost < cost1 && cost < cost2)
		{
			break;
		}
		//下降到最小cost
		if (cost1 < cost2)
		{
			index = child1;
		}
		else
		{
			index = child2;
		}
	}
	int32 sibling = index;
	//创建一个新的父节点
	//初始化
	int32 oldParent = m_nodes[sibling].parent;
	int32 newParent = AllocateNode();
	m_nodes[newParent].parent = oldParent;
	m_nodes[newParent].userData = NULL;
	m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
	m_nodes[newParent].height = m_nodes[sibling].height + 1;

	if (oldParent != b2_nullNode)
	{
		// 兄弟节点不是根节点
		if (m_nodes[oldParent].child1 == sibling)
		{
			m_nodes[oldParent].child1 = newParent;
		}
		else
		{
			m_nodes[oldParent].child2 = newParent;
		}

		m_nodes[newParent].child1 = sibling;
		m_nodes[newParent].child2 = leaf;
		m_nodes[sibling].parent = newParent;
		m_nodes[leaf].parent = newParent;
	}
	else
	{
		// 兄弟节点是根节点
		m_nodes[newParent].child1 = sibling;
		m_nodes[newParent].child2 = leaf;
		m_nodes[sibling].parent = newParent;
		m_nodes[leaf].parent = newParent;
		m_root = newParent;
	}
	// 向后走修复树的高度和aabb
	index = m_nodes[leaf].parent;
	while (index != b2_nullNode)
	{
		//平衡
		index = Balance(index);
		//左右孩子节点
		int32 child1 = m_nodes[index].child1;
		int32 child2 = m_nodes[index].child2;

		b2Assert(child1 != b2_nullNode);
		b2Assert(child2 != b2_nullNode);
		//获取高度和aabb
		m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
		m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
		//获取parent节点
		index = m_nodes[index].parent;
	}

	//Validate();
}

/**************************************************************************
* 功能描述：删除叶子节点
* 参数说明：leaf    ：叶子节点指针
* 返 回 值： (void)
***************************************************************************/
void b2DynamicTree::RemoveLeaf(int32 leaf)
{
	//只有一个节点
	if (leaf == m_root)
	{
		m_root = b2_nullNode;
		return;
	}
	//获取父节点和祖父节点
	int32 parent = m_nodes[leaf].parent;
	int32 grandParent = m_nodes[parent].parent;
	//选找兄弟节点
	int32 sibling;
	if (m_nodes[parent].child1 == leaf)
	{
		sibling = m_nodes[parent].child2;
	}
	else
	{
		sibling = m_nodes[parent].child1;
	}
	// 祖父节点不为空，即父节点不是根节点
	if (grandParent != b2_nullNode)
	{
		// Destroy parent and connect sibling to grandParent.
		// 销毁父节点和将兄弟节点链接到祖父节点中
		if (m_nodes[grandParent].child1 == parent)
		{
			m_nodes[grandParent].child1 = sibling;
		}
		else
		{
			m_nodes[grandParent].child2 = sibling;
		}
		m_nodes[sibling].parent = grandParent;
		//释放节点到内存池中
		FreeNode(parent);
		// 调整祖先界限
		int32 index = grandParent;
		while (index != b2_nullNode)
		{
			//平衡
			index = Balance(index);
			//获取左右孩子
			int32 child1 = m_nodes[index].child1;
			int32 child2 = m_nodes[index].child2;
			//合并aabb
			//高度
			m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
			m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
			//更新index
			index = m_nodes[index].parent;
		}
	}
	else
	{
		//获取根节点
		m_root = sibling;
		m_nodes[sibling].parent = b2_nullNode;
		//释放父节点
		FreeNode(parent);
	}

	//Validate();
}

我们首先来说说InsertLeaf函数

它有以下几种情况，如图：

以上就是动态树节点插入的几种情况，主要做法就是：

a)、通过遍历树上的节点，对比aabb找到代价最小的节点A

b)、将A作为兄弟节点，先建一个父节点N，将A的父节点的孩子指针指向N，同时N将A和要插入的叶子节点L作为左右孩子节点连接起来

c)、如果出现树不平衡，旋转动态树，使它成为新的平衡二叉树。大家可以看到，目前上图倒数第一和第二幅图上的都还不是平衡树，这部分等到说的平衡树的函数的时候再弄。

另外关于box2d中的动态树，我们来思考一些问题，就是我们插入的节点会始终是树的叶子节点吗？如果不是的那些节点又到哪去了呢？如果是的怎么做到的呢？

关于这个问题先卖个关子，我们再看看删除叶子函数RemoveLeaf，还是先看图：

关于删除函数，主要步骤是：

a)、根据索引找到父节点、祖父节点、和兄弟节点

b)、将祖父节点的原本指向父节点的孩子指针指向兄弟节点，并释放父亲节点

c)、如果出现树不平衡，旋转动态树，使它成为新的平衡二叉树。

还要注意一点的就是我们插入的有用的信息都是叶子节点，剩下的节点全部都是辅助节点。等我们看到重新构建一棵新的二叉树的时候会对这方面有新的认识。

 

相信大家对节点操作有一定的认识了吧，现在知道上面问题的答案了吗？其实，不管插入还是删除还是我们下面要说的使树平衡的函数，叶子节点还是叶子节点，丝毫没有变成其他节点，我们插入的有用的信息都是叶子节点，剩下的节点全部都是辅助节点，插入的时候添加父节点，删除的时候同时也删除了父节点。等我们看到重新构建一棵新的二叉树的时候会对这方面有新的认识。

5、 对树的操作

/**************************************************************************
* 功能描述：如果子树A不平衡，则执行一个向左或向右旋转
* 参数说明：iA   ：子树A
* 返 回 值： (void)
***************************************************************************/
int32 b2DynamicTree::Balance(int32 iA)
{
	//iA不是根节点
	b2Assert(iA != b2_nullNode);
	//获取子树A
	b2TreeNode* A = m_nodes + iA;
	//已是平衡树，不需要调整
	if (A->IsLeaf() || A->height < 2)
	{
		return iA;
	}
	// 获取A的左右孩子
	int32 iB = A->child1;
	int32 iC = A->child2;
	// iB、iC是否有效
	b2Assert(0 <= iB && iB < m_nodeCapacity);
	b2Assert(0 <= iC && iC < m_nodeCapacity);
	// 获取子树B、C 
	b2TreeNode* B = m_nodes + iB;
	b2TreeNode* C = m_nodes + iC;
	// 获得平衡值
	int32 balance = C->height - B->height;

	// 上旋C
	if (balance > 1)
	{
		//获取C的左右孩子iF、iG和子树F、G
		int32 iF = C->child1;
		int32 iG = C->child2;
		b2TreeNode* F = m_nodes + iF;
		b2TreeNode* G = m_nodes + iG;
		// 验证iF、iG是否有效
		b2Assert(0 <= iF && iF < m_nodeCapacity);
		b2Assert(0 <= iG && iG < m_nodeCapacity);
		// 交换A和C
		C->child1 = iA;
		C->parent = A->parent;
		A->parent = iC;
		// A的父指针应该指向c
		// A不是头节点
		if (C->parent != b2_nullNode)
		{
			if (m_nodes[C->parent].child1 == iA)
			{
				m_nodes[C->parent].child1 = iC;
			}
			else
			{
				b2Assert(m_nodes[C->parent].child2 == iA);
				m_nodes[C->parent].child2 = iC;
			}
		}
		else
		{
			//A是头节点
			m_root = iC;
		}
		// 旋转
		// 如果f的高度大，则旋转F
		if (F->height > G->height)
		{
			C->child2 = iF;
			A->child2 = iG;
			G->parent = iA;
			A->aabb.Combine(B->aabb, G->aabb);
			C->aabb.Combine(A->aabb, F->aabb);

			A->height = 1 + b2Max(B->height, G->height);
			C->height = 1 + b2Max(A->height, F->height);
		}
		else
		{
			// 旋转G
			C->child2 = iG;
			A->child2 = iF;
			F->parent = iA;
			A->aabb.Combine(B->aabb, F->aabb);
			C->aabb.Combine(A->aabb, G->aabb);

			A->height = 1 + b2Max(B->height, F->height);
			C->height = 1 + b2Max(A->height, G->height);
		}

		return iC;
	}
	
	// 上旋B
	if (balance < -1)
	{
		int32 iD = B->child1;
		int32 iE = B->child2;
		b2TreeNode* D = m_nodes + iD;
		b2TreeNode* E = m_nodes + iE;
		b2Assert(0 <= iD && iD < m_nodeCapacity);
		b2Assert(0 <= iE && iE < m_nodeCapacity);

		//交换A和B
		B->child1 = iA;
		B->parent = A->parent;
		A->parent = iB;
		// A的旧父指针指向B
		if (B->parent != b2_nullNode)
		{
			if (m_nodes[B->parent].child1 == iA)
			{
				m_nodes[B->parent].child1 = iB;
			}
			else
			{
				b2Assert(m_nodes[B->parent].child2 == iA);
				m_nodes[B->parent].child2 = iB;
			}
		}
		else
		{
			m_root = iB;
		}

		//旋转
		if (D->height > E->height)
		{
			// 旋转D
			B->child2 = iD;
			A->child1 = iE;
			E->parent = iA;
			A->aabb.Combine(C->aabb, E->aabb);
			B->aabb.Combine(A->aabb, D->aabb);

			A->height = 1 + b2Max(C->height, E->height);
			B->height = 1 + b2Max(A->height, D->height);
		}
		else
		{
			// 旋转E
			B->child2 = iE;
			A->child1 = iD;
			D->parent = iA;
			A->aabb.Combine(C->aabb, D->aabb);
			B->aabb.Combine(A->aabb, E->aabb);

			A->height = 1 + b2Max(C->height, D->height);
			B->height = 1 + b2Max(A->height, E->height);
		}

		return iB;
	}

	return iA;
}
/**************************************************************************
* 功能描述：获得树的高度
* 参数说明：(void)
* 返 回 值：树的高度
***************************************************************************/
int32 b2DynamicTree::GetHeight() const
{
	if (m_root == b2_nullNode)
	{
		return 0;
	}

	return m_nodes[m_root].height;
}

/**************************************************************************
* 功能描述：获得节点总数“面积”之和根“面积”的比
* 参数说明：(void)
* 返 回 值：树的高度
***************************************************************************/
float32 b2DynamicTree::GetAreaRatio() const
{
	//空树
	if (m_root == b2_nullNode)
	{
		return 0.0f;
	}
	//获取根子树
	const b2TreeNode* root = m_nodes + m_root;
	float32 rootArea = root->aabb.GetPerimeter();
	//获取所有节点的总“面积”，其实是周长
	float32 totalArea = 0.0f;
	for (int32 i = 0; i < m_nodeCapacity; ++i)
	{
		const b2TreeNode* node = m_nodes + i;
		if (node->height < 0)
		{
			//内存池内的空闲节点
			continue;
		}

		totalArea += node->aabb.GetPerimeter();
	}
	//获取比率
	return totalArea / rootArea;
}
/**************************************************************************
* 功能描述：计算子树的高度
* 参数说明：nodeid：子树的头指针索引
* 返 回 值：子树的高度
***************************************************************************/
int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
{
	//验证子树的头指针索引
	b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
	//获取子树头节点
	b2TreeNode* node = m_nodes + nodeId;
	// 是否是叶子
	if (node->IsLeaf())
	{
		return 0;
	}
	//递给调用，返回高度
	int32 height1 = ComputeHeight(node->child1);
	int32 height2 = ComputeHeight(node->child2);
	return 1 + b2Max(height1, height2);
}/**************************************************************************
* 功能描述：计算树的高度
* 参数说明：(void)
* 返 回 值：树的高度
***************************************************************************/
int32 b2DynamicTree::ComputeHeight() const
{
	int32 height = ComputeHeight(m_root);
	return height;
}
/**************************************************************************
* 功能描述：验证子树的结构
* 参数说明：index:子树的索引值
* 返 回 值：(void)
***************************************************************************/
void b2DynamicTree::ValidateStructure(int32 index) const
{
	//空树
	if (index == b2_nullNode)
	{
		return;
	}
	//子树是整个树
	if (index == m_root)
	{
		//验证有效性
		b2Assert(m_nodes[index].parent == b2_nullNode);
	}
	//获取子树的头指针
	const b2TreeNode* node = m_nodes + index;
	//获取左右孩子
	int32 child1 = node->child1;
	int32 child2 = node->child2;
	//node是否是叶子节点
	if (node->IsLeaf())
	{
		//验证左右孩子和高度
		b2Assert(child1 == b2_nullNode);
		b2Assert(child2 == b2_nullNode);
		b2Assert(node->height == 0);
		return;
	}
	//验证左右孩子
	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
	//验证左右孩子的父节点
	b2Assert(m_nodes[child1].parent == index);
	b2Assert(m_nodes[child2].parent == index);
	//递归验证左右孩子的结构
	ValidateStructure(child1);
	ValidateStructure(child2);
}
/**************************************************************************
* 功能描述：验证子树的度量值
* 参数说明：index:子树的索引值
* 返 回 值：(void)
***************************************************************************/
void b2DynamicTree::ValidateMetrics(int32 index) const
{
	//空子树
	if (index == b2_nullNode)
	{
		return;
	}
	//子树的头指针
	const b2TreeNode* node = m_nodes + index;
	//获取左右孩子
	int32 child1 = node->child1;
	int32 child2 = node->child2;
	//子树是否是叶子节点
	if (node->IsLeaf())
	{
		//验证左右孩子和高度
		b2Assert(child1 == b2_nullNode);
		b2Assert(child2 == b2_nullNode);
		b2Assert(node->height == 0);
		return;
	}
	//验证左右孩子
	b2Assert(0 <= child1 && child1 < m_nodeCapacity);
	b2Assert(0 <= child2 && child2 < m_nodeCapacity);
	//验证高度
	int32 height1 = m_nodes[child1].height;
	int32 height2 = m_nodes[child2].height;
	int32 height;
	height = 1 + b2Max(height1, height2);
	b2Assert(node->height == height);
	//验证aabb
	b2AABB aabb;
	aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);

	b2Assert(aabb.lowerBound == node->aabb.lowerBound);
	b2Assert(aabb.upperBound == node->aabb.upperBound);
	//递归验证左右孩子子树
	ValidateMetrics(child1);
	ValidateMetrics(child2);
}

/**************************************************************************
* 功能描述：验证这棵树，用于测试
* 参数说明：(void)
* 返 回 值：(void)
***************************************************************************/
void b2DynamicTree::Validate() const
{
	//验证这棵树的结构和度量
	ValidateStructure(m_root);
	ValidateMetrics(m_root);
	//遍历空闲链表，计算空闲节点的个数
	int32 freeCount = 0;
	int32 freeIndex = m_freeList;
	while (freeIndex != b2_nullNode)
	{
		b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
		freeIndex = m_nodes[freeIndex].next;
		++freeCount;
	}
	//验证高度
	b2Assert(GetHeight() == ComputeHeight());
	//验证内存池中的节点总容量
	b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
}
/**************************************************************************
* 功能描述：获取平衡值
* 参数说明：获取最大的平衡值
* 返 回 值：(void)
***************************************************************************/
int32 b2DynamicTree::GetMaxBalance() const
{
	//
	int32 maxBalance = 0;
	for (int32 i = 0; i < m_nodeCapacity; ++i)
	{
		const b2TreeNode* node = m_nodes + i;
		// 内存池中的空闲节点
		if (node->height <= 1)
		{
			continue;
		}

		b2Assert(node->IsLeaf() == false);
		//获取最大平衡值
		int32 child1 = node->child1;
		int32 child2 = node->child2;
		int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
		maxBalance = b2Max(maxBalance, balance);
	}

	return maxBalance;
}
/**************************************************************************
* 功能描述：构建一个最优的树，非常昂贵，用于测试
* 参数说明：(void)
* 返 回 值：(void)
***************************************************************************/
void b2DynamicTree::RebuildBottomUp()
{
	//从系统堆中申请一段内存
	int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
	int32 count = 0;
	//创建空闲的叶子数组。其余是空闲的
	for (int32 i = 0; i < m_nodeCapacity; ++i)
	{
		if (m_nodes[i].height < 0)
		{
			// 内存池中空闲的节点
			continue;
		}
		// 是否是叶子节点
		if (m_nodes[i].IsLeaf())
		{
			
			m_nodes[i].parent = b2_nullNode;
			nodes[count] = i;
			++count;
		}
		else
		{
			// 不是则释放到内存池中
			FreeNode(i);
		}
	}
	// 叶子节点的个数
	while (count > 1)
	{
		//最小cost
		float32 minCost = b2_maxFloat;
		int32 iMin = -1, jMin = -1;
		//获取最小(j)和第二小(i)的cost
		for (int32 i = 0; i < count; ++i)
		{
			b2AABB aabbi = m_nodes[nodes[i]].aabb;
			for (int32 j = i + 1; j < count; ++j)
			{
				b2AABB aabbj = m_nodes[nodes[j]].aabb;
				b2AABB b;
				b.Combine(aabbi, aabbj);
				float32 cost = b.GetPerimeter();
				//获取最小的cost
				if (cost < minCost)
				{
					iMin = i;
					jMin = j;
					minCost = cost;
				}
			}
		}
		//获取左右孩子节点和左右子树
		int32 index1 = nodes[iMin];
		int32 index2 = nodes[jMin];
		b2TreeNode* child1 = m_nodes + index1;
		b2TreeNode* child2 = m_nodes + index2;
		//申请父子树索引
		int32 parentIndex = AllocateNode();
		//获取父子树节点
		b2TreeNode* parent = m_nodes + parentIndex;
		parent->child1 = index1;
		parent->child2 = index2;
		parent->height = 1 + b2Max(child1->height, child2->height);
		parent->aabb.Combine(child1->aabb, child2->aabb);
		parent->parent = b2_nullNode;
		//
		child1->parent = parentIndex;
		child2->parent = parentIndex;
		//覆盖最小aabb节点
		nodes[jMin] = nodes[count-1];
	    //将第二小aabb节点用父节点覆盖
		nodes[iMin] = parentIndex;
		--count;
	}
	//获取跟节点
	m_root = nodes[0];
	//释放节点
	b2Free(nodes);
	//验证这棵树，用于测试
	Validate();
}

这部分，我们主要来看一看Blance函数和RebuildBottomUp函数，Blance函数主要是调节树的平衡，使之成为平衡二叉树。主要步骤是：

a)、获取当前节点的两个孩子节点，比对孩子子树B、C的高度，求的高度差b

b)、若b不在[-1,1]之间，则上旋孩子子树较高的（这里假设是B），将父节点作为子树B的一个孩子，同时树的根节点指针m_root指向B节点。

c)、然后在B子树上进行a、b操作，直到叶子节点。同时返回新的根节点

我们就用上图中插入时形成的动态树但还没有进行平衡处理的树进行调整，如图：

哈哈，一棵平衡树就这样就成了，当然这个步骤不复杂，但是原理是一样的，就不介绍了。注意，我们可以看到，平衡操作后，叶子节点依然是叶子节点，我们所需要的信息还是在那里面。

下面我们说说RebuildBottomUp这个函数，它主要重新构建一棵树。

主要步骤是：

a)、获取所有叶子节点放入动态数组中，其他释放到叶子节点内存池中

b)、获取所有叶子节点中aabb最小的两个，申请一个节点，作为它们的父节点，形成一个新的子树

c)、用动态数组最后一个节点覆盖最小aabb的节点，用父节点放入动态数组aabb第二小的位置上。同时将动态数组中的节点个数减少一个。

d)、重复a、b、c步骤，直到动态数组中的节点个数为1个。

e)、获取头指针。

 

今天的动态树部分就说到这了，如有本人理解不妥或错误之处，望大家多多指正，同时也希望和大家多多交流。