Bullet中最近点距离算法_迈克老狼

http://blog.sina.com.cn/u/1644101601

首页博文目录关于我

个人资料

微博

加好友发纸条

写留言加关注

博客等级：
博客积分：

博客访问：
关注人气：
获赠金笔：0支
赠出金笔：0支
荣誉徽章：

正文字体大小：大中小

Bullet中最近点距离算法

(2010-11-20 21:25:00)

标签：

杂谈

分类：物理引擎

在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 
   
case 1://一个顶点

2 
   
{

3 
   
m_cachedP1 = m_simplexPointsP[0];

4 
   
m_cachedP2 = m_simplexPointsQ[0];

5 
   
m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]

6 
   
m_cachedBC.reset();

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

8 
   
m_cachedValidClosest = m_cachedBC.isValid();

9 
   
break;

10 
   
};

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

2、两个顶点的单纯形

1 
   
case 2://两个顶点

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 
 
8 
   
btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));

9 
   
btVector3 diff = p - from;

10 
   
btVector3 v = to - from;

11 
   
btScalar t = v.dot(diff);

12 
   
13 
   
if (t > 0) {

14 
   
btScalar dotVV = v.dot(v);

15 
   
if (t < dotVV) {

16 
   
t /= dotVV;

17 
   
diff -= t*v;

18 
   
m_cachedBC.m_usedVertices.usedVertexA = true;

19 
   
m_cachedBC.m_usedVertices.usedVertexB = true;

20 
   
} else {

21 
   
t = 1;

22 
   
diff -= v;

23 
   
//reduce to 1 point

24 
   
m_cachedBC.m_usedVertices.usedVertexB = true;

25 
   
}

26 
   
} else

27 
   
{

28 
   
t = 0;

29 
   
//reduce to 1 point

30 
   
m_cachedBC.m_usedVertices.usedVertexA = true;

31 
   
}

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

33 
   
nearest = from + t*v;

34 
 
35 
   
m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);

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

37 
   
m_cachedV = m_cachedP1 - m_cachedP2;

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

40 
 
41 
   
m_cachedValidClosest = m_cachedBC.isValid();

42 
   
break;

43 
   
}

44

两个顶点的单纯形就是求原点到线段的距离。如图1所示，在a的情况下，返回t点，在b的情况下返回to点，在c的情况下返回from。【bullet 代码，原来挺乱的】

m_cachedBC.setBarycentricCoordinates(1-t,t)，设置最近点的重心坐标。

http://s11/middle/61feffe14957bf2fb86ca&690

图1 原点到线段的距离

3、三个顶点的单纯形

求空间点到空间三角形最近距离（以及三角形上的最近距离点）的算法主要在函数closestPtPointTriangle中。

例如对于顶点a，如果原点位于它的Voronoi区域，则a点就是三角形到原点的最近距离。代码如下：

1 
    // Check if P in vertex region outside A

2 
    btVector3 ab = b - a;

3 
    btVector3 ac = c - a;

4 
    btVector3 ap = p - a;

5 
    btScalar d1 = ab.dot(ap);

6 
    btScalar d2 = ac.dot(ap);

7 
    if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) 

8 
   
{

9 
   
result.m_closestPointOnSimplex = a;

10 
   
result.m_usedVertices.usedVertexA = true;

11 
   
result.setBarycentricCoordinates(1,0,0);

12 
   
return true;// a; // barycentric coordinates (1,0,0)

13 
   
}

接下来判断P是否在b点的Voronoi区域（代码省略），然后判断线段p是否在ab的Voronoi区域，代码如下:

1 
    // Check if P in edge region of AB, if so return projection of P onto AB

2 
    btScalar vc = d1*d4 - d3*d2;

3 
   
//=(p-a).(((p-a)x(p-b))x(c-a)

4 
    if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {

5 
        btScalar v = d1 / (d1 - d3);

6 
   
result.m_closestPointOnSimplex = a + v * ab;

7 
   
result.m_usedVertices.usedVertexA = true;

8 
   
result.m_usedVertices.usedVertexB = true;

9 
   
result.setBarycentricCoordinates(1-v,v,0);

10 
   
return true;

11 
        //return a + v * ab; // barycentric coordinates (1-v,v,0)

12 
    }

注意：d1*d4-d3*d2 = (c-a).(((b-a)x(p-b))x(p-a)

接下来是点c，线段ac，bc，代码类似，不再贴出来。如果以上的测试都为false，则P点位于三角形内。

4、四个顶点的单纯形

主要通过函数closestPtPointTetrahedron计算最短距离。在该函数中，考虑到了四面体退化的问题，就是四个点在一个平面上，此时没有求最短距离，否则的话，分别对四个平面求到原点最短距离，求出其中最短的距离即为结果。

因为代码比较长，这儿就不贴出来了。

阅读┊ 收藏 ┊ 喜欢 ▼ ┊打印┊举报/Report

前一篇：三重积的公式

后一篇：一个有用的GJK帖子地址

新浪BLOG意见反馈留言板　欢迎批评指正

1	case 1://一个顶点
2	{
3	m_cachedP1 = m_simplexPointsP[0];
4	m_cachedP2 = m_simplexPointsQ[0];
5	m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
6	m_cachedBC.reset();
7	m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); //只使用了第一个顶点
8	m_cachedValidClosest = m_cachedBC.isValid();
9	break;
10	};

1	case 2://两个顶点
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
8	btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
9	btVector3 diff = p - from;
10	btVector3 v = to - from;
11	btScalar t = v.dot(diff);
12
13	if (t > 0) {
14	btScalar dotVV = v.dot(v);
15	if (t < dotVV) {
16	t /= dotVV;
17	diff -= t*v;
18	m_cachedBC.m_usedVertices.usedVertexA = true;
19	m_cachedBC.m_usedVertices.usedVertexB = true;
20	} else {
21	t = 1;
22	diff -= v;
23	//reduce to 1 point
24	m_cachedBC.m_usedVertices.usedVertexB = true;
25	}
26	} else
27	{
28	t = 0;
29	//reduce to 1 point
30	m_cachedBC.m_usedVertices.usedVertexA = true;
31	}
32	m_cachedBC.setBarycentricCoordinates(1-t,t);
33	nearest = from + t*v;
34
35	m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
36	m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
37	m_cachedV = m_cachedP1 - m_cachedP2;
38
39	reduceVertices(m_cachedBC.m_usedVertices); //移去没有用的顶点，对最近点没有贡献。这个在求两个物体最近距离时候要用到，没用的点从单纯形中去掉
40
41	m_cachedValidClosest = m_cachedBC.isValid();
42	break;
43	}
44

1	// Check if P in vertex region outside A
2	btVector3 ab = b - a;
3	btVector3 ac = c - a;
4	btVector3 ap = p - a;
5	btScalar d1 = ab.dot(ap);
6	btScalar d2 = ac.dot(ap);
7	if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
8	{
9	result.m_closestPointOnSimplex = a;
10	result.m_usedVertices.usedVertexA = true;
11	result.setBarycentricCoordinates(1,0,0);
12	return true;// a; // barycentric coordinates (1,0,0)
13	}

1	// Check if P in edge region of AB, if so return projection of P onto AB
2	btScalar vc = d1d4 - d3d2;
3	//=(p-a).(((p-a)x(p-b))x(c-a)
4	if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
5	btScalar v = d1 / (d1 - d3);
6	result.m_closestPointOnSimplex = a + v * ab;
7	result.m_usedVertices.usedVertexA = true;
8	result.m_usedVertices.usedVertexB = true;
9	result.setBarycentricCoordinates(1-v,v,0);
10	return true;
11	//return a + v * ab; // barycentric coordinates (1-v,v,0)
12	}