c# 三维向量常用运算函数

求点到直线垂足交点

 /// <summary>
    /// 求点到直线垂足交点
    /// </summary>
    /// <param name="point">A点坐标</param>
    /// <param name="linePoint1">直线上一个点的坐标</param>
    /// <param name="linePoint2">直线上另一个点的坐标</param>
    /// <returns></returns>
    public static Vector3 VerticalIntersection(Vector3 point, Vector3 linePoint1, Vector3 linePoint2)
    {
        Vector3 vec1 = point - linePoint1;
        Vector3 vec2 = linePoint2 - linePoint1;
        Vector3 vecProj = Vector3.Project(vec1, vec2);       
        Vector3 v = linePoint1 + vecProj;
        return v;
    }

点到直线距离

 /// <summary>
    /// 点到直线距离
    /// </summary>
    /// <param name="point">点坐标</param>
    /// <param name="linePoint1">直线上一个点的坐标</param>
    /// <param name="linePoint2">直线上另一个点的坐标</param>
    /// <returns></returns>
    public static float DisPoint2Line(Vector3 point,Vector3 linePoint1,Vector3 linePoint2)
    {
        Vector3 vec1 = point - linePoint1;
        Vector3 vec2 = linePoint2 - linePoint1;
        Vector3 vecProj = Vector3.Project(vec1, vec2);
        float dis =  Mathf.Sqrt(Mathf.Pow(Vector3.Magnitude(vec1), 2) - Mathf.Pow(Vector3.Magnitude(vecProj), 2));
        return dis;
    }

计算向量夹角

  /// <summary>
    /// 计算向量夹角
    /// </summary>
    public static float TryGetIntersectAngle(Vector3 a, Vector3 b)
    {
        float angle = Vector3.Angle(a, b);
        //Debug.Log("向量夹角：" + angle);
        return angle;
    }

计算AB与CD两条直线的交点

    /// <summary>
    /// 计算AB与CD两条线段的交点.
    /// </summary>
    /// <param name="a">A点</param>
    /// <param name="b">B点</param>
    /// <param name="c">C点</param>
    /// <param name="d">D点</param>
    /// <param name="intersectPos">AB与CD的交点</param>
    /// <returns>是否相交 true:相交 false:未相交</returns>
    public static Vector3 TryGetIntersectPoint(Vector3 a, Vector3 b, Vector3 c, Vector3 d)
    {
        Vector3 ab = b - a;
        Vector3 ca = a - c;
        Vector3 cd = d - c;

        //叉乘
        Vector3 v1 = Vector3.Cross(ca, cd);

        Vector3 v2 = Vector3.Cross(cd, ab);
        float ratio = Vector3.Dot(v1, v2) / v2.sqrMagnitude;
        Vector3 intersectPos = a + ab * ratio;

        return intersectPos;
    }

计算AB与CD两条线段的交点

    /// <summary>
    /// 计算AB与CD两条线段的交点.
    /// </summary>
    /// <param name="a">A点</param>
    /// <param name="b">B点</param>
    /// <param name="c">C点</param>
    /// <param name="d">D点</param>
    /// <param name="intersectPos">AB与CD的交点</param>
    /// <returns>是否相交 true:相交 false:未相交</returns>
    private bool TryGetIntersectPoint(Vector3 a, Vector3 b, Vector3 c, Vector3 d, out Vector3 intersectPos)
    {
        intersectPos = Vector3.zero;

        Vector3 ab = b - a;
        Vector3 ca = a - c;
        Vector3 cd = d - c;

        Vector3 v1 = Vector3.Cross(ca, cd);

        if (Mathf.Abs(Vector3.Dot(v1, ab)) > 1e-6)
        {
            // 不共面
            return false;
        }

        if (Vector3.Cross(ab, cd).sqrMagnitude <= 1e-6)
        {
            // 平行
            return false;
        }

        Vector3 ad = d - a;
        Vector3 cb = b - c;
        // 快速排斥
        if (Mathf.Min(a.x, b.x) > Mathf.Max(c.x, d.x) || Mathf.Max(a.x, b.x) < Mathf.Min(c.x, d.x)
           || Mathf.Min(a.y, b.y) > Mathf.Max(c.y, d.y) || Mathf.Max(a.y, b.y) < Mathf.Min(c.y, d.y)
           || Mathf.Min(a.z, b.z) > Mathf.Max(c.z, d.z) || Mathf.Max(a.z, b.z) < Mathf.Min(c.z, d.z)
        )
            return false;

        // 跨立试验
        if (Vector3.Dot(Vector3.Cross(-ca, ab), Vector3.Cross(ab, ad)) > 0
            && Vector3.Dot(Vector3.Cross(ca, cd), Vector3.Cross(cd, cb)) > 0)
        {
            Vector3 v2 = Vector3.Cross(cd, ab);
            float ratio = Vector3.Dot(v1, v2) / v2.sqrMagnitude;
            intersectPos = a + ab * ratio;
            return true;
        }

        return false;
    }

判断点是否在四边形内 （abcd为正方形时不准确）

/// <summary>
    /// 判断点是否在四边形内
    /// </summary>
    /// <param name="A">四边形角坐标</param>
    /// <param name="B">四边形角坐标</param>
    /// <param name="C">四边形角坐标</param>
    /// <param name="D">四边形角坐标</param>
    /// <param name="P">点</param>
    /// <returns></returns>
    public bool IsInsideTheQuadrilateral(Vector3 A, Vector3 B, Vector3 C, Vector3 D, Vector3 P)
    {
        bool isIn = false;
        //先计算出所需要的叉积
        Vector3 v0 = Vector3.Cross(A - D, P - D);
        Vector3 v1 = Vector3.Cross(B - A, P - A);
        Vector3 v2 = Vector3.Cross(C - B, P - B);
        Vector3 v3 = Vector3.Cross(D - C, P - C);
        if (Vector3.Dot(v0, v1) < 0 || Vector3.Dot(v0, v2) < 0 || Vector3.Dot(v0, v3) < 0)
        {
            Debug.Log("P点在四边形区域外部");
        }
        else
        {
            isIn = true;
            Debug.Log("P点在四边形区域内部");
        }
        return isIn;
    }

判断点p 是否在四边形abcd内

/// <summary>
    /// 判断点p 是否在四边形abcd内
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <param name="c"></param>
    /// <param name="d"></param>
    /// <param name="p"></param>
    /// <returns></returns>
    public static bool pInQuadrangle(Vector3 a, Vector3 b, Vector3 c, Vector3 d,
            Vector3 p)
    {
        double dTriangle = triangleArea(a, b, p) + triangleArea(b, c, p)
                + triangleArea(c, d, p) + triangleArea(d, a, p);
        double dQuadrangle = triangleArea(a, b, c) + triangleArea(c, d, a);
        return dTriangle == dQuadrangle;
    }
    // 返回三个点组成三角形的面积
    private static double triangleArea(Vector3 a, Vector3 b, Vector3 c)
    {
        double result = Math.Abs((a.x * b.z + b.x * c.z + c.x * a.z - b.x * a.z
                - c.x * b.z - a.x * c.z) / 2.0D);
        return result;
    }