Box2d源码学习<十四>TOI之碰撞时间的实现

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

TOI全称Time of Impact，中文的意思是撞击时间，在Box2d中，我们用b2TimeOfImpact来确定两个形状运动时的撞击时间(TOI)。同时b2TimeOfImpact也主要防止两个形状快速移动时可能在一个时间步内彼此穿越对方的情况，也就是我们经常所说的隧道效应。

我们就一起看源码吧。

1)、b2TimeOfImpact.h文件。

// b2TimeOfImpace的输入参数
struct b2TOIInput
{
	b2DistanceProxy proxyA;  //距离代理A
	b2DistanceProxy proxyB;  //距离代理B
	b2Sweep sweepA;          //扫描A
	b2Sweep sweepB;          //扫描B
	float32 tMax;		     //定义扫频间隔 [0, tMax]
};

//b2TimeOfImpact的输出参数
struct b2TOIOutput
{
	enum State
	{
		e_unknown,      //未知
		e_failed,       //失败
		e_overlapped,   //重叠
		e_touching,     //触碰
		e_separated     //分离
	};

	State state;       //状态
	float32 t;         //扫频间隔
};
/**************************************************************************
* 功能描述：在两个形状穿透之前，及时的求出上边界。用分数表示时间
            在[0,tMax]之间。它使用扫频分离轴和可能丢失一些像非隧道效应碰撞的
			中间体，如果你改变时间间隔，你需要重新调用这个函数
			注意：使用b2Distance去求在一个撞击时间内的接触点和法线
* 参数说明：output：TOI输出参数指针
	        input ：TOI输入参数指针
* 返 回 值： (void)
**************************************************************************/
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);

我们可以看到此处定义了用于保存TOI信息的结构体，分别是b2TOIInput、b2TOIOutput结构体，表示碰撞时间的输入和输出参数。对于b2TimeOfImpact函数，则是这篇文章的主角，用于防止两物体之间的隧道效应，关于此函数的具体情况，等到实现的时候在详细的和大家聊聊。

 

2)、b2TimeOfImpact.cpp文件。

我们再来看看b2TimeOfImpact.cpp文件。为了更好的看源码，将分成以下三点：

1. 全局变量的定义
2. b2SeparationFunction结构体的实现
3. b2TimeOfImpact函数的实现

 

1、全局变量的定义

int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;   //调用次数、toi的迭代次数、toi的最大迭代次数（两层循环中取最大的那个）
int32 b2_toiRootIters, b2_toiMaxRootIters;        //根总共迭代次数、在所有根迭代中最大的那次

2、b2SeparationFunction结构体的实现

struct b2SeparationFunction
{
	enum Type
	{
		e_points,           //点
		e_faceA,            //面A
		e_faceB             //面B
	};
	/**************************************************************************
	* 功能描述：如果不需要就返回间距值
	* 参数说明：cache ：单纯形缓存指针
	            proxyA：多边形A的指针
				sweepA：扫频对象的引用
				proxyB：多边形B的指针
				sweepB：扫频对象的引用
				t1    ：扫频间隔
	* 返 回 值： 间距值
	**************************************************************************/
	float32 Initialize(const b2SimplexCache* cache,
		const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
		const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
		float32 t1)
	{
		//赋值代理
		m_proxyA = proxyA;
		m_proxyB = proxyB;
		// 获取缓存中的顶点数，并验证
		int32 count = cache->count;
		b2Assert(0 < count && count < 3);
		//赋值扫频
		m_sweepA = sweepA;
		m_sweepB = sweepB;
		//获取变换
		b2Transform xfA, xfB;
		m_sweepA.GetTransform(&xfA, t1);
		m_sweepB.GetTransform(&xfB, t1);
		//一个顶点
		if (count == 1)
		{
			//赋值，获得A、B的局部顶点
			m_type = e_points;
			b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
			//获取变换后的A、B点
			b2Vec2 pointA = b2Mul(xfA, localPointA);
			b2Vec2 pointB = b2Mul(xfB, localPointB);
			//获取从B到的A的向量，返回其长度，并标准化
			m_axis = pointB - pointA;
			float32 s = m_axis.Normalize();
			return s;
		}
		else if (cache->indexA[0] == cache->indexA[1])
		{
			// 两个点在B上和一个在A上
			//赋值，获取B上的两个局部顶点
			m_type = e_faceB;
			b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
			b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
			//获取B2到B1形成向量的垂直向量，并标准化
			m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
			m_axis.Normalize();
			//获取法向量
			b2Vec2 normal = b2Mul(xfB.q, m_axis);
			// 获取B1到B2的中间点
			m_localPoint = 0.5f * (localPointB1 + localPointB2);
			b2Vec2 pointB = b2Mul(xfB, m_localPoint);
			// 获取局部点A，并求得点A
			b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
			b2Vec2 pointA = b2Mul(xfA, localPointA);
			// 获取距离
			float32 s = b2Dot(pointA - pointB, normal);
			// 距离为负，置反
			if (s < 0.0f)
			{
				m_axis = -m_axis;
				s = -s;
			}
			return s;
		}
		else
		{
			// 两个点在A上和一个或者两个点在B上
			m_type = e_faceA;
			b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
			b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
			//获取A2到A1形成向量的垂直向量，并标准化
			m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
			m_axis.Normalize();
			//获取法向量
			b2Vec2 normal = b2Mul(xfA.q, m_axis);
			//获取A1和A2的中间点
			m_localPoint = 0.5f * (localPointA1 + localPointA2);
			b2Vec2 pointA = b2Mul(xfA, m_localPoint);
			//获取局部点，并求得点B
			b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
			b2Vec2 pointB = b2Mul(xfB, localPointB);
			//获取距离，并处理
			float32 s = b2Dot(pointB - pointA, normal);
			if (s < 0.0f)
			{
				m_axis = -m_axis;
				s = -s;
			}
			return s;
		}
	}
	/**************************************************************************
	* 功能描述：寻找最小距离
	* 参数说明：indexA ：点A的索引
	            indexB ：点B的索引
				t      ：时间值
	* 返 回 值： 最小距离
	**************************************************************************/
	float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
	{
		//声明变换A、B，用于获取在t时间里获得窜改变换
		b2Transform xfA, xfB;
		m_sweepA.GetTransform(&xfA, t);
		m_sweepB.GetTransform(&xfB, t);
		//处理不同的类型
		switch (m_type)
		{
		case e_points:                                //点
			{
				//通过转置旋转m_axis获取单纯形支撑点的方向向量
				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
				//通过方向向量获取局部顶点的索引
				*indexA = m_proxyA->GetSupport(axisA);
				*indexB = m_proxyB->GetSupport(axisB);
				//通过索引获取局部顶点
				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
				//通过变换局部点获取两形状之间的顶点
				b2Vec2 pointA = b2Mul(xfA, localPointA);
				b2Vec2 pointB = b2Mul(xfB, localPointB);
				//求两形状的间距，并返回。
				float32 separation = b2Dot(pointB - pointA, m_axis);
				return separation;
			}

		case e_faceA:                              //面A
			{
				//通过转置旋转m_axis获取单纯形支撑点的方向向量
				//通过变换局部点获取当前图形的点
				b2Vec2 normal = b2Mul(xfA.q, m_axis);
				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
				//通过转置旋转m_axis获取单纯形支撑点的方向向量
				b2Vec2 axisB = b2MulT(xfB.q, -normal);
				//通过索引获取局部顶点
				*indexA = -1;
				*indexB = m_proxyB->GetSupport(axisB);
				//通过变换局部点获形状B的顶点
				b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
				b2Vec2 pointB = b2Mul(xfB, localPointB);
				//求两形状的间距，并返回。
				float32 separation = b2Dot(pointB - pointA, normal);
				return separation;
			}

		case e_faceB:                             //面B
			{
				//通过转置旋转m_axis获取单纯形支撑点的方向向量
				//通过变换局部点获取当前图形的点
				b2Vec2 normal = b2Mul(xfB.q, m_axis);
				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
				//通过转置旋转m_axis获取单纯形支撑点的方向向量
				b2Vec2 axisA = b2MulT(xfA.q, -normal);
				//通过索引获取局部顶点
				*indexB = -1;
				*indexA = m_proxyA->GetSupport(axisA);
				//通过变换局部点获形状A的顶点
				b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
				b2Vec2 pointA = b2Mul(xfA, localPointA);
				//求两形状的间距，并返回。
				float32 separation = b2Dot(pointA - pointB, normal);
				return separation;
			}

		default:
			b2Assert(false);
			*indexA = -1;
			*indexB = -1;
			return 0.0f;
		}
	}
	/**************************************************************************
	* 功能描述：当前时间步里两形状的距离
	* 参数说明：indexA ：点A的索引
	            indexB ：点B的索引
				t      ：时间值
	* 返 回 值： 当前时间步里两形状的距离
	**************************************************************************/
	float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
	{
		b2Transform xfA, xfB;
		m_sweepA.GetTransform(&xfA, t);
		m_sweepB.GetTransform(&xfB, t);

		switch (m_type)
		{
		case e_points:                                //点
			{
				//通过转置旋转m_axis获取顶点的方向向量
				b2Vec2 axisA = b2MulT(xfA.q,  m_axis);
				b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
				//通过变换局部点获形状A、B的顶点
				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
				//获取当前时间步内的两形状上的点
				b2Vec2 pointA = b2Mul(xfA, localPointA);
				b2Vec2 pointB = b2Mul(xfB, localPointB);
				//计算间距，并返回间距
				float32 separation = b2Dot(pointB - pointA, m_axis);
				return separation;
			}

		case e_faceA:                                 //面A
			{
				//旋转m_axis向量，获取法向量，同时根据局部点求形状A上的点
				b2Vec2 normal = b2Mul(xfA.q, m_axis);
				b2Vec2 pointA = b2Mul(xfA, m_localPoint);
				//通过转置旋转m_axis获取单纯形支撑点的方向向量
				b2Vec2 axisB = b2MulT(xfB.q, -normal);
				//通过索引获取局部顶点，进而通过变换局部点获取当前时间步内的点
				b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
				b2Vec2 pointB = b2Mul(xfB, localPointB);
				//获取间距
				float32 separation = b2Dot(pointB - pointA, normal);
				return separation;
			}

		case e_faceB:                                 //面B
			{
				//旋转m_axis向量，获取法向量，同时根据局部点求形状B上的点
				b2Vec2 normal = b2Mul(xfB.q, m_axis);
				b2Vec2 pointB = b2Mul(xfB, m_localPoint);
				//通过转置旋转m_axis获取单纯形支撑点的方向向量
				b2Vec2 axisA = b2MulT(xfA.q, -normal);
				//通过索引获取局部顶点，进而通过变换局部点获取当前时间步内的点
				b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
				b2Vec2 pointA = b2Mul(xfA, localPointA);
				//获取间距
				float32 separation = b2Dot(pointA - pointB, normal);
				return separation;
			}

		default:
			b2Assert(false);
			return 0.0f;
		}
	}

	const b2DistanceProxy* m_proxyA;          //代理A
	const b2DistanceProxy* m_proxyB;          //代理B
	b2Sweep m_sweepA, m_sweepB;               //扫描A、B
	Type m_type;                              //类型变量
	b2Vec2 m_localPoint;                      //局部点
	b2Vec2 m_axis;                            //方向向量，主要用于变换次向量之后求形状的顶点
};

关于b2SeparationFunction结构体主要用于查找两个形状间距的相关操作。我们主要来说说其内部函数的实现。

关于Initialize函数主要初始化成员变量，并返回两个形状之间的距离。

关于FindMinSeparation函数主要是根据不同的单纯形类型在时间步内寻找最小距离，并返回其两个顶点的索引，作为两形状是否碰撞的见证点。

关于Evaluate函数主要是根据不同的单纯形类型和FindMinSeparation所查到的见证点获取当前两形状的距离。

 

3、 b2TimeOfImpact函数的实现

//CCD（continuous collision detection，持续碰撞检验）经过局部的分离轴方法。
//这种寻求进展通过计算最大的时间保持分离。
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
{
	//调用次数自加
	++b2_toiCalls;
	//赋值output
	output->state = b2TOIOutput::e_unknown;
	output->t = input->tMax;
	//获取距离代理
	const b2DistanceProxy* proxyA = &input->proxyA;
	const b2DistanceProxy* proxyB = &input->proxyB;
	//获取扫频
	b2Sweep sweepA = input->sweepA;
	b2Sweep sweepB = input->sweepB;
	// 大型旋转可以使根检索器失效，所以我们标准化扫频角度
	sweepA.Normalize();
	sweepB.Normalize();
	//获取扫频间隔
	float32 tMax = input->tMax;
	//获取两个形状半径之和
	float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
	float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
	//允许误差
	float32 tolerance = 0.25f * b2_linearSlop;
	//验证有效值
	b2Assert(target > tolerance);

	float32 t1 = 0.0f;
	//最大迭代次数
	const int32 k_maxIterations = 20;	// TODO_ERIN b2Settings
	//
	int32 iter = 0;
	// 初始化距离输入参数
	b2SimplexCache cache;
	cache.count = 0;
	b2DistanceInput distanceInput;
	distanceInput.proxyA = input->proxyA;
	distanceInput.proxyB = input->proxyB;
	distanceInput.useRadii = false;
	// 外面的循环逐步尝试计算新的分离轴
	// 当一个轴是重复的(没有进展)，这个循环终止
	for(;;)
	{
		b2Transform xfA, xfB;
		sweepA.GetTransform(&xfA, t1);
		sweepB.GetTransform(&xfB, t1);
		// 获取形状之间的距离。我们也可以使用这个结果去获得一个分离轴
		distanceInput.transformA = xfA;
		distanceInput.transformB = xfB;
		b2DistanceOutput distanceOutput;
		b2Distance(&distanceOutput, &cache, &distanceInput);
		// 如果形状重叠，我们放弃连续碰撞
		if (distanceOutput.distance <= 0.0f)
		{
			//失败！
			output->state = b2TOIOutput::e_overlapped;
			output->t = 0.0f;
			break;
		}

		if (distanceOutput.distance < target + tolerance)
		{
			//胜利！
			output->state = b2TOIOutput::e_touching;
			output->t = t1;
			break;
		}
		// 初始化分离轴
		b2SeparationFunction fcn;
		fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
#if 0
		// Dump the curve seen by the root finder
		{
			const int32 N = 100;
			float32 dx = 1.0f / N;
			float32 xs[N+1];
			float32 fs[N+1];

			float32 x = 0.0f;

			for (int32 i = 0; i <= N; ++i)
			{
				sweepA.GetTransform(&xfA, x);
				sweepB.GetTransform(&xfB, x);
				float32 f = fcn.Evaluate(xfA, xfB) - target;

				printf("%g %g\n", x, f);

				xs[i] = x;
				fs[i] = f;

				x += dx;
			}
		}
#endif
		//在分离轴上计算TOI（碰撞时间），我们先后解决最深处的点。这个循环是以顶点数为终止条件的
		bool done = false;
		float32 t2 = tMax;
		int32 pushBackIter = 0;
		for (;;)
		{
			// 在t2上查找最深点，存储见证点索引
			int32 indexA, indexB;
			float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
			// 是否是最终的外形分离
			if (s2 > target + tolerance)
			{
				//胜利！
				output->state = b2TOIOutput::e_separated;
				output->t = tMax;
				done = true;
				break;
			}
			//分离值是否达到误差值
			if (s2 > target - tolerance)
			{
				// 推进扫描
				t1 = t2;
				break;
			}
			// 使用见证点计算最初的间距
			float32 s1 = fcn.Evaluate(indexA, indexB, t1);
			// 检验最初重叠。有可能发生根检索器超出了迭代总的次数的现象。
			if (s1 < target - tolerance)
			{
				output->state = b2TOIOutput::e_failed;
				output->t = t1;
				done = true;
				break;
			}
			// 检查触碰
			if (s1 <= target + tolerance)
			{
				// 胜利！t1必须保留TOI(只有可能是0)
				output->state = b2TOIOutput::e_touching;
				output->t = t1;
				done = true;
				break;
			}
			//计算1D root ： f(x) - target = 0
			int32 rootIterCount = 0;
			float32 a1 = t1, a2 = t2;
			for (;;)
			{
				// 混合使用割线规则和二分法
				float32 t;
				if (rootIterCount & 1)
				{
					// 割线规则来提高收敛
					t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
				}
				else
				{
					// 二分法保证进度
					t = 0.5f * (a1 + a2);
				}

				float32 s = fcn.Evaluate(indexA, indexB, t);

				if (b2Abs(s - target) < tolerance)
				{
					// 赋值
					t2 = t;
					break;
				}
				// 确保我们查找根
				if (s > target)
				{
					a1 = t;
					s1 = s;
				}
				else
				{
					a2 = t;
					s2 = s;
				}
				//根迭代器
				++rootIterCount;
				++b2_toiRootIters;
				// 循环到达50次后，退出
				if (rootIterCount == 50)
				{
					break;
				}
			}
			
			b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
			//记录顶点迭代器
			++pushBackIter;
			//达到顶点的最大次数，退出
			if (pushBackIter == b2_maxPolygonVertices)
			{
				break;
			}
		}
		//根迭代器
		++iter;
		//toi的迭代次数自增
		++b2_toiIters;

		if (done)
		{
			break;
		}

		if (iter == k_maxIterations)
		{
			//没有找到根
			output->state = b2TOIOutput::e_failed;
			output->t = t1;
			break;
		}
	}
	//获取toi最大迭代器
	b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
}

关于b2TimeOfImpact函数，主要以3重for循环为主线的，第一层for循环主要是逐步尝试计算新的分离轴，并当出现一个轴是重复的时，终止循环。第二层for循环主要是在分离轴上计算TOI（碰撞时间），我们先后解决最深处的点。这个循环是以顶点数为终止条件的。第三层for循环主要使用割线规则和二分法进行求解在t时间内，两物体碰撞的具体的时间值。这个循环是以找到在误差允许的范围内的时间值或者循环50次为终止条件的。

另外想说一下，在这里我们每个循环的写法是for(;;)这样的，个人感觉不太雅致，也不能看一眼而不用思索的就知道是死循环的写法，如改成while(true)或者while(1)更好。

关于两物体间是否碰撞了？在Box2d中目前我们至少知道3种可以判断的方法，它们分别是：

• a)、通过两物体的aabb，判断是否重叠。
• b)、通过GJK算法算出两物体间的距离，根据距离判断是否碰撞
• c)、通过SAT分离轴算法看是否能找出两物体间的分离轴，如果找得出就没有碰撞，找不出则碰撞。

 

Ok，碰撞部分终于学完了，下面我们将继续学习动力学部分。不早了，各位早安。。。

ps：

 

以上文章仅是一家之言，若有不妥、错误之处，请大家多多指出。同时也希望能与大家多多交流，共同进步。