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);
}
