程序博客网 > 吉林大学网络教学平台

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

来源：互联网 发布：吉林大学网络教学平台 编辑：程序博客网 时间：2024/06/16 11:27

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

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

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

[cpp] view plain copy
  1. //声明类  
  2. class b2Shape;  
  3. // 距离代理，使用GJK算法实现  
  4. // 它封装了任何形状  
  5. struct b2DistanceProxy  
  6. {  
  7.     /************************************************************************** 
  8.     * 功能描述：构造函数，初始化信息 
  9.     * 参数说明： (void) 
  10.     * 返 回 值： (void) 
  11.     ***************************************************************************/  
  12.     b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {}  
  13.     /************************************************************************** 
  14.     * 功能描述：用给定的形状初始化代理。当使用代理时，形状必须保持在一定的范围内 
  15.     * 参数说明： shape：形状指针变量 
  16.                  index：索引值 
  17.     * 返 回 值： (void) 
  18.     ***************************************************************************/  
  19.     void Set(const b2Shape* shape, int32 index);  
  20.     /************************************************************************** 
  21.     * 功能描述：根据指定的方向获得支撑点的索引 
  22.     * 参数说明： d:向量的引用 
  23.     * 返 回 值： 顶点的索引 
  24.     ***************************************************************************/  
  25.     int32 GetSupport(const b2Vec2& d) const;  
  26.     /************************************************************************** 
  27.     * 功能描述：根据指定的方向获得支撑点 
  28.     * 参数说明： d:向量的引用 
  29.     * 返 回 值： 顶点坐标 
  30.     ***************************************************************************/  
  31.     const b2Vec2& GetSupportVertex(const b2Vec2& d) const;  
  32.     /************************************************************************** 
  33.     * 功能描述：获取顶点个数 
  34.     * 参数说明： (void) 
  35.     * 返 回 值： 顶点的个数 
  36.     ***************************************************************************/  
  37.     int32 GetVertexCount() const;  
  38.     /************************************************************************** 
  39.     * 功能描述：根据索引获得顶点 
  40.                  用于b2Distance 
  41.     * 参数说明： index：索引 
  42.     * 返 回 值： 顶点 
  43.     ***************************************************************************/  
  44.     const b2Vec2& GetVertex(int32 index) const;  
  45.     //顶点缓存，用于保存chain形状中的两个顶点  
  46.     b2Vec2 m_buffer[2];  
  47.     //坐标点  
  48.     const b2Vec2* m_vertices;  
  49.     //顶点的数量  
  50.     int32 m_count;  
  51.     //半径  
  52.     float32 m_radius;  
  53. };  
  54.   
  55. //第一次调用将count置0  
  56. struct b2SimplexCache  
  57. {  
  58.     float32 metric;     //长度或面积  
  59.     uint16 count;       //顶点数        
  60.     uint8 indexA[3];    //shapeA上的顶点索引数组  
  61.     uint8 indexB[3];    //shapeB上的顶点索引数组  
  62. };  
  63. //  b2Distance的输入  
  64. //  在计算中你必须选择使用形状的半径  
  65. struct b2DistanceInput  
  66. {  
  67.     b2DistanceProxy proxyA;    //距离代理A  
  68.     b2DistanceProxy proxyB;    //距离代理B  
  69.     b2Transform transformA;    //转换A  
  70.     b2Transform transformB;    //转换B  
  71.     bool useRadii;             //是否使用半径  
  72. };  
  73.   
  74. // b2Distance的输出  
  75. struct b2DistanceOutput  
  76. {  
  77.     b2Vec2 pointA;      //shapeA上最近的点  
  78.     b2Vec2 pointB;      //shapeB上最近的点  
  79.     float32 distance;   //距离  
  80.     int32 iterations;   //GJK的迭代次数  
  81. };  
  82.   
  83. /************************************************************************** 
  84. * 功能描述：在两个形状间计算最近点。支持下面的任何组合： 
  85.             圆形、多边形、边缘形状。单纯形缓存输入/输出 
  86.             第一次调用设置b2SimplexCache.count为0 
  87. * 参数说明： output ：距离输出指针 
  88.              cache  ：单纯形缓存指针 
  89.              input  ：距离输入指针 
  90. * 返 回 值： (void) 
  91. ***************************************************************************/  
  92. void b2Distance(b2DistanceOutput* output,  
  93.                 b2SimplexCache* cache,   
  94.                 const b2DistanceInput* input);  
  95.   
  96.   
  97. //////////////////////////////////////////////////////////////////////////  
  98. //获取顶点个数  
  99. inline int32 b2DistanceProxy::GetVertexCount() const  
  100. {  
  101.     return m_count;  
  102. }  
  103. //根据索引获得顶点   
  104. inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const  
  105. {  
  106.     b2Assert(0 <= index && index < m_count);  
  107.     return m_vertices[index];  
  108. }  
  109. //根据指定的方向获得支撑点索引，用于构建单纯形  
  110. inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const  
  111. {  
  112.     int32 bestIndex = 0;  
  113.     // 获取最远的点，用于产生的单纯形包含最大的空间区域  
  114.     float32 bestValue = b2Dot(m_vertices[0], d);  
  115.     //遍历所有的顶点，获取最远点的索引  
  116.     for (int32 i = 1; i < m_count; ++i)  
  117.     {  
  118.         float32 value = b2Dot(m_vertices[i], d);  
  119.         if (value > bestValue)  
  120.         {  
  121.             bestIndex = i;  
  122.             bestValue = value;  
  123.         }  
  124.     }  
  125.     //返回索引  
  126.     return bestIndex;  
  127. }  
  128. //根据指定的方向向量获得支撑点，用于构建单纯形  
  129. inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const  
  130. {  
  131.     int32 bestIndex = 0;  
  132.     float32 bestValue = b2Dot(m_vertices[0], d);  
  133.     //遍历所有的顶点，获得远的点  
  134.     for (int32 i = 1; i < m_count; ++i)  
  135.     {  
  136.         float32 value = b2Dot(m_vertices[i], d);  
  137.         if (value > bestValue)  
  138.         {  
  139.             bestIndex = i;  
  140.             bestValue = value;  
  141.         }  
  142.     }  
  143.     //返回顶点  
  144.     return m_vertices[bestIndex];  
  145. }  

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

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

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

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

c)、b2Distance方法的实现

废话不多说，开工。

 

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

[cpp] view plain copy
  1. //调用b2Distance函数总次数、查找单纯形顶点的总次数、查找单纯形顶点每次的最大次数  
  2.   
  3. int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;  
  4.   
  5. //用给定的形状初始化代理。当使用代理时，形状必须保持在一定的范围内  
  6.   
  7. void b2DistanceProxy::Set(const b2Shape* shape, int32 index)  
  8.   
  9. {  
  10.   
  11.        //获取形状  
  12.   
  13.        switch (shape->GetType())  
  14.   
  15.        {  
  16.   
  17.        case b2Shape::e_circle:                   //根据圆形信息设置成员变量  
  18.   
  19.               {  
  20.   
  21.                      const b2CircleShape* circle = (b2CircleShape*)shape;  
  22.   
  23.                      m_vertices = &circle->m_p;  
  24.   
  25.                      m_count = 1;  
  26.   
  27.                      m_radius = circle->m_radius;  
  28.   
  29.               }  
  30.   
  31.               break;  
  32.   
  33.    
  34.   
  35.        case b2Shape::e_polygon:                  //根据多边形信息设置成员变量  
  36.   
  37.               {  
  38.   
  39.                      const b2PolygonShape* polygon = (b2PolygonShape*)shape;  
  40.   
  41.                      m_vertices = polygon->m_vertices;  
  42.   
  43.                      m_count = polygon->m_vertexCount;  
  44.   
  45.                      m_radius = polygon->m_radius;  
  46.   
  47.               }  
  48.   
  49.               break;  
  50.   
  51.    
  52.   
  53.        case b2Shape::e_chain:                   //根据链形信息设置成员变量  
  54.   
  55.               {  
  56.   
  57.                      const b2ChainShape* chain = (b2ChainShape*)shape;  
  58.   
  59.                      b2Assert(0 <= index && index < chain->m_count);  
  60.   
  61.                      //获取索引为index的顶点  
  62.   
  63.                      m_buffer[0] = chain->m_vertices[index];  
  64.   
  65.                      //判断索引号是否超出顶点数量  
  66.   
  67.                      if (index + 1 < chain->m_count)  
  68.   
  69.                      {  
  70.   
  71.                             //没超出，获取接下来的一个点  
  72.   
  73.                             m_buffer[1] = chain->m_vertices[index + 1];  
  74.   
  75.                      }  
  76.   
  77.                      else  
  78.   
  79.                      {  
  80.   
  81.                             //超出，获取第一个点  
  82.   
  83.                             m_buffer[1] = chain->m_vertices[0];  
  84.   
  85.                      }  
  86.   
  87.    
  88.   
  89.                      m_vertices = m_buffer;  
  90.   
  91.                      m_count = 2;  
  92.   
  93.                      m_radius = chain->m_radius;  
  94.   
  95.               }  
  96.   
  97.               break;  
  98.   
  99.    
  100.   
  101.        case b2Shape::e_edge:                 //根据边缘形信息设置成员变量  
  102.   
  103.               {  
  104.   
  105.                      const b2EdgeShape* edge = (b2EdgeShape*)shape;  
  106.   
  107.                      m_vertices = &edge->m_vertex1;  
  108.   
  109.                      m_count = 2;  
  110.   
  111.                      m_radius = edge->m_radius;  
  112.   
  113.               }  
  114.   
  115.               break;  
  116.   
  117.    
  118.   
  119.        default:  
  120.   
  121.               b2Assert(false);  
  122.   
  123.        }  
  124.   
  125. }  

 

上面全局变量主要用于记录调用次数的。暂时没啥用处，但是可以作为相应函数中算法性能的重要指标。大家还发现什么了没？所有变量都没有初始化，怎么回事呢？大家还记得我们曾经说过这个问题了，在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结构体的定义

 

[cpp] view plain copy
  1. //单纯形顶点  
  2. struct b2SimplexVertex  
  3. {  
  4.     b2Vec2 wA;      // proxyA的顶点  
  5.     b2Vec2 wB;      // proxyB的顶点  
  6.     b2Vec2 w;       // 这个是支撑点，也可能是单纯形的一个顶点，等于 wB - wA  
  7.     float32 a;      // 参数，用于求重心坐标的  
  8.     int32 indexA;   // wA的索引  
  9.     int32 indexB;   // wB的索引  
  10. };  

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

 

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

 

[cpp] view plain copy
  1. /************************************************************************** 
  2.     * 功能描述：读取缓存 
  3.     * 参数说明：cache     ：缓存对象指针 
  4.                 proxyA    ：距离代理A 
  5.                 transfromA：变换A 
  6.                 proxyB    ：距离代理B 
  7.                 transfromB：变化B 
  8.     * 返 回 值： (void) 
  9.     ***************************************************************************/  
  10.     void ReadCache( const b2SimplexCache* cache,  
  11.                     const b2DistanceProxy* proxyA, const b2Transform& transformA,  
  12.                     const b2DistanceProxy* proxyB, const b2Transform& transformB)  
  13.     {  
  14.         //验证count的有效性  
  15.         b2Assert(cache->count <= 3);  
  16.           
  17.         // 从缓存中拷贝数据  
  18.         m_count = cache->count;  
  19.         b2SimplexVertex* vertices = &m_v1;  
  20.         for (int32 i = 0; i < m_count; ++i)  
  21.         {  
  22.             b2SimplexVertex* v = vertices + i;  
  23.             v->indexA = cache->indexA[i];  
  24.             v->indexB = cache->indexB[i];  
  25.             b2Vec2 wALocal = proxyA->GetVertex(v->indexA);  
  26.             b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);  
  27.             v->wA = b2Mul(transformA, wALocal);  
  28.             v->wB = b2Mul(transformB, wBLocal);  
  29.             v->w = v->wB - v->wA;  
  30.             v->a = 0.0f;  
  31.         }  
  32.         // 如果大体上有别与就得尺度，就计算新的单纯形尺度（长度或面积），然后刷新它  
  33.         if (m_count > 1)  
  34.         {  
  35.             // 获取旧的和新的尺度  
  36.             float32 metric1 = cache->metric;  
  37.             float32 metric2 = GetMetric();  
  38.             if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)  
  39.             {  
  40.                 //重置单纯形  
  41.                 m_count = 0;  
  42.             }  
  43.         }  
  44.   
  45.         //如果缓存是空或者无效  
  46.         if (m_count == 0)  
  47.         {  
  48.             b2SimplexVertex* v = vertices + 0;  
  49.             v->indexA = 0;  
  50.             v->indexB = 0;  
  51.             b2Vec2 wALocal = proxyA->GetVertex(0);  
  52.             b2Vec2 wBLocal = proxyB->GetVertex(0);  
  53.             v->wA = b2Mul(transformA, wALocal);  
  54.             v->wB = b2Mul(transformB, wBLocal);  
  55.             v->w = v->wB - v->wA;  
  56.             m_count = 1;  
  57.         }  
  58.     }  
  59.   
  60.     /************************************************************************** 
  61.     * 功能描述：写入缓存 
  62.     * 参数说明：cache     ：缓存对象指针 
  63.     * 返 回 值： (void) 
  64.     ***************************************************************************/  
  65.     void WriteCache(b2SimplexCache* cache) const  
  66.     {  
  67.         //设置cache  
  68.         cache->metric = GetMetric();  
  69.         cache->count = uint16(m_count);  
  70.         const b2SimplexVertex* vertices = &m_v1;  
  71.         for (int32 i = 0; i < m_count; ++i)  
  72.         {  
  73.             cache->indexA[i] = uint8(vertices[i].indexA);  
  74.             cache->indexB[i] = uint8(vertices[i].indexB);  
  75.         }  
  76.     }  

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

[cpp] view plain copy
  1. //单纯形的顶点变量  
  2. b2SimplexVertex m_v1, m_v2, m_v3;  
  3. //单纯形顶点个数  
  4. 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等函数

 

[cpp] view plain copy
  1. /************************************************************************** 
  2.     * 功能描述：获取查找方向 
  3.     * 参数说明： (void) 
  4.     * 返 回 值： (void) 
  5.     ***************************************************************************/  
  6.     b2Vec2 GetSearchDirection() const  
  7.     {  
  8.         switch (m_count)  
  9.         {  
  10.         case 1:                                             //一个顶点从相反的方向去找  
  11.             return -m_v1.w;  
  12.             //两个顶点，1、两个点所组成边向量e12与第一个顶点和原点的所组成的向量-m_v1.w先去判断符号  
  13.             //          2、再求e12相应方向的法向量a  
  14.             //          3、将a作为查找方向返回  
  15.         case 2:                                               
  16.             {  
  17.                 //获取方向  
  18.                 b2Vec2 e12 = m_v2.w - m_v1.w;  
  19.                 float32 sgn = b2Cross(e12, -m_v1.w);  
  20.                 if (sgn > 0.0f)  
  21.                 {  
  22.                     // 原点在e12的左边  
  23.                     return b2Cross(1.0f, e12);  
  24.                 }  
  25.                 else  
  26.                 {  
  27.                     // 原点在e12的右边  
  28.                     return b2Cross(e12, 1.0f);  
  29.                 }  
  30.             }  
  31.             //这里不会出现多个点的情况，因为查找方向是对两个以下的顶点而言的，  
  32.             //没找到合适的第三个顶点之前永远都会将离原点最远的第三个点移除。  
  33.         default:  
  34.             b2Assert(false);  
  35.             return b2Vec2_zero;  
  36.         }  
  37.     }  
  38.     /************************************************************************** 
  39.     * 功能描述：获取最近的点 
  40.     * 参数说明： (void) 
  41.     * 返 回 值： (void) 
  42.     ***************************************************************************/  
  43.     b2Vec2 GetClosestPoint() const  
  44.     {  
  45.         //顶点数量  
  46.         switch (m_count)  
  47.         {  
  48.             //对于0个顶点，没有最近点这一说，所以我们在此处断言  
  49.         case 0:  
  50.             b2Assert(false);  
  51.             return b2Vec2_zero;  
  52.             //对于1个顶点，最近点就是那个顶点  
  53.         case 1:  
  54.             return m_v1.w;  
  55.             //对于两个顶点，重心是它们的最近点  
  56.         case 2:  
  57.             return m_v1.a * m_v1.w + m_v2.a * m_v2.w;  
  58.             //对于三个顶点来说，单纯形（此处是三角形）已形成，不需要在找最近点  
  59.             //故返回零向量  
  60.         case 3:  
  61.             return b2Vec2_zero;  
  62.             //多个点只有出现异常的时候会到这儿，故此处断言  
  63.         default:  
  64.             b2Assert(false);  
  65.             return b2Vec2_zero;  
  66.         }  
  67.     }  
  68.   
  69.     /************************************************************************** 
  70.     * 功能描述：获取见证点【形状上的参与求解的两个顶点】 
  71.     * 参数说明： pA：形状A上的点 
  72.                  pB：形状B上的点 
  73.     * 返 回 值： (void) 
  74.     ***************************************************************************/  
  75.     void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const  
  76.     {  
  77.         switch (m_count)  
  78.         {  
  79.             //对于0个点，还没有开始查找单纯形的顶点，不存在见证点，故断言  
  80.         case 0:  
  81.             b2Assert(false);  
  82.             break;  
  83.             //对于1个点，见证点就是wA和wB，直接赋值  
  84.         case 1:  
  85.             *pA = m_v1.wA;  
  86.             *pB = m_v1.wB;  
  87.             break;  
  88.             //对于2个点，我们用重心坐标求的的，至于为什么要用重心坐标，我们在下面的讲解中将会看到  
  89.         case 2:  
  90.             *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;  
  91.             *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;  
  92.             break;  
  93.             //对于3个点，我们可以判断此时两形状一碰撞了，见证点，就是两形状的碰撞点，故是一样的。  
  94.         case 3:  
  95.             *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;  
  96.             *pB = *pA;  
  97.             break;  
  98.   
  99.         default:  
  100.             b2Assert(false);  
  101.             break;  
  102.         }  
  103.     }  
  104.     /************************************************************************** 
  105.     * 功能描述：获取尺度（长度） 
  106.     * 参数说明： (void) 
  107.     * 返 回 值： 尺度（长度） 
  108.     ***************************************************************************/  
  109.     float32 GetMetric() const  
  110.     {  
  111.         switch (m_count)  
  112.         {  
  113.             //对于0个点，还没有开始查找单纯形的顶点，不存在长度，故断言  
  114.         case 0:  
  115.             b2Assert(false);  
  116.             return 0.0;  
  117.             // 对于1个点，只是一个点，没有长度，返回0  
  118.         case 1:  
  119.             return 0.0f;  
  120.             //对于2个点，计算其长度，并返回  
  121.         case 2:  
  122.             return b2Distance(m_v1.w, m_v2.w);  
  123.             //对于3个点，获取(m_v2.w - m_v1.w)与(m_v3.w - m_v1.w)  
  124.             //两向量组成的平行四边形面积  
  125.         case 3:  
  126.             return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);  
  127.             //不存在更多的点，如果到这里就出现了异常  
  128.         default:  
  129.             b2Assert(false);  
  130.             return 0.0f;  
  131.         }  
  132.     }  

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

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

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

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

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

 

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

 

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

[cpp] view plain copy
  1. /************************************************************************** 
  2.     * 功能描述：用重心坐标求解一条线 
  3.     p = a1 * w1 + a2 * w2 
  4.     a1 + a2 = 1 
  5.     向量垂直与从原点到最近点形成的线 
  6.     e12 = w2 -w1; 
  7.     dot(p,e) = 0 
  8.     a1 * dot(w1,e) + a2 * dot(w2,e) = 0 
  9.     2X2线性方程 
  10.     [1      1     ][a1] = [1] 
  11.     [w1.e12 w2.e12][a2] = [0] 
  12.     定义 
  13.     d12_1 = dot(w2,e12) 
  14.     d12_2 = -dot(21,e12) 
  15.     解决 
  16.     a1 = d12_1 /d12 
  17.     a2 = d12_2/ d12 
  18.     * 参数说明： (void) 
  19.     * 返 回 值： (void) 
  20.     ***************************************************************************/  
  21.     void Solve2();  
  22.     /************************************************************************** 
  23.     * 功能描述：可能区域： 
  24.                 1、points[2] 
  25.                 2、边缘形状 points[0] - points[2] 
  26.                 3、边缘形状 points[1] - points[2] 
  27.                 4、三角形内部 
  28.     * 参数说明： (void) 
  29.     * 返 回 值： (void) 
  30.     ***************************************************************************/  
  31.     void Solve3();  

实现部分。

[cpp] view plain copy
  1. //2个单纯顶点时，判断原点位置函数  
[cpp] view plain copy
  1. void b2Simplex::Solve2()  
  2. {  
  3.     b2Vec2 w1 = m_v1.w;  
  4.     b2Vec2 w2 = m_v2.w;  
  5.     b2Vec2 e12 = w2 - w1;  
  6.   
  7.     // w1区域  
  8.     float32 d12_2 = -b2Dot(w1, e12);  
  9.     if (d12_2 <= 0.0f)  
  10.     {  
  11.         // a2 <= 0, 所以我们将它置为0  
  12.         m_v1.a = 1.0f;  
  13.         m_count = 1;  
  14.         return;  
  15.     }  
  16.     // w2区域  
  17.     float32 d12_1 = b2Dot(w2, e12);  
  18.     if (d12_1 <= 0.0f)  
  19.     {  
  20.         // a1 <= 0,所以我们将它置为0  
  21.         m_v2.a = 1.0f;  
  22.         m_count = 1;  
  23.         m_v1 = m_v2;  
  24.         return;  
  25.     }  
  26.     //必须在e12区域  
  27.     float32 inv_d12 = 1.0f / (d12_1 + d12_2);  
  28.     m_v1.a = d12_1 * inv_d12;  
  29.     m_v2.a = d12_2 * inv_d12;  
  30.     m_count = 2;  
  31. }  
  32. //3个单纯顶点时，判断原点位置函数  
[cpp] view plain copy
  1. void b2Simplex::Solve3()  
  2. {  
  3.     b2Vec2 w1 = m_v1.w;  
  4.     b2Vec2 w2 = m_v2.w;  
  5.     b2Vec2 w3 = m_v3.w;  
  6.   
  7.     // Edge12  
  8.     // [1      1     ][a1] = [1]  
  9.     // [w1.e12 w2.e12][a2] = [0]  
  10.     // a3 = 0  
  11.     b2Vec2 e12 = w2 - w1;  
  12.     float32 w1e12 = b2Dot(w1, e12);  
  13.     float32 w2e12 = b2Dot(w2, e12);  
  14.     float32 d12_1 = w2e12;  
  15.     float32 d12_2 = -w1e12;  
  16.   
  17.     // Edge13  
  18.     // [1      1     ][a1] = [1]  
  19.     // [w1.e13 w3.e13][a3] = [0]  
  20.     // a2 = 0  
  21.     b2Vec2 e13 = w3 - w1;  
  22.     float32 w1e13 = b2Dot(w1, e13);  
  23.     float32 w3e13 = b2Dot(w3, e13);  
  24.     float32 d13_1 = w3e13;  
  25.     float32 d13_2 = -w1e13;  
  26.   
  27.     // Edge23  
  28.     // [1      1     ][a2] = [1]  
  29.     // [w2.e23 w3.e23][a3] = [0]  
  30.     // a1 = 0  
  31.     b2Vec2 e23 = w3 - w2;  
  32.     float32 w2e23 = b2Dot(w2, e23);  
  33.     float32 w3e23 = b2Dot(w3, e23);  
  34.     float32 d23_1 = w3e23;  
  35.     float32 d23_2 = -w2e23;  
  36.       
  37.     // 三角形e123  
  38.     float32 n123 = b2Cross(e12, e13);  
  39.   
  40.     float32 d123_1 = n123 * b2Cross(w2, w3);  
  41.     float32 d123_2 = n123 * b2Cross(w3, w1);  
  42.     float32 d123_3 = n123 * b2Cross(w1, w2);  
  43.   
  44.     // w1区域  
  45.     if (d12_2 <= 0.0f && d13_2 <= 0.0f)  
  46.     {  
  47.         m_v1.a = 1.0f;  
  48.         m_count = 1;  
  49.         return;  
  50.     }  
  51.   
  52.     // e12  
  53.     if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)  
  54.     {  
  55.         float32 inv_d12 = 1.0f / (d12_1 + d12_2);  
  56.         m_v1.a = d12_1 * inv_d12;  
  57.         m_v2.a = d12_2 * inv_d12;  
  58.         m_count = 2;  
  59.         return;  
  60.     }  
  61.   
  62.     // e13  
  63.     if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)  
  64.     {  
  65.         float32 inv_d13 = 1.0f / (d13_1 + d13_2);  
  66.         m_v1.a = d13_1 * inv_d13;  
  67.         m_v3.a = d13_2 * inv_d13;  
  68.         m_count = 2;  
  69.         m_v2 = m_v3;  
  70.         return;  
  71.     }  
  72.   
  73.     // w2区域  
  74.     if (d12_1 <= 0.0f && d23_2 <= 0.0f)  
  75.     {  
  76.         m_v2.a = 1.0f;  
  77.         m_count = 1;  
  78.         m_v1 = m_v2;  
  79.         return;  
  80.     }  
  81.   
  82.     // w3区域  
  83.     if (d13_1 <= 0.0f && d23_1 <= 0.0f)  
  84.     {  
  85.         m_v3.a = 1.0f;  
  86.         m_count = 1;  
  87.         m_v1 = m_v3;  
  88.         return;  
  89.     }  
  90.   
  91.     // e23  
  92.     if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)  
  93.     {  
  94.         float32 inv_d23 = 1.0f / (d23_1 + d23_2);  
  95.         m_v2.a = d23_1 * inv_d23;  
  96.         m_v3.a = d23_2 * inv_d23;  
  97.         m_count = 2;  
  98.         m_v1 = m_v3;  
  99.         return;  
  100.     }  
  101.   
  102.     //一定在三角形123内部  
  103.     float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);  
  104.     m_v1.a = d123_1 * inv_d123;  
  105.     m_v2.a = d123_2 * inv_d123;  
  106.     m_v3.a = d123_3 * inv_d123;  
  107.     m_count = 3;  
  108. }  

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

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

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

3、b2Distance方法的实现

 

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

[cpp] view plain copy
  1. //在两个形状间计算最近点  
  2. void b2Distance(b2DistanceOutput* output,  
  3.                 b2SimplexCache* cache,  
  4.                 const b2DistanceInput* input)  
  5. {  
  6.     ++b2_gjkCalls;  
  7.     //初始化代理  
  8.     const b2DistanceProxy* proxyA = &input->proxyA;  
  9.     const b2DistanceProxy* proxyB = &input->proxyB;  
  10.     //获取变换  
  11.     b2Transform transformA = input->transformA;  
  12.     b2Transform transformB = input->transformB;  
  13.     //初始化单纯形  
  14.     b2Simplex simplex;  
  15.     simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);  
  16.     //获取单纯形顶点  
  17.     b2SimplexVertex* vertices = &simplex.m_v1;  
  18.     const int32 k_maxIters = 20;  
  19.     // 存储最近的单纯形顶点，以便我们检测是否重复和阻止循环  
  20.     int32 saveA[3], saveB[3];  
  21.     int32 saveCount = 0;  
  22.     //获取单纯形最近的顶点，并获取该点与原点组成的向量的长度平方  
  23.     b2Vec2 closestPoint = simplex.GetClosestPoint();  
  24.     float32 distanceSqr1 = closestPoint.LengthSquared();  
  25.     float32 distanceSqr2 = distanceSqr1;  
  26.     //主迭代循环  
  27.     int32 iter = 0;  
  28.     while (iter < k_maxIters)  
  29.     {  
  30.         // 拷贝单纯形，我们可以识别重复  
  31.         saveCount = simplex.m_count;  
  32.         for (int32 i = 0; i < saveCount; ++i)  
  33.         {  
  34.             saveA[i] = vertices[i].indexA;  
  35.             saveB[i] = vertices[i].indexB;  
  36.         }  
  37.         //获取单纯形顶点,并判断  
  38.         switch (simplex.m_count)  
  39.         {  
  40.         case 1:  
  41.             break;  
  42.   
  43.         case 2:  
  44.             simplex.Solve2();  
  45.             break;  
  46.   
  47.         case 3:  
  48.             simplex.Solve3();  
  49.             break;  
  50.   
  51.         default:  
  52.             b2Assert(false);  
  53.         }  
  54.         // 如果我们有3个点，原点在相应的三角形内。  
  55.         if (simplex.m_count == 3)  
  56.         {  
  57.             break;  
  58.         }  
  59.         //计算最近的点  
  60.         b2Vec2 p = simplex.GetClosestPoint();  
  61.         distanceSqr2 = p.LengthSquared();  
  62.   
  63.           
  64.         //确保前进  
  65.         if (distanceSqr2 >= distanceSqr1)  
  66.         {  
  67.             //break;  
  68.         }  
  69.         distanceSqr1 = distanceSqr2;  
  70.         //获取查找方向  
  71.         b2Vec2 d = simplex.GetSearchDirection();  
  72.         //确保查找的方向是有效的  
  73.         if (d.LengthSquared() < b2_epsilon * b2_epsilon)  
  74.         {  
  75.             //原点有可能包含在线段或三角形内，因此形状是重叠的  
  76.             //尽管这里有可能重叠，我们不能在这里返回0  
  77.             //如果单纯形是一个点、线、三角形，它很难判断原点是否包含在在其中或者是非常靠近  
  78.             break;  
  79.         }  
  80.         //用支撑点计算一个试探性的新的单纯形顶点  
  81.         b2SimplexVertex* vertex = vertices + simplex.m_count;  
  82.         vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));  
  83.         vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));  
  84.         b2Vec2 wBLocal;  
  85.         vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));  
  86.         vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));  
  87.         vertex->w = vertex->wB - vertex->wA;  
  88.   
  89.         //迭代计数等于支撑点的调用数量  
  90.         ++iter;  
  91.         ++b2_gjkIters;  
  92.         //检测重复的支撑点，这是主要的终止条件  
  93.         bool duplicate = false;  
  94.         for (int32 i = 0; i < saveCount; ++i)  
  95.         {  
  96.             if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])  
  97.             {  
  98.                 duplicate = true;  
  99.                 break;  
  100.             }  
  101.         }  
  102.         // 如果我们发现了一个重复的支撑点我们必须退出循环  
  103.         if (duplicate)  
  104.         {  
  105.             break;  
  106.         }  
  107.         // 新的顶点是好的且需要的  
  108.         ++simplex.m_count;  
  109.     }  
  110.   
  111.     b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);  
  112.     // 准备输出  
  113.     simplex.GetWitnessPoints(&output->pointA, &output->pointB);  
  114.     output->distance = b2Distance(output->pointA, output->pointB);  
  115.     output->iterations = iter;  
  116.     //将单纯形写入缓存  
  117.     simplex.WriteCache(cache);  
  118.     // 如果被要求，则提供半径  
  119.     if (input->useRadii)  
  120.     {  
  121.         float32 rA = proxyA->m_radius;  
  122.         float32 rB = proxyB->m_radius;  
  123.   
  124.         if (output->distance > rA + rB && output->distance > b2_epsilon)  
  125.         {  
  126.             // 形状没有重叠,移动见证点到外面  
  127.             output->distance -= rA + rB;  
  128.             b2Vec2 normal = output->pointB - output->pointA;  
  129.             normal.Normalize();  
  130.             output->pointA += rA * normal;  
  131.             output->pointB -= rB * normal;  
  132.         }  
  133.         else  
  134.         {  
  135.             // 当形状重叠，移动见证点到中间  
  136.             b2Vec2 p = 0.5f * (output->pointA + output->pointB);  
  137.             output->pointA = p;  
  138.             output->pointB = p;  
  139.             output->distance = 0.0f;  
  140.         }  
  141.     }  
  142. }  

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

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

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

c)、将B写入缓存

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

 

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

 

ps：

 

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

0 0
吉林大学网络教学平台 吉林大学网络教学平台
原创粉丝点击
热门IT博客
淘宝店铺销售排行淘宝店铺销售排行
友曼网络友曼网络
淘宝维护权中心和介入
降临知乎
mac svn服务器地址
淘宝直通车溢价怎么算
淘宝免单优惠白菜群
linux boot 引导文件
巡洋舰 驱逐舰 知乎
网络贷款风险
linux重命名文件 空格
淘宝域名助手
python while无限循环
c++ 网络编程
csol剑伤害数据
蓝牙控制单片机程序
网络代购中心
态势感知软件
成都闪电seo
网站访客qq获取源码
热门问题 老师的惩罚 人脸识别 我在镇武司摸鱼那些年 重生之率土为王 我在大康的咸鱼生活 盘龙之生命进化 天生仙种 凡人之先天五行 春回大明朝 姑娘不必设防，我是瞎子 广告伞批发订做 庭院伞厂家 自动雨伞机 太阳伞定做厂家 儿童手工雨伞 太阳伞制作 广告伞多少钱 定做太阳伞 遮阳伞遮阳伞 礼品雨伞定制 定做广告伞 广告雨伞厂家 生产雨伞厂家 太阳伞厂 儿童雨伞舞蹈 户外庭院伞 钓鱼伞厂家 户外广告伞厂家 各种雨伞 雨伞定做厂家 广告伞制作厂家 伞制作 订制广告伞 飞行员跳伞后被挂在电线上 2米太阳伞 遮阳伞黑胶好还是银胶好 定做遮阳伞 伞下铭 伞下 蕉下伞 三尺伞下作品 伞下铭歌词 蕉下伞怎么收 蕉下小黑伞下雨能用吗 斗罗之守护伞下修罗 正品蕉下伞多少钱一把 伞修 自动伞坏了怎么修 伞兵 第一伞兵队 地狱伞兵

程序博客网，程序员的互联网技术博客家园。csdn论坛精品 msdn技术资料都在这里