《实时碰撞检测算法技术》读书笔记（五）：最近点计算（上）

点到面的最近距离

一点P以及法线n定义了一个平面π，所有该平面上的点X都满足方程n·(X - P) = 0(即从点P指向X的向量垂直于n)。现令Q为空间内任意一点，则面内距Q的最近点R为点QQ在该面上的正交投影，即从顶点Q处向平面垂直（依据法线n）移动。同时，对于值t，有R = Q - tn，将该表达式代入平面方程求解t：

将t代入到R = Q - tn并得到投影点R：

从等式中不难发现t为正值，则Q位于该面的正面，t为负值则Q位于该面的背面。

当平面按照格式给出时，则有表达式。因此，一点距平面最近点的代码如下所示：

Point ClosestPtPointPlane(Point q, Plane p) {    float t = (Dot(p.n, q) - p.d) / Dot(p.n, p.n);    return q - t * p.n;}

 

如果平面方程采用单位向量，则t简化为t = (n·Q) - d。

Q至平面的有符号距离可以通过t的计算返回值得到：

float DistPointPlane(Point q, Plane p) {    //return Dot(p.n, q) - p.d; if plane equation normalized (||p.n|| == 1)    return (Dot(p.n, q) - p.d) / Dot(p.n, p.n);}

点至线段的最近点

令AB为两端点A、B定义的线段，给定任意一点C，该问题为：确定线段AB上距C的最近点D。可以将点C投影至AB延长线上，如果投影点P位于线段内，则点P为所求；若投影点P位于线段外部，则线段上的近C端点为最近点。

 

直线AB上的一点可参数化为P(t) = A+ t(B - A)。利用点积的投影性质，t可表示为C在直线上的投影，且t = (C - A)·n / ||B - A||，其中n = (B - A)/(||B - A||)即为AB方向上的单位向量。

若在线段内获取最近点，则有0<=t<=1，且在参数方程中代入t值即可得到最近点D。代码如下所示：

//Given segment ab and point c, copmutes closest point d on ab.//Also returns t for the position of d, d(t) = a + t*(b - a)void ClosestPtPointSegment(Point c, Point a, Point b, float &t, Point &d){    Vector ab = b - a;    //Progect c onto ab, computing parameterized position d(t) = a + t*(b - a)    t = Dot(c - a, ab) / Dot(ab, ab);    //If outside segment, clamp t(and therefore d) to the closest endpoint    if(t < 0.0f) t = 0.0f;    if(t > 1.0f) t = 1.0f;    //Compute projected position from the clamped t    d = a + t * ab;}

如果除法操作成本高昂，则可预先保存非负的分母值并延迟该除法操作。优化后的代码如下：

//Given segment ab and point c, copmutes closest point d on ab.//Also returns t for the position of d, d(t) = a + t*(b - a)void ClosestPtPointSegment(Point c, Point a, Point b, float &t, Point &d){    Vector ab = b - a;    //Project c onto ab,  but deferring divide by Dot(ab,ab)    t = Dot(c - a, ab) ;    if(t <= 0.0f) {        //c projects outside the [a, b] interval, on the a side;clamp to a        t = 0.0f;        d = a;    } else {        float denom = Dot(ab,ab);        if(t >= denom) {            //c projects outside the [a,b] interval, on the b side; clamp to b            t = 1.0f;            d = b;        } else {            //c projects inside the [a,b] interval;must do deferred divide now            t = t / denom;            d = a + t * ab;        }    }}

点到线段的距离

一点C与线段AB之间的（平方）距离可以直接获取，而无须显式计算线段AB上距C的最近点D。如前所诉，需要考查3种情形：当AC·AB <= 0 时，点A为距C的最近点且平方距离为AC·AC；当AC·AB >= AB·AB时，点B为距离C的最近点且平方距离为BC·BC；最后一种情况，当0 < AC·AB< AB·AB时，平方距离为CD·CD，其中

由于CD·CD可化简为

则点D无需计算。实现代码如下：

//Returns the squared distance between point c and segment abfloat SqDistPointSegment(Point a, Point b, Point c){    Vector ab = b - a, ac = c - a, bc = c - b;    float e = Dot(ac, ab);    //Handle cases where c projects outside ab    if(e <= 0.0f) return Dot(ac, ac);    float f = Dot(ab, ab);    if(e >= f) return Dot(bc,bc);    //Handle cases where c projects onto ab    return Dot(ac, ac) - e* e / f;}

点至AABB的最近点

//Given point p, return the point q on or in AABB b that is closest to pvoid ClosestPtPointAABB(Point p, AABB b, Point &q) {    .//For each coordinate axis, if the point coordinate value is    //outside box, clamp it to the box, else keep it as is    for(int i = 0; i < 3; i++) {        float v = p[i];        if(v < b.min[i]) v = b.min[i];    //v = max(v, b.min[i]);        if(v > b.max[i]) v = b.max[i];    //v = min(v, b.max[i]);        q[i] = v;    }}

在支持SMID指令的CPU体系结构中，上诉函数常实现为两条指令：max指令及其后的min指令。

点到AABB的距离

对于顶点P，当获取包围盒上的最近点Q并计算两者距离时，实际上无须显式计算最近点Q，如下代码所示（为了简化计算呢过程并避免昂贵的开方计算，代码中使用了平方距离）：

//Compute the square distance between a point and an AABB bfloat SqDistPointAABB(Point p, AABB b){    float sqDist = 0.0f;    for(int i = 0; i < 3; i++) {        //For each axis count any excess distance outside the box extents        float v = p[i];        if(v < b.min[i]) sqDist += (b.min[i] - v) * (b.min[i] - v);        if(v > b.max[i]) sqDist += (v - b.max[i]) * (v - b.max[i]);    }    return sqDist;}

点至OBB的最近点

设定B为中心点为C的OBB，且3个正交单位向量u0、u1和u2定义了B的x、y及z轴。3个标量e0、e1、e2确定了沿各轴向的1/2轴长。在该表达方式中，所有B中的点S可记为S = C + au0 + bu1 + cu2，其中|a| <= e0, |b| <= e1, |c| <= e2。

世界坐标空间中的一点P与OBB局部坐标空间中的一点Q = （x，y，z）之间存在下列关系：P = C + xu0 + yu1 + zu2.给定一点P，其OBB空间坐标可按下列方式计算。以下只给出坐标x的推导过程：

于是将顶点P转换至OBB的局部坐标系统中，并计算OBB上距该转换点的最近点，最后将结果转换回世界坐标系统。代码如下：

//Given point p, return point q on (or in) OBB b, closest to pvoid ClosestPtPointOBB(Point p, OBB b, Point &q){    Vector d = p - b.c;    //Start result at center of box; make steps from there    q = b.c;    //For each OBB axis...    for(int i = 0; i < 3; i++) {         //...project d onto that axis to get the distance        //along the axis of d from the box center        float dist = Dot(d, b.u[i]);        //if distance farther than the box extents, clamp to the box        if(dist > b.e[i]) dist = b.e[i];        if(disr < -b.e[i]) dist = -b.e[i];        //Step that distance along the axis to get world coordinate        q += dist * b.u[i];    }}

 

点到OBB的距离

计算P与OBB上最近点之间的平方距离时，前述函数可调用如下：

//Compute the square distance between point p and OBB bfloat SqDistPointOBB(Point p, OBB b){    Point closest;    ClosestPtPointOBB(p, b, closest);    float sqDist = Dot(closest - p, closest - p);    return sqDist;}

可以只计算平方距离而不获取最近点。

//Compute the square distance between point p and OBB bfloat SqDistPointOBB(Point p, OBB b){    Vector v = p - b.c;    float sqDist = 0.0f;    for(int i = 0; i < 3; i++) {        //Project vector from box center to p on each axis, getting the distance        //of p along that axis, and count any excess distance outside box extents        float d = Dot(v, b.u[i]), excess = 0.0f;        if(d < -b.e[i])            excess = d + b.e[i];        else if(d > b.e[i])            excess = d - b.e[i];        sqDist += excess * excess;    }    return sqDist;}