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

点到面的最近距离

一点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 ab
float 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 p
void 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 b
float 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 p
void 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 b
float 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 b
float 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;
}