Bullet中最近点距离算法

Bullet中最近点距离算法

在Bullet中，通过类btVoronoiSimplexSolve实现了1到4个顶点的单纯形到原点的距离计算，该类可以在GJK算法中调用，用以代替Johnson distance algorithm[我们前篇文章原点到四面体的距离，实际上就是介绍该算法]算法。

本文我们研究bullet中如何实现该类以及该类的用法。

首先我们看看顶点在btVoronoiSimplexSolve中是什么存储的：

用到了三个变量：

btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS];

btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS];

btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS];

m_simplexVectorW中存放的是单纯形的顶点，m_simplexVectorP,

m_simplexVectorQ中存放的是两个物体中和单纯形顶点m_simplexVectorW相对应的顶点，注意，这儿单纯形表示是明可夫斯基差形状中的单纯形，所以m_simplexVectorP,

m_simplexVectorQ就是表示两个物体中的顶点，并且m_simplexVectorP -

m_simplexVectorQ = m_simplexVectorW。（都是世界坐标系中的值）

该类中计算机单纯形到原点距离通过函数closest计算，该函数又调用函数updateClosestVectorAndPoints具体实施。下面我看看updateClosestVectorAndPoints中的代码：

1、一个顶点的单纯形

1

case1://一个顶点

2

{

3m_cachedP1 = m_simplexPointsP[0];

4

m_cachedP2 = m_simplexPointsQ[0];

5m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVe ctorW[0]

6

m_cachedBC.reset();

7m_cachedBC.setBarycentricCoordinates(btScalar(1.),b tScalar(0.),btScalar(0.),btScalar(0.)); //只使用了第一个顶点

8

m_cachedValidClosest = m_cachedBC.isValid();

9

break;

1

0};

单纯形是一个顶点的情况下，直接返回m_simplexVectorW（m_simplexVectorP - m_simplexVectorQ）顶点坐标。单纯形到原点的距离放在向量m_cachedV中。

2、两个顶点的单纯形

1

case2://两个顶点

2

{

3

//closest point origin from line segment

4

const btVector3& from = m_simplexVectorW[0];

5

const btVector3& to = m_simplexVectorW[1];

6

btVector3 nearest;

7

8btVector3 p (btScalar(0.),btScalar(0.),btScalar (0.));

9

btVector3 diff = p - from;

1

btVector3 v = to - from;

1

1btScalar t = v.dot(diff);

1

2

1

3if (t >0) {

1

4btScalar dotVV = v.dot(v);

1

5if (t < dotVV) {

1

6t /= dotVV;

1

7diff -= t*v;

1 8

m_cachedBC.m_edVertexA = true;

1 9

m_cachedBC.m_edVertexB = true;

2

0} else {

2

1t =1;

2

2diff -= v;

2

3//reduce to 1 point

2 4

m_cachedBC.m_edVertexB = true;

2}

2

6} else

2

7{

2

8t =0;

2

9//reduce to 1 point

3 0

m_cachedBC.m_edVertexA =tru e;

3

1}

3

2m_cachedBC.setBarycentricCoordinates(1-t,t);

3

3nearest = from + t*v;

3

4

3 5

m_cachedP1 = m_simplexPointsP[0] + t * (m_simpl exPointsP[1] - m_simplexPointsP[0]);

3 6

m_cachedP2 = m_simplexPointsQ[0] + t * (m_simpl exPointsQ[1] - m_simplexPointsQ[0]);

3

7m_cachedV = m_cachedP1 - m_cachedP2;

3

8

3 9

reduceVertices(m_cachedBC.m_usedVertices); //移去没有用的顶点，对最近点没有贡献。这个在求两个物体最近距离时候要用到，没用的点从单纯形中去掉