最近点计算_最近点投影法-CSDN博客

本文链接：https://blog.csdn.net/weileniupai/article/details/52551674

点到面的最近点

平面定义：空间中的一点P与法线n，所有在该平面上的点 X 都满足：

n \cdot (X - P) = 0

${n} \cdot (X - P )= 0$
求解：空间中的点 Q到平面的最近点：
1. 使用点 Q减去点 P，然后与平面的法线进行 Dot点积运算，求出了点到平面的距离 t。
2. 利用点 Q减去 t 乘于法线 n，则求出了平面上的点 R。


public class PointPlane
{
    #region 如果平面定义为： float pDistance & Vector3 pNormal
    public static float DistancePointPlane(float pDistance, Vector3 pNormal, Vector3 qPoint)
    {
        float distance = Vector3.Dot(pNormal, qPoint) - pDistance;
        return distance;
    }

    public static Vector3 ClosestPtPointPlane(float pDistance, Vector3 pNormal, Vector3 qPoint)
    {
        float distance = DistancePointPlane(pDistance, pNormal, qPoint);
        return qPoint - distance * pNormal;
    }
    #endregion

    #region 如果平面定义为： Vector pPoint & Vector3 pNormal
    public static float DistancePointPlane(Vector3 pPoint, Vector3 pNormal, Vector3 qPoint)
    {
        Vector3 line = qPoint - pPoint;
        float distance = Vector3.Dot(pNormal, line);
        return distance;
    }

    public static Vector3 ClosestPtPointPlane(Vector3 pPoint, Vector3 pNormal, Vector3 qPoint)
    {
        float distance = DistancePointPlane(pPoint, pNormal, qPoint);
        return qPoint - distance * pNormal;
    }
    #endregion
}

点到线段的最近点

直线上AB两点的线段可表示为

P (t) = A + t (B - A)

$P(t)=A + t (B-A)$
利用点积（dot）的性质，t可以表示C在线段AB上的投影，并且有：

t = (C - A) \cdot n / ∥ B - A ∥

$t = (C-A) \cdot {n}/ \left \| B - A \right \|$

n = (B - A) / ∥ B - A ∥

${n} = (B - A ) / \left \| B - A \right \|$
* 如果t小于0，表示点C的投影在A的左侧。所以最近点是A
* 如果t在0到1之间，说明C的投影在线段AB之间。
* 如果t大于1，表示点C的投影在B的右侧，所以最几点是B

public static Vector3 ClosestPtPointSegment(Vector3 a, Vector3 b, Vector3 c)
    {
        Vector3 d = Vector3.zero;
        Vector3 ab = b - a;//线段AB
        float t = Vector3.Dot(ab, c - a);
        if (t <= 0)//if t <= 0 , so t / ab.normal <= 0
        {
            t = 0;
            d = a;
        }
        else
        {
            float denom = Vector3.Dot(ab, ab);// ab.normal
            if (t >= denom)//t / ab.normal >= 1
            {
                t = 1;
                d = b;
            }
            else
            {
                t = t / denom;
                d = a + t * ab;
            }
        }
        return d;
    }

如果求C到AB见的最短距离，一个方法是直接用上面计算获得的点与C点距离。
* 当C点在A点的左侧时，距离是AC*AC的开方
* 当C点在B点的右侧时，距离是BC*BC的开方
* 当C点在AB线段的中间时,距离是CD *CD的开方，有:

D = A + A C \cdot A B A B \cdot A B A B

$D = A + \frac{AC \cdot AB}{AB \cdot AB} AB$

C D \cdot C D = A C * A C - ( A C \cdot A B ) 2 A B \cdot A B

$CD \cdot CD = AC * AC - \frac{(AC \cdot AB)^2}{AB \cdot AB}$
因此D点可以简化

 public static float DistancePointSegment(Vector3 a, Vector3 b, Vector3 c)
    {
        float distance = 0;
        Vector3 ab = b - a;
        Vector3 ac = c - a;
        Vector3 bc = c - b;
        float e = Vector3.Dot(ac, ab);
        if (e <= 0)
        {
            distance = Vector3.Dot(ac, ac);
        }
        else
        {
            float f = Vector3.Dot(ab, ab);
            if (e >= f)
            {
                distance = Vector3.Dot(bc,bc);
            }
            else
            {
                //简化计算
                distance = Vector3.Dot(ac, ac) - e * e / f;
            }
        }
        return Mathf.Sqrt(distance);
    }

点到AABB的最近点

AABB ： 指的是轴对齐包围盒。
* 当点在AABB中的时候，最近点是点本身
* 当点在AABB的一侧时，则点在AABB的Voronoi域上。


public class PointAABB
{

    #region AABB -> Vector3 min,Vectro3 max
    public static Vector3 ClosestPtPointAABB(Vector3 min,Vector3 max,Vector3 pPoint)
    {
        Vector3 qPoint = pPoint;//默认情况是点本身
        for(int i = 0 ; i < 3 ; i++)
        {
            if (qPoint[i] < min[i]) qPoint[i] = min[i];//如果点比最小点还小，那么最近点就是最小点【正交投影】
            else if (qPoint[i] > max[i]) qPoint[i] = max[i];//如果点比最大点还大，那么最近定就是最大点【正交投影】
        }
        return qPoint;

    }

    public static float DistancePointAABB(Vector3 min, Vector3 max, Vector3 pPoint)
    {
        float distance = 0;
        for (int i = 0; i < 3; i++)
        {
            if (pPoint[i] < min[i]) distance += Mathf.Pow(min[i] - pPoint[i], 2);
            else if (pPoint[i] > max[i]) distance += Mathf.Pow(max[i] - pPoint[i], 2);
        }
        return Mathf.Sqrt(distance);
    }

    #endregion
}

点在OBB的最近点

OBB ： 指的是带旋转的包围盒

解法一：将点P转化到OBB的空间坐标中，效果就是将OBB转化为AABB对待。直接使用AABB的解法

解法二：将点O与OBB之间的关系进行预处理

设定B为中心点位C的OBB，且3个正交向量单位u0,u1和u2分别定义了B的x，y和z轴。3个标量e0，e1和e2确定了沿各轴向的1/2轴长。则所有的B上的点S可以表示为 S = C + a*u0 + b*u1 + c*u2。其中|a| < e0,|b| < e1 , |c| < e2.

进一步转化可得出,x = (P - C) * u0 , y = (P - C) * u1 , z = (P - C) * u2


public class PointOBB
{
    public static Vector3 ClosestPtPointOBB(Vector3 center, Vector3[] u, Vector3 eSize, Vector3 pPoint)
    {
        Vector3 qPoint = center;
        Vector3 d = pPoint - center;
        for (int i = 0; i < 3; i++)
        {
            float dist = Vector3.Dot(d, u[i]);
            if (dist < -eSize[i]) dist = -eSize[i];
            else if (dist > eSize[i]) dist = eSize[i];

            qPoint += dist * u[i];
        }
        return qPoint;
    }


    public static float DistancePointOBB(Vector3 center, Vector3[] u, Vector3 eSize, Vector3 pPoint)
    {
        float distance = 0;
        Vector3 v = pPoint - center;
        for (int i = 0; i < 3; i++)
        {
            float dist = Vector3.Dot(u[i], v);
            float excess = 0;
            if (dist > eSize[i]) excess = dist - eSize[i];
            else if (dist < -eSize[i]) excess = dist + eSize[i];

            distance += excess * excess;
        }
        return Mathf.Sqrt(distance);
    }

}

点在3D矩形中的最近点

3D矩形可以看成是OBB包围盒，z轴的宽度为0


public class Point3DRect
{
    public static Vector3 ClosestPtPointRect(Vector3 center, Vector3[] u, Vector2 eSize, Vector3 pPoint)
    {
        Vector3 qPoint = center;
        Vector3 d = pPoint - center;
        for (int i = 0; i < 2; i++)
        {
            float dist = Vector3.Dot(d, u[i]);
            if (dist < -eSize[i]) dist = -eSize[i];
            else if (dist > eSize[i]) dist = eSize[i];

            qPoint += dist * u[i];
        }
        return qPoint;
    }
    public static float DistancePointRect(Vector3 center, Vector3[] u, Vector3 eSize, Vector3 pPoint)
    {
        float distance = 0;
        Vector3 v = pPoint - center;
        for (int i = 0; i < 3; i++)
        {
            float dist = Vector3.Dot(u[i], v);
            float excess = 0;
            if (dist > eSize[i]) excess = dist - eSize[i];
            else if (dist < -eSize[i]) excess = dist + eSize[i];

            distance += excess * excess;
        }
        return Mathf.Sqrt(distance);
    }

}

点在空间三角形中的最近点

点在三角形中最近点的可能分别为：
* 最近点为点P在三角形中的投影，中心域
* 最近点为三角形的三个顶点，顶点域
* 最近点为点P在三角形三条边上的投影，边域


public class PointTriangle
{
    //未简化版
    public static Vector3 ClosestPt_(Vector3 a, Vector3 b, Vector3 c, Vector3 p)
    {
        Vector3 ab = b - a;
        Vector3 ac = c - a;
        Vector3 bc = c - b;
        // Compute parametric position s for projection P’ of P on AB,
        // P’ = A + s*AB, s = snom/(snom+sdenom)
        float snom = Vector3.Dot(p - a, ab), sdenom = Vector3.Dot(p - b, a - b);
        // Compute parametric position t for projection P’ of P on AC,
        // P’ = A + t*AC, s = tnom/(tnom+tdenom)
        float tnom = Vector3.Dot(p - a, ac), tdenom = Vector3.Dot(p - c, a - c);
        //p在ab上的投影位于a后侧，p在ac上的投影位于c的后侧，因此p在a的顶点域上
        if (snom <= 0.0f && tnom <= 0.0f) return a; // Vertex region early out
        // Compute parametric position u for projection P’ of P on BC,
        // P’ = B + u*BC, u = unom/(unom+udenom)
        float unom = Vector3.Dot(p - b, bc), udenom = Vector3.Dot(p - c, b - c);
        //同理p位于b的顶点域上
        if (sdenom <= 0.0f && unom <= 0.0f) return b; // Vertex region early out
        //同理p位于c的顶点域上
        if (tdenom <= 0.0f && udenom <= 0.0f) return c; // Vertex region early out


        // P is outside (or on) AB if the triple scalar product [N PA PB] <= 0
        Vector3 n = Vector3.Cross(b - a, c - a);
        float vc = Vector3.Dot(n, Vector3.Cross(a - p, b - p));
        // If P outside AB and within feature region of AB,
        // return projection of P onto AB
        //因为vc<= 0 ,因此p位于ab的外部，并且p位于ab的中间，因此最近点位于ab边域上，剩下的可求出点p在ab上的投影则为最近点 （snom + sdenom） == Vector3.dot（ab，ab）
        //snom = (p.x -a.x) * (ab.x)
        //sdenom = (p.x - b.x) * ba.x
        //snom + sdenom = p.x * ab.x - a.x * ab.x  + p.x * ba.x - b.x * ba.x = p.x * ab.x - a.x * ab.x - p.x * ab.x + b.x * ab.x = b.x * ab.x - a.x * ab.x = ab.x * ab.x = Vector3.dot(ab,ab);
        if (vc <= 0.0f && snom >= 0.0f && sdenom >= 0.0f)
            return a + snom / (snom + sdenom) * ab;

        // P is outside (or on) BC if the triple scalar product [N PB PC] <= 0
        float va = Vector3.Dot(n, Vector3.Cross(b - p, c - p));
        // If P outside BC and within feature region of BC,
        // return projection of P onto BC
        if (va <= 0.0f && unom >= 0.0f && udenom >= 0.0f)
            return b + unom / (unom + udenom) * bc;

        // P is outside (or on) CA if the triple scalar product [N PC PA] <= 0
        float vb = Vector3.Dot(n, Vector3.Cross(c - p, a - p));
        // If P outside CA and within feature region of CA,
        // return projection of P onto CA
        if (vb <= 0.0f && tnom >= 0.0f && tdenom >= 0.0f)
            return a + tnom / (tnom + tdenom) * ac;

        //最近点在abc的中心域中，利用质心表达式求出p点在abc中的投影
        // P must project inside face region. Compute Q using barycentric coordinates
        float u = va / (va + vb + vc);
        float v = vb / (va + vb + vc);
        float w = 1.0f - u - v; // = vc / (va + vb + vc)
        return u * a + v * b + w * c;
    }

    public static Vector3 ClosestPt(Vector3 a, Vector3 b, Vector3 c, Vector3 p)
    {
        /**
        //(a × b) · (c × d) = (a · c)(b · d) − (a · d)(b · c)

        Vector n = Cross(b - a, c - a);
        float va = Dot(n, Cross(b - p, c - p));
        float vb = Dot(n, Cross(c - p, a - p));
        float vc = Dot(n, Cross(a - p, b - p));

        float d1 = Dot(b - a, p - a);
        float d2 = Dot(c - a, p - a);
        float d3 = Dot(b - a, p - b);
        float d4 = Dot(c - a, p - b);
        float d5 = Dot(b - a, p - c);
        float d6 = Dot(c - a, p - c); 

        float va = d3*d6 - d5*d4;
        float vb = d5*d2 - d1*d6;
        float vc = d1*d4 - d3*d2;
        **/

        // Check if P in vertex region outside A
        Vector3 ab = b - a;
        Vector3 ac = c - a;
        Vector3 ap = p - a;
        float d1 = Vector3.Dot(ab, ap);
        float d2 = Vector3.Dot(ac, ap);
        if (d1 <= 0.0f && d2 <= 0.0f) return a; // barycentric coordinates (1,0,0)

        // Check if P in vertex region outside B
        Vector3 bp = p - b;
        float d3 = Vector3.Dot(ab, bp);
        float d4 = Vector3.Dot(ac, bp);
        if (d3 >= 0.0f && d4 <= d3) return b; // barycentric coordinates (0,1,0)

        // Check if P in edge region of AB, if so return projection of P onto AB
        float vc = d1 * d4 - d3 * d2;
        if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
        {
            float v = d1 / (d1 - d3);
            return a + v * ab; // barycentric coordinates (1-v,v,0)
        }
        // Check if P in vertex region outside C
        Vector3 cp = p - c;
        float d5 = Vector3.Dot(ab, cp);
        float d6 = Vector3.Dot(ac, cp);
        if (d6 >= 0.0f && d5 <= d6) return c; // barycentric coordinates (0,0,1)
        // Check if P in edge region of AC, if so return projection of P onto AC
        float vb = d5 * d2 - d1 * d6;
        if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
        {
            float w = d2 / (d2 - d6);
            return a + w * ac; // barycentric coordinates (1-w,0,w)
        }
        // Check if P in edge region of BC, if so return projection of P onto BC
        float va = d3 * d6 - d5 * d4;
        if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
        {
            float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
            return b + w * (c - b); // barycentric coordinates (0,1-w,w)
        }
        // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
        float denom = 1.0f / (va + vb + vc);
        float v1 = vb * denom;
        float w1 = vc * denom;
        return a + ab * v1 + ac * w1; // = u*a + v*b + w*c, u = va * denom = 1.0f - v - w
    }
}

点距离空间中四面体的最近点

利用求点距离三角形的最近点的方式分别求出点距离四面体四个面的最近点，然后取出最近的点，如果点在四面体中，则最近点为点本身。当点P都不在四面体的四个面的表面上时，则可以判定点在四面体中心。因此在求点距离四面体当前面的最近点时，可以先判定点是否位于当前面的上方。


public class PointTetrahedron
{
    public static Vector3 ClosestPt(Vector3 a, Vector3 b, Vector3 c, Vector3 d, Vector3 p, ref float sqDist)
    {
        // Start out assuming point inside all halfspaces, so closest to itself
        Vector3 closestPt = p;
        float bestSqDist = float.MaxValue;
        // If point outside face abc then compute closest point on abc
        //如果在abc的表面
        if (PointOutsideOfPlane(p, a, b, c, d))
        {
            Vector3 q = PointTriangle.ClosestPt(a, b, c, p);
            sqDist = Vector3.Dot(q - p, q - p);
            // Update best closest point if (squared) distance is less than current best
            //根据最近点距离进行比较
            if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPt = q; }
        }
        // Repeat test for face acd
        if (PointOutsideOfPlane(p, a, c, d, b))
        {
            Vector3 q = PointTriangle.ClosestPt(a, c, d, p);
            sqDist = Vector3.Dot(q - p, q - p);
            if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPt = q; }
        }
        // Repeat test for face adb
        if (PointOutsideOfPlane(p, a, d, b, c))
        {
            Vector3 q = PointTriangle.ClosestPt(a, d, b, p);
            sqDist = Vector3.Dot(q - p, q - p);
            if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPt = q; }
        }
        // Repeat test for face bdc
        if (PointOutsideOfPlane(p, b, d, c, a))
        {
            Vector3 q = PointTriangle.ClosestPt(b, d, c, p);
            sqDist = Vector3.Dot(q - p, q - p);
            if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPt = q; }
        }
        //如果都没有位于四面体面的表面，则点p位于四面体的内部，因此最近点为点本身
        return closestPt;

    }


    //因为不能确定abc三角形的表面朝向，因此需要加入d辅助判断，d一定是位于abc的后面，因此当p与d与abc的表面向量（无须校验是正法）的点积相反时，
    //就可以确定点p位于abc的表面
    public static bool PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d)
    {
        float signp = Vector3.Dot(p - a, Vector3.Cross(b - a, c - a)); // [AP AB AC]
        float signd = Vector3.Dot(d - a, Vector3.Cross(b - a, c - a)); // [AD AB AC]
        // Points on opposite sides if expression signs are opposite
        return signp * signd < 0.0f;
    }

}