void GetMapCoordsFromIntel(IFleetIntelligence intel, TimeSpan localTime, out Vector3D localCoords, out Vector2 flatPosition, out Vector2 altitudePosition) { var worldCoords = intel.GetPositionFromCanonicalTime(localTime + IntelProvider.CanonicalTimeDiff); localCoords = WorldCoordsToLocalCoords(worldCoords); flatPosition = LocalCoordsToMapPosition(localCoords, true); altitudePosition = LocalCoordsToMapPosition(localCoords); }
public static Vector3D PlotPath(Dictionary <MyTuple <IntelItemType, long>, IFleetIntelligence> IntelItems, TimeSpan canonicalTime, IAutopilot Autopilot, Waypoint Destination, List <IFleetIntelligence> IntelScratchpad, List <Vector3> PositionScratchpad, MyGridProgram Program, WaypointTask.AvoidObstacleMode ObstacleMode) { if (Autopilot.Reference == null) { return(Vector3D.Zero); } Vector3D o = Autopilot.Reference.WorldMatrix.Translation; Vector3D targetPosition = Destination.Position; float SafetyRadius = (float)(Autopilot.Controller.CubeGrid.WorldAABB.Max - Autopilot.Controller.CubeGrid.WorldAABB.Min).Length(); float brakingDist = Autopilot.GetBrakingDistance(); IntelScratchpad.Clear(); PositionScratchpad.Clear(); bool type1 = false; // Quick filter on intel items that might interfere with pathing at this time based on type and distance foreach (var kvp in IntelItems) { // If it's us, don't care if (kvp.Key.Item2 == Program.Me.CubeGrid.EntityId) { continue; } if (kvp.Value.Radius == 0) { continue; } // If it's an asteroid or a ship, we might be interested if (kvp.Value.Type == IntelItemType.Asteroid || kvp.Value.Type == IntelItemType.Friendly || kvp.Value.Type == IntelItemType.Enemy) { Vector3D c = kvp.Value.GetPositionFromCanonicalTime(canonicalTime); float r = kvp.Value.Radius + SafetyRadius; double distTo = (c - o).Length(); // Check if distance is close enough. If so, shortlist this. if (distTo < r + brakingDist + Autopilot.Controller.GetShipSpeed() * 0.16 + 100) { IntelScratchpad.Add(kvp.Value); PositionScratchpad.Add(c); // If distance is closer than, we are inside its bounding sphere and must escape unless our destination is also in the radius if (distTo < r && (Destination.Position - c).Length() > r) { type1 = true; break; } } } } Vector3D target = Destination.Position; if (type1) { // Escape maneuver - move directly away from center of bounding sphere var dir = o - PositionScratchpad.Last(); dir.Normalize(); target = dir * (IntelScratchpad.Last().Radius + SafetyRadius * 2) + PositionScratchpad.Last(); } else if (IntelScratchpad.Count > 0) { bool targetClear; int iter = 0; // Find a clear path around any obstacles: do { iter += 1; targetClear = true; IFleetIntelligence closestObstacle = null; float closestDist = float.MaxValue; float closestApporoach = 0; bool closestType3 = false; var l = target - o; double d = l.Length(); l.Normalize(); // Go through each intel item we shortlisted earlier for (int i = 0; i < IntelScratchpad.Count; i++) { float lDoc = Vector3.Dot(l, o - PositionScratchpad[i]); double det = lDoc * lDoc - ((o - PositionScratchpad[i]).LengthSquared() - (IntelScratchpad[i].Radius + SafetyRadius) * (IntelScratchpad[i].Radius + SafetyRadius)); // Check if we intersect the sphere at all if (det > 0) { // Check if this is a type 2 obstacle - that is, we enter its bounding sphere and the closest approach is some point along our path. if (-lDoc > 0 && -lDoc < d) { closestObstacle = IntelScratchpad[i]; var distIntersect = -lDoc - (float)Math.Sqrt(det); // Only care about the closest one. Hopefully this works well enough in practice. if (closestDist > distIntersect) { closestDist = distIntersect; closestApporoach = -lDoc; closestObstacle = IntelScratchpad[i]; closestType3 = false; } } // Check if this is a type 3 obstacle - that is, we enter its bonding sphere and the destination is inside else if ((target - PositionScratchpad[i]).Length() < IntelScratchpad[i].Radius + SafetyRadius) { var distIntersect = -lDoc - (float)Math.Sqrt(det); if (closestDist > distIntersect) { closestDist = distIntersect; closestApporoach = -lDoc; closestObstacle = IntelScratchpad[i]; closestType3 = true; } } } } // If there is a potential collision if (closestDist != float.MaxValue) { targetClear = false; Vector3D closestObstaclePos = closestObstacle.GetPositionFromCanonicalTime(canonicalTime); Vector3D v; if (!closestType3) { var c = l * closestApporoach + o; Vector3D dir = c - closestObstaclePos; dir.Normalize(); v = dir * (closestObstacle.Radius + SafetyRadius * 2) + closestObstaclePos; var vdir = v - o; vdir.Normalize(); target = o + vdir * (o - Destination.Position).Length(); } else { Vector3D dirCenterToDest = target - closestObstaclePos; dirCenterToDest.Normalize(); Vector3D dirCenterToMe = o - closestObstaclePos; var distToMe = dirCenterToMe.Length(); dirCenterToMe.Normalize(); var angle = Math.Acos(Vector3.Dot(dirCenterToDest, dirCenterToMe)); if (angle < 0.2 && ObstacleMode == WaypointTask.AvoidObstacleMode.SmartEnter) { target = Destination.Position; break; } else if (angle > 0.6 && distToMe < (closestObstacle.Radius + SafetyRadius)) { target = dirCenterToMe * (closestObstacle.Radius + SafetyRadius * 2) + closestObstaclePos; break; } else { target = dirCenterToDest * (closestObstacle.Radius + SafetyRadius * 2) + closestObstaclePos; } } } } while (!targetClear && iter < 5); } return(target); }