unity 线段到三角形的最近点

Geometric tools for computer graphics

3D Game Engine Design Second Edition

real-time collision detection

三本书查看相关数学知识，两本比较繁，用了第三本的简单粗暴法，也不知道有没有什么问题，先往下走了。效果还行。

    public Vector3 segmentCloestPointTriangle(Vector3 P, Vector3 Q, Vector3 A, Vector3 B, Vector3 C)
    {
        float s = -1.0f;
        float t = -1.0f;
        Vector3 c1 = Vector3.zero;
        Vector3 c2 = Vector3.zero;
        Vector3 PQAB = ClosestPtSegmentSegment(P, Q, A, B, ref s, ref t, ref c1, ref c2);
        float PQABmag = PQAB.magnitude;
        Vector3 PQBC = ClosestPtSegmentSegment(P, Q, B, C, ref s, ref t, ref c1, ref c2);
        float PQBCmag = PQBC.magnitude;
        Vector3 PQAC = ClosestPtSegmentSegment(P, Q, A, C, ref s, ref t, ref c1, ref c2);
        float PQACmag = PQAC.magnitude;
        Vector3 PABC = CloestPointTriangle1(P, A, B, C);
        float PABCmag = PABC.magnitude;
        Vector3 QABC = CloestPointTriangle1(Q, A, B, C);
        float QABCmag = QABC.magnitude;

        float minmag = Mathf.Min(PQABmag, PQBCmag, PQACmag, PABCmag, QABCmag);
        if (minmag == PQABmag)
        {
            s = -1.0f;
            t = -1.0f;
            c1 = Vector3.zero;
            c2 = Vector3.zero;
            return ClosestPtSegmentSegment(P, Q, A, B, ref s, ref t, ref c1, ref c2);
        }
        else if (minmag == PQBCmag)
        {
            s = -1.0f;
            t = -1.0f;
            c1 = Vector3.zero;
            c2 = Vector3.zero;
            return ClosestPtSegmentSegment(P, Q, B, C, ref s, ref t, ref c1, ref c2);
        }
        else if (minmag == PQACmag)
        {
            s = -1.0f;
            t = -1.0f;
            c1 = Vector3.zero;
            c2 = Vector3.zero;
            return ClosestPtSegmentSegment(P, Q, A, C, ref s, ref t, ref c1, ref c2);
        }
        else if (minmag == PABCmag)
        {
            s = -1.0f;
            t = -1.0f;
            c1 = Vector3.zero;
            c2 = Vector3.zero;
            Vector3 dispointtri = CloestPointTriangle1(P, A, B, C);
            return (P - dispointtri);
        }
        else /*if (minmag == QABCmag)*/
        {
            s = -1.0f;
            t = -1.0f;
            c1 = Vector3.zero;
            c2 = Vector3.zero;
            Vector3 dispointtri = CloestPointTriangle1(Q, A, B, C);
            return (Q - dispointtri);
        }
    }

 Vector3 ClosestPtSegmentSegment(Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2,
                                      ref float s, ref float t, ref Vector3 c1, ref Vector3 c2)
    {
        Vector3 d1 = q1 - p1;
        Vector3 d2 = q2 - p2;
        Vector3 r = p1 - p2;
        float a = Vector3.Dot(d1, d1);
        float e = Vector3.Dot(d2, d2);
        float f = Vector3.Dot(d2, r);

        //check if either or both segments degenerate into points
        //检查是否有一段或两段退化为点  
        if (a <= float.Epsilon && e <= float.Epsilon)//微小浮点值//a=e=0--d1=d2=0
        {
            //both segments degenerate into points
            //两段都退化成点
            s = t = 0.0f;
            c1 = p1;
            c2 = p2;
            return c1 - c2;
        }
        if (a <= float.Epsilon)//a=0--d1=0
        {
            //first segment degenerates into a point
            //第一线段退化成一个点
            s = 0.0f;
            t = f / e;
            t = Clamp(t, 0.0f, 1.0f);
        }
        else
        {
            float c = Vector3.Dot(d1, r);
            if (e <= float.Epsilon)//e=0--d2=0
            {
                //second segment degenerates into a point
                //第二线段退化成一个点
                t = 0.0f;
                s = Clamp(-c / a, 0.0f, 1.0f);
            }
            else
            {
                //the general nondegenerate case starts here
                //一般的非简并情况从这里开始
                float b = Vector3.Dot(d1, d2);
                float denom = a * e - b * b; //always nonnegative--zz总是非负

                //if segments not parallel,compute closest point on L1 to L2 and
                //clamp to segment S1. else pick arbitrary s(here 0)
                //如果线段不平行，计算L1到L2上最近的点，并夹紧线段sl。
                //否则取任意s(这里denom为0)  
                if (denom != 0.0f)
                {
                    s = Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
                }
                else
                {
                    s = 0.0f;
                }
                //compute point on L2 closest to s1(s) using
                //t=Dot((P1+D1*s)-P2,D2)/Dot(D2, D2)=(b*s+f)/e
                //计算L2上最接近s1(s)的点
                t = (b * s + f) / e;

                //if t in [0,1] done.else clamp t,recompute s for the new value
                //of t using s=Dot((P2+D2*s)-P1,D1)/Dot(D1, D1)=(t*b-c)/a
                //and clamp s to [0,1]
                //如果[0,1]中的t已完成。
                //其他情况就夹t，使用s重新计算新的t值，夹s为[0,1]  
                if (t < 0.0f)
                {
                    t = 0.0f;
                    s = Clamp(-c / a, 0.0f, 1.0f);
                }
                else if (t > 1.0f)
                {
                    t = 1.0f;
                    s = Clamp((b - c) / a, 0.0f, 1.0f);
                }
            }
        }
        c1 = p1 + d1 * s;
        c2 = p2 + d2 * t;
        return c1 - c2;
    }

public Vector3 CloestPointTriangle1(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
    {
        //检查P是否在顶点区域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 <= 0f && d2 <= 0f)
            return a;

        //检查P是否在顶点区域B之外
        Vector3 bp = p - b;
        float d3 = Vector3.Dot(ab, bp);
        float d4 = Vector3.Dot(ac, bp);
        if (d3 >= 0f && d4 <= d3)
            return b;

        //检查P是否在顶点区域C之外
        Vector3 cp = p - c;
        float d5 = Vector3.Dot(ab, cp);
        float d6 = Vector3.Dot(ac, cp);
        if (d6 >= 0f && d5 <= d6)
            return c;

        //检查P是否在AB的边缘区域，如果是，则返回P在AB上的投影
        float vc = d1 * d4 - d3 * d2;
        if (vc <= 0f && d1 >= 0f && d3 <= 0f)
        {
            float v1 = d1 / (d1 - d3);
            return (a + v1 * ab);
        }

        //检查P是否在AC的边缘区域，如果是，则返回P在AC上的投影
        float vb = d5 * d2 - d1 * d6;
        if (vb <= 0f && d2 >= 0f && d6 <= 0f)
        {
            float w1 = d2 / (d2 - d6);
            return (a + w1 * ac);
        }

        //检查P是否在BC的边缘区域，如果是，则返回P在BC上的投影
        float va = d3 * d6 - d5 * d4;
        if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
        {
            float w1 = (d4 - d3) / ((d4 - d3) + (d5 - d6));
            return (b + w1 * (c - b));
        }

        //P面内侧区域。通过其重心坐标(u.v.w)计算Q(面内最近点)
        float denom = 1f / (va + vb + vc);
        float v = vb * denom;
        float w = vc * denom;
        return (a + ab * v + ac * w);
    }