Box2d源码学习<十一>GJK之距离的实现

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

Box2d中距离是指两个形状最近点之间的距离，主要用于形状的碰撞检测，通过GJK算法实现，在GJK中又使用voroni区域算法和重心坐标来完成的。在Box2d最终调用b2Ditance方法来求得距离。使用此方法需要将两个形状转换成一个b2DistanceProxy。为了提供高内部调用的效率，在外部开始调用b2Distance方法时，内部做了一个缓冲，用来提高计算距离的效率。关于GJK、voronic区域算法和重心坐标，大家可以点击对应的红色字体。

我们将不在文章中分析这些算法了，感兴趣的童鞋自行深入的研究。不罗嗦了，我们来看看它的定义，上代码：

//声明类
class b2Shape;
// 距离代理，使用GJK算法实现
// 它封装了任何形状
struct b2DistanceProxy
{
	/**************************************************************************
	* 功能描述：构造函数，初始化信息
	* 参数说明： (void)
	* 返 回 值： (void)
	***************************************************************************/
	b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {}
	/**************************************************************************
	* 功能描述：用给定的形状初始化代理。当使用代理时，形状必须保持在一定的范围内
	* 参数说明： shape：形状指针变量
	             index：索引值
	* 返 回 值： (void)
	***************************************************************************/
	void Set(const b2Shape* shape, int32 index);
	/**************************************************************************
	* 功能描述：根据指定的方向获得支撑点的索引
	* 参数说明： d:向量的引用
	* 返 回 值： 顶点的索引
	***************************************************************************/
	int32 GetSupport(const b2Vec2& d) const;
	/**************************************************************************
	* 功能描述：根据指定的方向获得支撑点
	* 参数说明： d:向量的引用
	* 返 回 值： 顶点坐标
	***************************************************************************/
	const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
	/**************************************************************************
	* 功能描述：获取顶点个数
	* 参数说明： (void)
	* 返 回 值： 顶点的个数
	***************************************************************************/
	int32 GetVertexCount() const;
	/**************************************************************************
	* 功能描述：根据索引获得顶点
	             用于b2Distance
	* 参数说明： index：索引
	* 返 回 值： 顶点
	***************************************************************************/
	const b2Vec2& GetVertex(int32 index) const;
	//顶点缓存，用于保存chain形状中的两个顶点
	b2Vec2 m_buffer[2];
	//坐标点
	const b2Vec2* m_vertices;
	//顶点的数量
	int32 m_count;
	//半径
	float32 m_radius;
};

//第一次调用将count置0
struct b2SimplexCache
{
	float32 metric;		//长度或面积
	uint16 count;       //顶点数      
	uint8 indexA[3];	//shapeA上的顶点索引数组
	uint8 indexB[3];	//shapeB上的顶点索引数组
};
//  b2Distance的输入
//  在计算中你必须选择使用形状的半径
struct b2DistanceInput
{
	b2DistanceProxy proxyA;    //距离代理A
	b2DistanceProxy proxyB;    //距离代理B
	b2Transform transformA;    //转换A
	b2Transform transformB;    //转换B
	bool useRadii;             //是否使用半径
};

// b2Distance的输出
struct b2DistanceOutput
{
	b2Vec2 pointA;		//shapeA上最近的点
	b2Vec2 pointB;		//shapeB上最近的点
	float32 distance;   //距离
	int32 iterations;	//GJK的迭代次数
};

/**************************************************************************
* 功能描述：在两个形状间计算最近点。支持下面的任何组合：
            圆形、多边形、边缘形状。单纯形缓存输入/输出
			第一次调用设置b2SimplexCache.count为0
* 参数说明： output ：距离输出指针
             cache  ：单纯形缓存指针
			 input  ：距离输入指针
* 返 回 值： (void)
***************************************************************************/
void b2Distance(b2DistanceOutput* output,
				b2SimplexCache* cache, 
				const b2DistanceInput* input);


//////////////////////////////////////////////////////////////////////////
//获取顶点个数
inline int32 b2DistanceProxy::GetVertexCount() const
{
	return m_count;
}
//根据索引获得顶点 
inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const
{
	b2Assert(0 <= index && index < m_count);
	return m_vertices[index];
}
//根据指定的方向获得支撑点索引，用于构建单纯形
inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const
{
	int32 bestIndex = 0;
	// 获取最远的点，用于产生的单纯形包含最大的空间区域
	float32 bestValue = b2Dot(m_vertices[0], d);
	//遍历所有的顶点，获取最远点的索引
	for (int32 i = 1; i < m_count; ++i)
	{
		float32 value = b2Dot(m_vertices[i], d);
		if (value > bestValue)
		{
			bestIndex = i;
			bestValue = value;
		}
	}
	//返回索引
	return bestIndex;
}
//根据指定的方向向量获得支撑点，用于构建单纯形
inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const
{
	int32 bestIndex = 0;
	float32 bestValue = b2Dot(m_vertices[0], d);
	//遍历所有的顶点，获得远的点
	for (int32 i = 1; i < m_count; ++i)
	{
		float32 value = b2Dot(m_vertices[i], d);
		if (value > bestValue)
		{
			bestIndex = i;
			bestValue = value;
		}
	}
	//返回顶点
	return m_vertices[bestIndex];
}

可以看到，我们在这里定义了一个距离代理b2DistanceProxy，使用了GJK算法，同时它封装了任何形状。我们先在此注意一下m_buffer[2]这个变量，它专门保存链条形状而弄的。等到具体实现部分，我们再讨论。再看看b2Distance函数，它是这部分的主角，主要通过使用GJK算法获取单纯形，然后写入缓存中。再看内联函数中GetSupport和GetSupportVertex函数，都将在构建单纯形时使用。

关于b2Distance.cpp文件中相应方法的实现，主要做了一下几件事：

a)、距离代理类中Set函数的实现

b)、有关单纯形的定义和实现

c)、b2Distance方法的实现

废话不多说，开工。

 

1、距离代理类中Set函数的实现

//调用b2Distance函数总次数、查找单纯形顶点的总次数、查找单纯形顶点每次的最大次数

int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;

//用给定的形状初始化代理。当使用代理时，形状必须保持在一定的范围内

void b2DistanceProxy::Set(const b2Shape* shape, int32 index)

{

       //获取形状

       switch (shape->GetType())

       {

       case b2Shape::e_circle:                   //根据圆形信息设置成员变量

              {

                     const b2CircleShape* circle = (b2CircleShape*)shape;

                     m_vertices = &circle->m_p;

                     m_count = 1;

                     m_radius = circle->m_radius;

              }

              break;

 

       case b2Shape::e_polygon:                  //根据多边形信息设置成员变量

              {

                     const b2PolygonShape* polygon = (b2PolygonShape*)shape;

                     m_vertices = polygon->m_vertices;

                     m_count = polygon->m_vertexCount;

                     m_radius = polygon->m_radius;

              }

              break;

 

       case b2Shape::e_chain:                   //根据链形信息设置成员变量

              {

                     const b2ChainShape* chain = (b2ChainShape*)shape;

                     b2Assert(0 <= index && index < chain->m_count);

                     //获取索引为index的顶点

                     m_buffer[0] = chain->m_vertices[index];

                     //判断索引号是否超出顶点数量

                     if (index + 1 < chain->m_count)

                     {

                            //没超出，获取接下来的一个点

                            m_buffer[1] = chain->m_vertices[index + 1];

                     }

                     else

                     {

                            //超出，获取第一个点

                            m_buffer[1] = chain->m_vertices[0];

                     }

 

                     m_vertices = m_buffer;

                     m_count = 2;

                     m_radius = chain->m_radius;

              }

              break;

 

       case b2Shape::e_edge:                 //根据边缘形信息设置成员变量

              {

                     const b2EdgeShape* edge = (b2EdgeShape*)shape;

                     m_vertices = &edge->m_vertex1;

                     m_count = 2;

                     m_radius = edge->m_radius;

              }

              break;

 

       default:

              b2Assert(false);

       }

}

 

上面全局变量主要用于记录调用次数的。暂时没啥用处，但是可以作为相应函数中算法性能的重要指标。大家还发现什么了没？所有变量都没有初始化，怎么回事呢？大家还记得我们曾经说过这个问题了，在C++/C中对于全局变量，如果没有初始值的话，编译器会自动附初值。因为都是数值类型，此处都设为0（这里又遇到原来学过的知识了，好开心，）。再来看看Set函数，我们来说说设置链条形状的时候的情形吧，首先，我们在末尾顶点处做了一个特殊处理，当索引对应的顶点为最后一个时，我们将第一个节点赋给m_buffer[1]，其次，box2d中专门为设置链条申请了一个缓存数组m_buffer,主要是为了以后使用的时候更加方便。

 

2、有关单纯形的定义和实现

 

该部分主要是通过围绕单纯形进展的，关于单纯形，这里主要这里通过voronoi区域算法和重心坐标找到去寻找合适的三个顶点构建三角形（2d空间中一般用的单纯形是三角形）。我们不禁要问怎样找到的顶点才算是最合适的呢？这个三角形有用啥作用呢？带着这两个问题，我们继续往下看。

在此处我们也将分为以下几部分：

a)、b2SimplexVertex结构体的定义

b)、缓存操作的函数，ReadCache函数和WriteCache函数

c)、查找适合顶点的辅助函数，GetSearchDirection、GetClosestPoint、GetWitnessPoints、GetMetric等函数

d)、寻找原点位置的函数，Solve2函数和Solve3函数

我们就来看看相关源码的实现吧。

 a)、b2SimplexVertex结构体的定义

 

//单纯形顶点
struct b2SimplexVertex
{
	b2Vec2 wA;		// proxyA的顶点
	b2Vec2 wB;		// proxyB的顶点
	b2Vec2 w;		// 这个是支撑点，也可能是单纯形的一个顶点，等于 wB - wA
	float32 a;		// 参数，用于求重心坐标的
	int32 indexA;	// wA的索引
	int32 indexB;	// wB的索引
};

我们来看看b2SimplexVertex这个结构体，它是作为单纯形的顶点而存在的，主要用于存储相关的信息，其一个单纯形顶点是通过来自两个形状的各一个顶点求得的。

 

b)、缓存操作的函数，ReadCache函数和WriteCache函数

 

/**************************************************************************
	* 功能描述：读取缓存
	* 参数说明：cache     ：缓存对象指针
	            proxyA    ：距离代理A
				transfromA：变换A
				proxyB    ：距离代理B
				transfromB：变化B
	* 返 回 值： (void)
	***************************************************************************/
	void ReadCache(	const b2SimplexCache* cache,
					const b2DistanceProxy* proxyA, const b2Transform& transformA,
					const b2DistanceProxy* proxyB, const b2Transform& transformB)
	{
		//验证count的有效性
		b2Assert(cache->count <= 3);
		
		// 从缓存中拷贝数据
		m_count = cache->count;
		b2SimplexVertex* vertices = &m_v1;
		for (int32 i = 0; i < m_count; ++i)
		{
			b2SimplexVertex* v = vertices + i;
			v->indexA = cache->indexA[i];
			v->indexB = cache->indexB[i];
			b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
			b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
			v->wA = b2Mul(transformA, wALocal);
			v->wB = b2Mul(transformB, wBLocal);
			v->w = v->wB - v->wA;
			v->a = 0.0f;
		}
		// 如果大体上有别与就得尺度，就计算新的单纯形尺度（长度或面积），然后刷新它
		if (m_count > 1)
		{
			// 获取旧的和新的尺度
			float32 metric1 = cache->metric;
			float32 metric2 = GetMetric();
			if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
			{
				//重置单纯形
				m_count = 0;
			}
		}

		//如果缓存是空或者无效
		if (m_count == 0)
		{
			b2SimplexVertex* v = vertices + 0;
			v->indexA = 0;
			v->indexB = 0;
			b2Vec2 wALocal = proxyA->GetVertex(0);
			b2Vec2 wBLocal = proxyB->GetVertex(0);
			v->wA = b2Mul(transformA, wALocal);
			v->wB = b2Mul(transformB, wBLocal);
			v->w = v->wB - v->wA;
			m_count = 1;
		}
	}

	/**************************************************************************
	* 功能描述：写入缓存
	* 参数说明：cache     ：缓存对象指针
	* 返 回 值： (void)
	***************************************************************************/
	void WriteCache(b2SimplexCache* cache) const
	{
		//设置cache
		cache->metric = GetMetric();
		cache->count = uint16(m_count);
		const b2SimplexVertex* vertices = &m_v1;
		for (int32 i = 0; i < m_count; ++i)
		{
			cache->indexA[i] = uint8(vertices[i].indexA);
			cache->indexB[i] = uint8(vertices[i].indexB);
		}
	}

关于b2Simplex结构体成员变量的定义，上代码：

	//单纯形的顶点变量
	b2SimplexVertex m_v1, m_v2, m_v3;
	//单纯形顶点个数
	int32 m_count;

ReadCache函数和WriteCache函数，说白了就是对结构中成员变量赋值和被赋值。对于这两个函数，有个有意思的代码，各位请看ReadCache函数第三句代码和下面的for循环，这里将m_v1的地址赋给vertices变量，并用for循环遍历。我们马上又有一个疑问了，m_v1不是数组，为什么可以用for循环遍历呢？我们不妨看看它的定义，这里连续定义了三个相同类型的变量m_v1，m_v2，m_v3中，说明此处虽然m_v1不是数组，但是这三个变量的内存是相连的，所以可以当数组访问。

 

c)、查找适合顶点的辅助函数，GetSearchDirection、GetClosestPoint、GetWitnessPoints、GetMetric等函数

 

/**************************************************************************
	* 功能描述：获取查找方向
	* 参数说明： (void)
	* 返 回 值： (void)
	***************************************************************************/
	b2Vec2 GetSearchDirection() const
	{
		switch (m_count)
		{
		case 1:                                             //一个顶点从相反的方向去找
			return -m_v1.w;
			//两个顶点，1、两个点所组成边向量e12与第一个顶点和原点的所组成的向量-m_v1.w先去判断符号
			//          2、再求e12相应方向的法向量a
			//          3、将a作为查找方向返回
		case 2:                                             
			{
				//获取方向
				b2Vec2 e12 = m_v2.w - m_v1.w;
				float32 sgn = b2Cross(e12, -m_v1.w);
				if (sgn > 0.0f)
				{
					// 原点在e12的左边
					return b2Cross(1.0f, e12);
				}
				else
				{
					// 原点在e12的右边
					return b2Cross(e12, 1.0f);
				}
			}
			//这里不会出现多个点的情况，因为查找方向是对两个以下的顶点而言的，
			//没找到合适的第三个顶点之前永远都会将离原点最远的第三个点移除。
		default:
			b2Assert(false);
			return b2Vec2_zero;
		}
	}
	/**************************************************************************
	* 功能描述：获取最近的点
	* 参数说明： (void)
	* 返 回 值： (void)
	***************************************************************************/
	b2Vec2 GetClosestPoint() const
	{
		//顶点数量
		switch (m_count)
		{
			//对于0个顶点，没有最近点这一说，所以我们在此处断言
		case 0:
			b2Assert(false);
			return b2Vec2_zero;
			//对于1个顶点，最近点就是那个顶点
		case 1:
			return m_v1.w;
			//对于两个顶点，重心是它们的最近点
		case 2:
			return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
			//对于三个顶点来说，单纯形（此处是三角形）已形成，不需要在找最近点
			//故返回零向量
		case 3:
			return b2Vec2_zero;
			//多个点只有出现异常的时候会到这儿，故此处断言
		default:
			b2Assert(false);
			return b2Vec2_zero;
		}
	}

	/**************************************************************************
	* 功能描述：获取见证点【形状上的参与求解的两个顶点】
	* 参数说明： pA：形状A上的点
	             pB：形状B上的点
	* 返 回 值： (void)
	***************************************************************************/
	void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
	{
		switch (m_count)
		{
			//对于0个点，还没有开始查找单纯形的顶点，不存在见证点，故断言
		case 0:
			b2Assert(false);
			break;
			//对于1个点，见证点就是wA和wB，直接赋值
		case 1:
			*pA = m_v1.wA;
			*pB = m_v1.wB;
			break;
			//对于2个点，我们用重心坐标求的的，至于为什么要用重心坐标，我们在下面的讲解中将会看到
		case 2:
			*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
			*pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
			break;
			//对于3个点，我们可以判断此时两形状一碰撞了，见证点，就是两形状的碰撞点，故是一样的。
		case 3:
			*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
			*pB = *pA;
			break;

		default:
			b2Assert(false);
			break;
		}
	}
	/**************************************************************************
	* 功能描述：获取尺度（长度）
	* 参数说明： (void)
	* 返 回 值： 尺度（长度）
	***************************************************************************/
	float32 GetMetric() const
	{
		switch (m_count)
		{
			//对于0个点，还没有开始查找单纯形的顶点，不存在长度，故断言
		case 0:
			b2Assert(false);
			return 0.0;
			// 对于1个点，只是一个点，没有长度，返回0
		case 1:
			return 0.0f;
			//对于2个点，计算其长度，并返回
		case 2:
			return b2Distance(m_v1.w, m_v2.w);
			//对于3个点，获取(m_v2.w - m_v1.w)与(m_v3.w - m_v1.w)
			//两向量组成的平行四边形面积
		case 3:
			return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
			//不存在更多的点，如果到这里就出现了异常
		default:
			b2Assert(false);
			return 0.0f;
		}
	}

关于这组函数，每一个里面都分了几种不同的情况，注释里已具体分析了，不在此赘述了。下面我们总体概括一下吧，

对于GetSearchDirection函数，主要决定从哪个方向去找顶点，通过计算，不断的调整查找的方向。

对于GetClosestPoint函数，获取一个单纯形最近的点。

对于GetWitnessPoints函数，主要获取两形状之间的见证点，所谓见证点，就是形成一个单纯形顶点的两个形状上的顶点。

对于GetMetric函数，我们将获取单纯形的距离或面积，作为判断是否需要更新缓存的一个条件。

 

d)、寻找原点位置的函数，Solve2函数和Solve3函数

 

在b2Simplex定义这两个函数的，各位请看。

/**************************************************************************
	* 功能描述：用重心坐标求解一条线
	p = a1 * w1 + a2 * w2
	a1 + a2 = 1
	向量垂直与从原点到最近点形成的线
	e12 = w2 -w1;
	dot(p,e) = 0
	a1 * dot(w1,e) + a2 * dot(w2,e) = 0
	2X2线性方程
	[1      1     ][a1] = [1]
	[w1.e12 w2.e12][a2] = [0]
	定义
	d12_1 = dot(w2,e12)
	d12_2 = -dot(21,e12)
	解决
	a1 = d12_1 /d12
	a2 = d12_2/ d12
	* 参数说明： (void)
	* 返 回 值： (void)
	***************************************************************************/
	void Solve2();
	/**************************************************************************
	* 功能描述：可能区域：
				1、points[2]
				2、边缘形状 points[0] - points[2]
				3、边缘形状 points[1] - points[2]
				4、三角形内部
	* 参数说明： (void)
	* 返 回 值： (void)
	***************************************************************************/
	void Solve3();

实现部分。

//2个单纯顶点时，判断原点位置函数

void b2Simplex::Solve2()
{
	b2Vec2 w1 = m_v1.w;
	b2Vec2 w2 = m_v2.w;
	b2Vec2 e12 = w2 - w1;

	// w1区域
	float32 d12_2 = -b2Dot(w1, e12);
	if (d12_2 <= 0.0f)
	{
		// a2 <= 0, 所以我们将它置为0
		m_v1.a = 1.0f;
		m_count = 1;
		return;
	}
	// w2区域
	float32 d12_1 = b2Dot(w2, e12);
	if (d12_1 <= 0.0f)
	{
		// a1 <= 0,所以我们将它置为0
		m_v2.a = 1.0f;
		m_count = 1;
		m_v1 = m_v2;
		return;
	}
	//必须在e12区域
	float32 inv_d12 = 1.0f / (d12_1 + d12_2);
	m_v1.a = d12_1 * inv_d12;
	m_v2.a = d12_2 * inv_d12;
	m_count = 2;
}
//3个单纯顶点时，判断原点位置函数

void b2Simplex::Solve3()
{
	b2Vec2 w1 = m_v1.w;
	b2Vec2 w2 = m_v2.w;
	b2Vec2 w3 = m_v3.w;

	// Edge12
	// [1      1     ][a1] = [1]
	// [w1.e12 w2.e12][a2] = [0]
	// a3 = 0
	b2Vec2 e12 = w2 - w1;
	float32 w1e12 = b2Dot(w1, e12);
	float32 w2e12 = b2Dot(w2, e12);
	float32 d12_1 = w2e12;
	float32 d12_2 = -w1e12;

	// Edge13
	// [1      1     ][a1] = [1]
	// [w1.e13 w3.e13][a3] = [0]
	// a2 = 0
	b2Vec2 e13 = w3 - w1;
	float32 w1e13 = b2Dot(w1, e13);
	float32 w3e13 = b2Dot(w3, e13);
	float32 d13_1 = w3e13;
	float32 d13_2 = -w1e13;

	// Edge23
	// [1      1     ][a2] = [1]
	// [w2.e23 w3.e23][a3] = [0]
	// a1 = 0
	b2Vec2 e23 = w3 - w2;
	float32 w2e23 = b2Dot(w2, e23);
	float32 w3e23 = b2Dot(w3, e23);
	float32 d23_1 = w3e23;
	float32 d23_2 = -w2e23;
	
	// 三角形e123
	float32 n123 = b2Cross(e12, e13);

	float32 d123_1 = n123 * b2Cross(w2, w3);
	float32 d123_2 = n123 * b2Cross(w3, w1);
	float32 d123_3 = n123 * b2Cross(w1, w2);

	// w1区域
	if (d12_2 <= 0.0f && d13_2 <= 0.0f)
	{
		m_v1.a = 1.0f;
		m_count = 1;
		return;
	}

	// e12
	if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
	{
		float32 inv_d12 = 1.0f / (d12_1 + d12_2);
		m_v1.a = d12_1 * inv_d12;
		m_v2.a = d12_2 * inv_d12;
		m_count = 2;
		return;
	}

	// e13
	if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
	{
		float32 inv_d13 = 1.0f / (d13_1 + d13_2);
		m_v1.a = d13_1 * inv_d13;
		m_v3.a = d13_2 * inv_d13;
		m_count = 2;
		m_v2 = m_v3;
		return;
	}

	// w2区域
	if (d12_1 <= 0.0f && d23_2 <= 0.0f)
	{
		m_v2.a = 1.0f;
		m_count = 1;
		m_v1 = m_v2;
		return;
	}

	// w3区域
	if (d13_1 <= 0.0f && d23_1 <= 0.0f)
	{
		m_v3.a = 1.0f;
		m_count = 1;
		m_v1 = m_v3;
		return;
	}

	// e23
	if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
	{
		float32 inv_d23 = 1.0f / (d23_1 + d23_2);
		m_v2.a = d23_1 * inv_d23;
		m_v3.a = d23_2 * inv_d23;
		m_count = 2;
		m_v1 = m_v3;
		return;
	}

	//一定在三角形123内部
	float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
	m_v1.a = d123_1 * inv_d123;
	m_v2.a = d123_2 * inv_d123;
	m_v3.a = d123_3 * inv_d123;
	m_count = 3;
}

这两个函数主要通过通过两步来完成的：

i)、用重心坐标求的判断条件

ii)、用求的的判断条件将现在的平面分成不同的区域，然后进行不同的判断，即voronoi区域算法

3、b2Distance方法的实现

 

到我们今天的压轴主角了，还是先上代码。

//在两个形状间计算最近点
void b2Distance(b2DistanceOutput* output,
				b2SimplexCache* cache,
				const b2DistanceInput* input)
{
	++b2_gjkCalls;
	//初始化代理
	const b2DistanceProxy* proxyA = &input->proxyA;
	const b2DistanceProxy* proxyB = &input->proxyB;
	//获取变换
	b2Transform transformA = input->transformA;
	b2Transform transformB = input->transformB;
	//初始化单纯形
	b2Simplex simplex;
	simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
	//获取单纯形顶点
	b2SimplexVertex* vertices = &simplex.m_v1;
	const int32 k_maxIters = 20;
	// 存储最近的单纯形顶点，以便我们检测是否重复和阻止循环
	int32 saveA[3], saveB[3];
	int32 saveCount = 0;
	//获取单纯形最近的顶点，并获取该点与原点组成的向量的长度平方
	b2Vec2 closestPoint = simplex.GetClosestPoint();
	float32 distanceSqr1 = closestPoint.LengthSquared();
	float32 distanceSqr2 = distanceSqr1;
	//主迭代循环
	int32 iter = 0;
	while (iter < k_maxIters)
	{
		// 拷贝单纯形，我们可以识别重复
		saveCount = simplex.m_count;
		for (int32 i = 0; i < saveCount; ++i)
		{
			saveA[i] = vertices[i].indexA;
			saveB[i] = vertices[i].indexB;
		}
		//获取单纯形顶点,并判断
		switch (simplex.m_count)
		{
		case 1:
			break;

		case 2:
			simplex.Solve2();
			break;

		case 3:
			simplex.Solve3();
			break;

		default:
			b2Assert(false);
		}
		// 如果我们有3个点，原点在相应的三角形内。
		if (simplex.m_count == 3)
		{
			break;
		}
		//计算最近的点
		b2Vec2 p = simplex.GetClosestPoint();
		distanceSqr2 = p.LengthSquared();

		
		//确保前进
		if (distanceSqr2 >= distanceSqr1)
		{
			//break;
		}
		distanceSqr1 = distanceSqr2;
		//获取查找方向
		b2Vec2 d = simplex.GetSearchDirection();
		//确保查找的方向是有效的
		if (d.LengthSquared() < b2_epsilon * b2_epsilon)
		{
			//原点有可能包含在线段或三角形内，因此形状是重叠的
			//尽管这里有可能重叠，我们不能在这里返回0
			//如果单纯形是一个点、线、三角形，它很难判断原点是否包含在在其中或者是非常靠近
			break;
		}
		//用支撑点计算一个试探性的新的单纯形顶点
		b2SimplexVertex* vertex = vertices + simplex.m_count;
		vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
		vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
		b2Vec2 wBLocal;
		vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
		vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
		vertex->w = vertex->wB - vertex->wA;

		//迭代计数等于支撑点的调用数量
		++iter;
		++b2_gjkIters;
		//检测重复的支撑点，这是主要的终止条件
		bool duplicate = false;
		for (int32 i = 0; i < saveCount; ++i)
		{
			if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
			{
				duplicate = true;
				break;
			}
		}
		// 如果我们发现了一个重复的支撑点我们必须退出循环
		if (duplicate)
		{
			break;
		}
		// 新的顶点是好的且需要的
		++simplex.m_count;
	}

	b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
	// 准备输出
	simplex.GetWitnessPoints(&output->pointA, &output->pointB);
	output->distance = b2Distance(output->pointA, output->pointB);
	output->iterations = iter;
	//将单纯形写入缓存
	simplex.WriteCache(cache);
	// 如果被要求，则提供半径
	if (input->useRadii)
	{
		float32 rA = proxyA->m_radius;
		float32 rB = proxyB->m_radius;

		if (output->distance > rA + rB && output->distance > b2_epsilon)
		{
			// 形状没有重叠,移动见证点到外面
			output->distance -= rA + rB;
			b2Vec2 normal = output->pointB - output->pointA;
			normal.Normalize();
			output->pointA += rA * normal;
			output->pointB -= rB * normal;
		}
		else
		{
			// 当形状重叠，移动见证点到中间
			b2Vec2 p = 0.5f * (output->pointA + output->pointB);
			output->pointA = p;
			output->pointB = p;
			output->distance = 0.0f;
		}
	}
}

关于这个函数，主要步骤就以下四步：

a)、通过代理输入对象input中的信息读取缓存中的单纯形A

b)、迭代验证A是否是最优的单纯形，是则退出迭代，不是则迭代构建新的单纯形B

c)、将B写入缓存

d)、如果代理输入对象input使用了半径，则加入半径，重新计算距离。

 

Ok,关于这部分内容，我们就说到这里了。唉，又不早了，说好的末日呢，我在等你呢。。。

 

ps：

 

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