EscapistsReintroduced/addons/ai/Scripts/Game/ERR/ERR_ConvoyRoadPlan.c

// Deterministic projection/sampling adapted from the owner's road-polyline helper.
// One explicit seed selects the nearest valid native road within 500m; no random or offroad fallback.
class ERR_ConvoyRoadPlan
{
 static bool Sample(array<vector> points, float distance, out vector position, out vector direction)
 {
  if (distance < 0 || !ERR_ForceRules.Finite(distance)) return false;
  for (int i = 1; i < points.Count(); i++)
  {
   float length = vector.DistanceXZ(points[i - 1], points[i]);
   if (length < 0.01) continue;
   if (distance <= length)
   {
    position = vector.Lerp(points[i - 1], points[i], distance / length);
    direction = points[i] - points[i - 1]; direction[1] = 0; direction.Normalize();
    return true;
   }
   distance -= length;
  }
  return false;
 }
 static bool CandidateStart(float length, float projected, int candidateIndex, out float start)
 {
  if (!ERR_ForceRules.Finite(length) || !ERR_ForceRules.Finite(projected) || length < 390 || projected < 0 || projected > length || candidateIndex < 0 || candidateIndex >= 33) return false;
  float preferred = Math.Clamp(projected - 87.5, 15, length - 375);
  float offset;
  if (candidateIndex > 0)
  {
   int step = (candidateIndex + 1) / 2;
   offset = step * 35;
   if (candidateIndex % 2 == 0) offset = -offset;
  }
  start = preferred + offset;
  // Refuse outside offsets; clamping would repeatedly test the same formation.
  return start >= 15 && start <= length - 375;
 }
 static bool FromPolyline(array<vector> source, vector seed, out array<vector> positions, out array<vector> angles, out ERR_ForceRoute route, out float proximity, int candidateIndex = 0)
 {
  positions = {}; angles = {}; route = null; proximity = 1e20;
  if (!source || source.Count() < 2 || source.Count() > 1024 || !ERR_ForceRules.FiniteVector(seed)) return false;
  array<vector> points = {};
  float length, projected, nearest = 1e20;
  foreach (vector point : source)
  {
   if (!ERR_ForceRules.FiniteVector(point)) return false;
   if (points.IsEmpty() || vector.DistanceXZ(points[points.Count() - 1], point) >= 0.01) points.Insert(point);
  }
  if (points.Count() < 2) return false;
  for (int i = 1; i < points.Count(); i++)
  {
   vector delta = points[i] - points[i - 1]; delta[1] = 0;
   vector offset = seed - points[i - 1]; offset[1] = 0;
   float segment = delta.Length();
   float fraction = Math.Clamp(vector.Dot(offset, delta) / (segment * segment), 0, 1);
   vector closest = vector.Lerp(points[i - 1], points[i], fraction);
   float distance = vector.DistanceXZ(seed, closest);
   if (distance < nearest) { nearest = distance; projected = length + fraction * segment; }
   length += segment;
  }
  // 205m is the minimum formation road. A 200m forward tail also allows the
  // lead vehicle to travel beyond the native 35m waypoint completion radius.
  if (!ERR_ForceRules.Finite(length) || length < 390 || nearest > 500) return false;
  // Orient along the same authored road toward its longer remaining end.
  if (projected > length * 0.5)
  {
   array<vector> reversed = {};
   for (int i = points.Count() - 1; i >= 0; i--) reversed.Insert(points[i]);
   points = reversed; projected = length - projected;
  }
  float start;
  if (!CandidateStart(length, projected, candidateIndex, start)) return false;
  float leadDistance = start + 175;
  for (int vehicle = 0; vehicle < 6; vehicle++)
  {
   vector position, direction;
   if (!Sample(points, leadDistance - vehicle * 35, position, direction)) return false;
   if (vector.DistanceXZ(position, seed) > 500) return false;
   foreach (vector previous : positions) if (vector.DistanceXZ(previous, position) < 18) return false;
   positions.Insert(position);
   float heading = ERR_ForceRules.Heading(position, position + direction, 0);
   angles.Insert(Vector(heading, 0, 0));
  }
  ERR_ForceRoute candidate = new ERR_ForceRoute();
  if (!candidate.Add(positions[0])) return false;
  float traversed;
  for (int i = 1; i < points.Count(); i++)
  {
   traversed += vector.DistanceXZ(points[i - 1], points[i]);
   if (traversed <= leadDistance) continue;
   // Retain every intermediate bend. Never connect omitted vertices with an
   // offroad chord to fit the 64-point route cap.
   if (!candidate.Add(points[i])) return false;
   if (traversed >= leadDistance + 200) break;
  }
  if (!candidate.Seal() || candidate.Length() < 200) return false;
  vector center, centerDirection;
  if (!Sample(points, start + 87.5, center, centerDirection)) return false;
  proximity = vector.DistanceXZ(center, seed);
  if (proximity > 500) return false;
  route = candidate;
  return true;
 }
 static bool Prefer(float distance, array<vector> positions, ERR_ForceRoute route, float bestDistance, array<vector> bestPositions, ERR_ForceRoute bestRoute)
 {
  if (!bestRoute || distance < bestDistance) return true;
  if (distance > bestDistance) return false;
  for (int i = 0; i < positions.Count(); i++)
  {
   for (int axis = 0; axis < 3; axis++)
   {
    if (positions[i][axis] < bestPositions[i][axis]) return true;
    if (positions[i][axis] > bestPositions[i][axis]) return false;
   }
  }
  int count = Math.Min(route.Count(), bestRoute.Count());
  for (int i = 0; i < count; i++)
  {
   vector point = route.At(i), bestPoint = bestRoute.At(i);
   for (int axis = 0; axis < 3; axis++)
   {
    if (point[axis] < bestPoint[axis]) return true;
    if (point[axis] > bestPoint[axis]) return false;
   }
  }
  return route.Count() < bestRoute.Count();
 }
 static bool Build(vector seed, out array<vector> positions, out array<vector> angles, out ERR_ForceRoute route)
 {
  positions = {}; angles = {}; route = null;
  if (!Replication.IsServer() || !ERR_ForceRules.FiniteVector(seed)) return false;
  SCR_AIWorld world = SCR_AIWorld.Cast(GetGame().GetAIWorld());
  if (!world || !world.GetRoadNetworkManager()) return false;
  array<BaseRoad> roads = {};
  world.GetRoadNetworkManager().GetRoadsInAABB(seed + "-500 -1000 -500", seed + "500 1000 500", roads);
  if (roads.Count() > 128) return false;
  float bestDistance = 1e20;
  foreach (BaseRoad road : roads)
  {
   if (!road || road.GetWidth() < 4) continue;
   array<vector> points = {}; road.GetPoints(points);
   for (int candidateIndex = 0; candidateIndex < 33; candidateIndex++)
   {
    array<vector> candidatePositions, candidateAngles;
    ERR_ForceRoute polyline;
    float distance;
    if (!FromPolyline(points, seed, candidatePositions, candidateAngles, polyline, distance, candidateIndex)) continue;
    for (int i = 0; i < candidatePositions.Count(); i++) candidatePositions[i] = ERR_Actors.Ground(candidatePositions[i]);
    ERR_ForceRoute grounded = new ERR_ForceRoute();
    bool valid = true;
    for (int i = 0; i < polyline.Count(); i++)
    {
     vector point = ERR_Actors.Ground(polyline.At(i));
     if (ChimeraWorldUtils.TryGetWaterSurfaceSimple(GetGame().GetWorld(), point) || !grounded.Add(point)) { valid = false; break; }
    }
    if (!valid || !grounded.Seal() || !ERR_ConvoyRules.Plan(candidatePositions, candidateAngles, grounded)) continue;
    if (!Prefer(distance, candidatePositions, grounded, bestDistance, positions, route)) continue;
    if (!ERR_ConvoyRules.GroundClear(candidatePositions, candidateAngles)) continue;
    positions = candidatePositions; angles = candidateAngles; route = grounded; bestDistance = distance;
   }
  }
  return route != null;
 }
}