Bullet中最近点距离算法.docVIP

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???? ??? 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?segment4???? ??? ??? ??? ??? 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)?