| | | 1 | | using System; |
| | | 2 | | using CounterDrone.Core.Models; |
| | | 3 | | |
| | | 4 | | namespace CounterDrone.Core.Algorithms |
| | | 5 | | { |
| | | 6 | | /// <summary>通用运动学 / 几何工具</summary> |
| | | 7 | | public static class Kinematics |
| | | 8 | | { |
| | | 9 | | /// <summary>风向 → 单位速度矢量 (X, 0, Z)，右手系 Y-up</summary> |
| | | 10 | | public static (float X, float Y, float Z) WindToVector(WindDirection dir, float speed) |
| | 5930 | 11 | | { |
| | 5930 | 12 | | var angle = dir switch |
| | 5930 | 13 | | { |
| | 4642 | 14 | | WindDirection.N => 0f, |
| | 0 | 15 | | WindDirection.NE => 45f, |
| | 136 | 16 | | WindDirection.E => 90f, |
| | 0 | 17 | | WindDirection.SE => 135f, |
| | 0 | 18 | | WindDirection.S => 180f, |
| | 0 | 19 | | WindDirection.SW => 225f, |
| | 1152 | 20 | | WindDirection.W => 270f, |
| | 0 | 21 | | WindDirection.NW => 315f, |
| | 0 | 22 | | _ => 0f, |
| | 5930 | 23 | | }; |
| | 5930 | 24 | | var rad = angle * (float)Math.PI / 180f; |
| | 5930 | 25 | | return ((float)Math.Sin(rad) * speed, 0, (float)Math.Cos(rad) * speed); |
| | 5930 | 26 | | } |
| | | 27 | | |
| | | 28 | | /// <summary>计算抛物线炮弹的发射角</summary> |
| | | 29 | | /// <param name="range">水平距离 (m)</param> |
| | | 30 | | /// <param name="muzzleVelocity">初速 (m/s)</param> |
| | | 31 | | /// <param name="releaseAltitude">释放高度 (m)，弹道顶点必须 ≥ 此值</param> |
| | | 32 | | /// <returns>发射角 (rad)，取低弹道</returns> |
| | | 33 | | public static float CalculateLaunchAngle(float range, float muzzleVelocity, float releaseAltitude) |
| | 29 | 34 | | { |
| | 29 | 35 | | var v2 = muzzleVelocity * muzzleVelocity; |
| | 29 | 36 | | var g = 9.81f; |
| | | 37 | | |
| | | 38 | | // 1. 射程所需角：θr = arcsin(R*g/v²) / 2 |
| | 29 | 39 | | var rangeRatio = range * g / v2; |
| | 29 | 40 | | var angleRange = rangeRatio >= 1.0f |
| | 29 | 41 | | ? 45f * (float)Math.PI / 180f |
| | 29 | 42 | | : (float)Math.Asin(rangeRatio) / 2f; |
| | | 43 | | |
| | | 44 | | // 2. 释放高度所需最小角：H = v²*sin²(θ)/(2g) → sin(θ) = √(2gH)/v |
| | 29 | 45 | | var sinMinHeight = (float)Math.Sqrt(2f * g * releaseAltitude * 1.05f) / muzzleVelocity; |
| | 29 | 46 | | var angleHeight = sinMinHeight >= 1.0f |
| | 29 | 47 | | ? 90f * (float)Math.PI / 180f |
| | 29 | 48 | | : (float)Math.Asin(sinMinHeight); |
| | | 49 | | |
| | 29 | 50 | | return Math.Max(angleRange, angleHeight); |
| | 29 | 51 | | } |
| | | 52 | | |
| | | 53 | | /// <summary>根据发射参数计算抛物线位置</summary> |
| | | 54 | | public static (float X, float Y, float Z) ParabolicPosition( |
| | | 55 | | float startX, float startY, float startZ, |
| | | 56 | | float launchAngle, float azimuth, |
| | | 57 | | float muzzleVelocity, float time) |
| | 738 | 58 | | { |
| | 738 | 59 | | var cosA = (float)Math.Cos(launchAngle); |
| | 738 | 60 | | var sinA = (float)Math.Sin(launchAngle); |
| | 738 | 61 | | var g = 9.81f; |
| | | 62 | | |
| | 738 | 63 | | var horizontalDist = muzzleVelocity * cosA * time; |
| | 738 | 64 | | var x = startX + horizontalDist * (float)Math.Sin(azimuth); |
| | 738 | 65 | | var z = startZ + horizontalDist * (float)Math.Cos(azimuth); |
| | 738 | 66 | | var y = startY + muzzleVelocity * sinA * time - 0.5f * g * time * time; |
| | | 67 | | |
| | 738 | 68 | | return (x, y, z); |
| | 738 | 69 | | } |
| | | 70 | | |
| | | 71 | | /// <summary>点到矩形距离（简化判定）</summary> |
| | | 72 | | public static float Distance2D(float x1, float z1, float x2, float z2) |
| | 0 | 73 | | { |
| | 0 | 74 | | var dx = x2 - x1; |
| | 0 | 75 | | var dz = z2 - z1; |
| | 0 | 76 | | return (float)Math.Sqrt(dx * dx + dz * dz); |
| | 0 | 77 | | } |
| | | 78 | | |
| | | 79 | | /// <summary>点到三维点距离</summary> |
| | | 80 | | public static float Distance3D(float x1, float y1, float z1, float x2, float y2, float z2) |
| | 0 | 81 | | { |
| | 0 | 82 | | var dx = x2 - x1; |
| | 0 | 83 | | var dy = y2 - y1; |
| | 0 | 84 | | var dz = z2 - z1; |
| | 0 | 85 | | return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); |
| | 0 | 86 | | } |
| | | 87 | | |
| | | 88 | | /// <summary>射线法 — 点是否在水平多边形内</summary> |
| | | 89 | | public static bool PointInPolygon(float px, float pz, ReadOnlySpan<(float X, float Z)> vertices) |
| | 77 | 90 | | { |
| | 77 | 91 | | if (vertices.Length < 3) return false; |
| | 77 | 92 | | bool inside = false; |
| | 77 | 93 | | int j = vertices.Length - 1; |
| | 770 | 94 | | for (int i = 0; i < vertices.Length; i++) |
| | 308 | 95 | | { |
| | 616 | 96 | | var xi = vertices[i].X; var zi = vertices[i].Z; |
| | 616 | 97 | | var xj = vertices[j].X; var zj = vertices[j].Z; |
| | 308 | 98 | | if ((zi > pz) != (zj > pz) && px < (xj - xi) * (pz - zi) / (zj - zi) + xi) |
| | 145 | 99 | | inside = !inside; |
| | 308 | 100 | | j = i; |
| | 308 | 101 | | } |
| | 77 | 102 | | return inside; |
| | 77 | 103 | | } |
| | | 104 | | } |
| | | 105 | | } |