部分算法记录

// 获取start到target的stepNumber个等分点
public static bool GetPoints(Vector3 start, Vector3 target, float theta, int stepNumber, Vertex center, out List<Vector3> points)
        {
            points = new List<Vector3>();

            Vector3 normalVec = center.Normal;

            Vector3 localStartPoint = start - center.Position;
            Vector3 localEndPoint = target - center.Position;

            if (stepNumber > 0)
            {
                for (int i = 1; i < stepNumber; i++)
                {
                    Matrix4x4 rotateMat = Matrix4x4.CreateFromAxisAngle(normalVec, MathEx.Lerp(0, (float)theta, (float)i / stepNumber));
                    Vector3 localPos = Vector3.Transform(localStartPoint, rotateMat);
                    points.Add(center.Position + localPos);
                }
                points.Add(target);
            }
            return true;
        }

// 检查三点是否共线
private static bool ArePointsColinear(Vector3 p1, Vector3 p2, Vector3 p3)
        {
            // 计算向量
            Vector3 v1 = p2 - p1;
            Vector3 v2 = p3 - p1;

            // 计算叉积
            Vector3 cross = Vector3.Cross(v1, v2);

            // 如果叉积接近零向量，则三点共线
            return cross.LengthSquared() < 1e-10f;
        }

// 计算平面法向量
private static Vector3 CalculatePlaneNormal(Vector3 p1, Vector3 p2, Vector3 p3)
        {
            Vector3 v1 = p2 - p1;
            Vector3 v2 = p3 - p1;
            Vector3 normal = Vector3.Cross(v1, v2);
            return Vector3.Normalize(normal);
        }

// 计算圆心（外心）
private static Vector3 CalculateCircumcenter(Vector3 p0, Vector3 p1, Vector3 p2)
        {
            var center = Vector3.Zero;
            Vector3 v0 = Vector3.Normalize(p1 - p0);
            Vector3 v1 = Vector3.Normalize(p2 - p0);
            if (v0 == v1 || v0 == -v1) { throw new Exception("计算圆心错误！"); }

            double a1 = (p0.Y * p1.Z - p1.Y * p0.Z - p0.Y * p2.Z + p2.Y * p0.Z + p1.Y * p2.Z - p2.Y * p1.Z),
                   b1 = -(p0.X * p1.Z - p1.X * p0.Z - p0.X * p2.Z + p2.X * p0.Z + p1.X * p2.Z - p2.X * p1.Z),
                   c1 = (p0.X * p1.Y - p1.X * p0.Y - p0.X * p2.Y + p2.X * p0.Y + p1.X * p2.Y - p2.X * p1.Y),
                   d1 = -(p0.X * p1.Y * p2.Z - p0.X * p2.Y * p1.Z - p1.X * p0.Y * p2.Z + p1.X * p2.Y * p0.Z + p2.X * p0.Y * p1.Z - p2.X * p1.Y * p0.Z);

            double a2 = 2 * (p1.X - p0.X),
                   b2 = 2 * (p1.Y - p0.Y),
                   c2 = 2 * (p1.Z - p0.Z),
                   d2 = p0.X * p0.X + p0.Y * p0.Y + p0.Z * p0.Z - p1.X * p1.X - p1.Y * p1.Y - p1.Z * p1.Z;

            double a3 = 2 * (p2.X - p0.X),
                   b3 = 2 * (p2.Y - p0.Y),
                   c3 = 2 * (p2.Z - p0.Z),
                   d3 = p0.X * p0.X + p0.Y * p0.Y + p0.Z * p0.Z - p2.X * p2.X - p2.Y * p2.Y - p2.Z * p2.Z;

            center.X = (float)(-(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1)
                / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1));
            center.Y = (float)((a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1)
                / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1));
            center.Z = (float)(-(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1)
                / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1));
            return center;
        }

// 计算弧度
private static float CalculateMinorArcAngle(Vector3 start, Vector3 target, Vertex center)
        {
            //圆的法向量
            Vector3 normalVec = center.Normal;

            Vector3 localStartPoint = start - center.Position;
            Vector3 localEndPoint = target - center.Position;
            double theta = Math.Acos(Vector3.Dot(Vector3.Normalize(localStartPoint), Vector3.Normalize(localEndPoint)));

            //算出从起点旋转theta角度以及(2 * Math.PI - theta)角度的点去与终点比对
            Matrix4x4 startEndMat1 = Matrix4x4.CreateFromAxisAngle(normalVec, (float)theta);
            Vector3 localEndPos1 = Vector3.Transform(localStartPoint, startEndMat1);
            Matrix4x4 startEndMat2 = Matrix4x4.CreateFromAxisAngle(normalVec, (float)(2 * Math.PI - theta));
            Vector3 localEndPos2 = Vector3.Transform(localStartPoint, startEndMat2);
            if (Vector3.Distance(localEndPos1, localEndPoint) > Vector3.Distance(localEndPos2, localEndPoint))
            {
                theta = 2 * Math.PI - theta;
            }
            return (float)theta;
        }

// 从start经过middle到target，检测movingPoint是否经过了middle点
public static bool HasPassedMiddlePoint(Vector3 start, Vector3 middle, Vector3 target, Vector3 movingPoint)
        {
            // 计算路径向量
            Vector3 pathVector = target - start;
            float pathLength = pathVector.Length();

            // 如果路径长度接近0，说明起点和终点重合，无法形成有效路径
            if (pathLength < float.Epsilon) return false;

            // 计算中间点在路径上的投影参数
            Vector3 middleVector = middle - start;
            float middleProjection = Vector3.Dot(middleVector, pathVector) / (pathLength * pathLength);

            // 计算运动点在路径上的投影参数
            Vector3 movingVector = movingPoint - start;
            float movingProjection = Vector3.Dot(movingVector, pathVector) / (pathLength * pathLength);

            // 检查运动点是否已经过了中间点的投影位置
            bool hasPassedProjection = movingProjection > middleProjection;

            // 综合判断：在路径附近且投影位置已过中间点
            return hasPassedProjection;
        }