当前位置：首页 > news >正文

部分算法记录

news 2025/9/17 5:15:05

  1 // 获取start到target的stepNumber个等分点
  2 public static bool GetPoints(Vector3 start, Vector3 target, float theta, int stepNumber, Vertex center, out List<Vector3> points)
  3         {
  4             points = new List<Vector3>();
  5 
  6             Vector3 normalVec = center.Normal;
  7 
  8             Vector3 localStartPoint = start - center.Position;
  9             Vector3 localEndPoint = target - center.Position;
 10 
 11             if (stepNumber > 0)
 12             {
 13                 for (int i = 1; i < stepNumber; i++)
 14                 {
 15                     Matrix4x4 rotateMat = Matrix4x4.CreateFromAxisAngle(normalVec, MathEx.Lerp(0, (float)theta, (float)i / stepNumber));
 16                     Vector3 localPos = Vector3.Transform(localStartPoint, rotateMat);
 17                     points.Add(center.Position + localPos);
 18                 }
 19                 points.Add(target);
 20             }
 21             return true;
 22         }
 23 
 24 // 检查三点是否共线
 25 private static bool ArePointsColinear(Vector3 p1, Vector3 p2, Vector3 p3)
 26         {
 27             // 计算向量
 28             Vector3 v1 = p2 - p1;
 29             Vector3 v2 = p3 - p1;
 30 
 31             // 计算叉积
 32             Vector3 cross = Vector3.Cross(v1, v2);
 33 
 34             // 如果叉积接近零向量，则三点共线
 35             return cross.LengthSquared() < 1e-10f;
 36         }
 37 
 38 // 计算平面法向量
 39 private static Vector3 CalculatePlaneNormal(Vector3 p1, Vector3 p2, Vector3 p3)
 40         {
 41             Vector3 v1 = p2 - p1;
 42             Vector3 v2 = p3 - p1;
 43             Vector3 normal = Vector3.Cross(v1, v2);
 44             return Vector3.Normalize(normal);
 45         }
 46 
 47 // 计算圆心（外心）
 48 private static Vector3 CalculateCircumcenter(Vector3 p0, Vector3 p1, Vector3 p2)
 49         {
 50             var center = Vector3.Zero;
 51             Vector3 v0 = Vector3.Normalize(p1 - p0);
 52             Vector3 v1 = Vector3.Normalize(p2 - p0);
 53             if (v0 == v1 || v0 == -v1) { throw new Exception("计算圆心错误！"); }
 54 
 55             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),
 56                    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),
 57                    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),
 58                    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);
 59 
 60             double a2 = 2 * (p1.X - p0.X),
 61                    b2 = 2 * (p1.Y - p0.Y),
 62                    c2 = 2 * (p1.Z - p0.Z),
 63                    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;
 64 
 65             double a3 = 2 * (p2.X - p0.X),
 66                    b3 = 2 * (p2.Y - p0.Y),
 67                    c3 = 2 * (p2.Z - p0.Z),
 68                    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;
 69 
 70             center.X = (float)(-(b1 * c2 * d3 - b1 * c3 * d2 - b2 * c1 * d3 + b2 * c3 * d1 + b3 * c1 * d2 - b3 * c2 * d1)
 71                 / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1));
 72             center.Y = (float)((a1 * c2 * d3 - a1 * c3 * d2 - a2 * c1 * d3 + a2 * c3 * d1 + a3 * c1 * d2 - a3 * c2 * d1)
 73                 / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1));
 74             center.Z = (float)(-(a1 * b2 * d3 - a1 * b3 * d2 - a2 * b1 * d3 + a2 * b3 * d1 + a3 * b1 * d2 - a3 * b2 * d1)
 75                 / (a1 * b2 * c3 - a1 * b3 * c2 - a2 * b1 * c3 + a2 * b3 * c1 + a3 * b1 * c2 - a3 * b2 * c1));
 76             return center;
 77         }
 78 
 79 // 计算弧度
 80 private static float CalculateMinorArcAngle(Vector3 start, Vector3 target, Vertex center)
 81         {
 82             //圆的法向量
 83             Vector3 normalVec = center.Normal;
 84 
 85             Vector3 localStartPoint = start - center.Position;
 86             Vector3 localEndPoint = target - center.Position;
 87             double theta = Math.Acos(Vector3.Dot(Vector3.Normalize(localStartPoint), Vector3.Normalize(localEndPoint)));
 88 
 89             //算出从起点旋转theta角度以及(2 * Math.PI - theta)角度的点去与终点比对
 90             Matrix4x4 startEndMat1 = Matrix4x4.CreateFromAxisAngle(normalVec, (float)theta);
 91             Vector3 localEndPos1 = Vector3.Transform(localStartPoint, startEndMat1);
 92             Matrix4x4 startEndMat2 = Matrix4x4.CreateFromAxisAngle(normalVec, (float)(2 * Math.PI - theta));
 93             Vector3 localEndPos2 = Vector3.Transform(localStartPoint, startEndMat2);
 94             if (Vector3.Distance(localEndPos1, localEndPoint) > Vector3.Distance(localEndPos2, localEndPoint))
 95             {
 96                 theta = 2 * Math.PI - theta;
 97             }
 98             return (float)theta;
 99         }
100 
101 // 从start经过middle到target，检测movingPoint是否经过了middle点
102 public static bool HasPassedMiddlePoint(Vector3 start, Vector3 middle, Vector3 target, Vector3 movingPoint)
103         {
104             // 计算路径向量
105             Vector3 pathVector = target - start;
106             float pathLength = pathVector.Length();
107 
108             // 如果路径长度接近0，说明起点和终点重合，无法形成有效路径
109             if (pathLength < float.Epsilon) return false;
110 
111             // 计算中间点在路径上的投影参数
112             Vector3 middleVector = middle - start;
113             float middleProjection = Vector3.Dot(middleVector, pathVector) / (pathLength * pathLength);
114 
115             // 计算运动点在路径上的投影参数
116             Vector3 movingVector = movingPoint - start;
117             float movingProjection = Vector3.Dot(movingVector, pathVector) / (pathLength * pathLength);
118 
119             // 检查运动点是否已经过了中间点的投影位置
120             bool hasPassedProjection = movingProjection > middleProjection;
121 
122             // 综合判断：在路径附近且投影位置已过中间点
123             return hasPassedProjection;
124         }