private void UpdateAutoPilot(float deltaTime)
{
if (controlledSub == null)
{
return;
}
if (posToMaintain != null)
{
Vector2 steeringVel = GetSteeringVelocity((Vector2)posToMaintain, 10.0f);
TargetVelocity = Vector2.Lerp(TargetVelocity, steeringVel, AutoPilotSteeringLerp);
showIceSpireWarning = false;
return;
}
autopilotRayCastTimer -= deltaTime;
autopilotRecalculatePathTimer -= deltaTime;
if (autopilotRecalculatePathTimer <= 0.0f)
{
//periodically recalculate the path in case the sub ends up to a position
//where it can't keep traversing the initially calculated path
UpdatePath();
autopilotRecalculatePathTimer = RecalculatePathInterval;
}
if (steeringPath == null)
{
showIceSpireWarning = false;
return;
}
steeringPath.CheckProgress(ConvertUnits.ToSimUnits(controlledSub.WorldPosition), 10.0f);
connectedSubUpdateTimer -= deltaTime;
if (connectedSubUpdateTimer <= 0.0f)
{
connectedSubs.Clear();
connectedSubs = controlledSub?.GetConnectedSubs();
connectedSubUpdateTimer = ConnectedSubUpdateInterval;
}
if (autopilotRayCastTimer <= 0.0f && steeringPath.NextNode != null)
{
Vector2 diff = ConvertUnits.ToSimUnits(steeringPath.NextNode.Position - controlledSub.WorldPosition);
//if the node is close enough, check if it's visible
float lengthSqr = diff.LengthSquared();
if (lengthSqr > 0.001f && lengthSqr < AutopilotMinDistToPathNode * AutopilotMinDistToPathNode)
{
diff = Vector2.Normalize(diff);
//check if the next waypoint is visible from all corners of the sub
//(i.e. if we can navigate directly towards it or if there's obstacles in the way)
bool nextVisible = true;
for (int x = -1; x < 2; x += 2)
{
for (int y = -1; y < 2; y += 2)
{
Vector2 cornerPos =
new Vector2(controlledSub.Borders.Width * x, controlledSub.Borders.Height * y) / 2.0f;
cornerPos = ConvertUnits.ToSimUnits(cornerPos * 1.1f + controlledSub.WorldPosition);
float dist = Vector2.Distance(cornerPos, steeringPath.NextNode.SimPosition);
if (Submarine.PickBody(cornerPos, cornerPos + diff * dist, null, Physics.CollisionLevel) == null)
{
continue;
}
nextVisible = false;
x = 2;
y = 2;
}
}
if (nextVisible)
{
steeringPath.SkipToNextNode();
}
}
autopilotRayCastTimer = AutopilotRayCastInterval;
}
Vector2 newVelocity = Vector2.Zero;
if (steeringPath.CurrentNode != null)
{
newVelocity = GetSteeringVelocity(steeringPath.CurrentNode.WorldPosition, 2.0f);
}
Vector2 avoidDist = new Vector2(
Math.Max(1000.0f * Math.Abs(controlledSub.Velocity.X), controlledSub.Borders.Width * 0.75f),
Math.Max(1000.0f * Math.Abs(controlledSub.Velocity.Y), controlledSub.Borders.Height * 0.75f));
float avoidRadius = avoidDist.Length();
float damagingWallAvoidRadius = MathHelper.Clamp(avoidRadius * 1.5f, 5000.0f, 10000.0f);
Vector2 newAvoidStrength = Vector2.Zero;
debugDrawObstacles.Clear();
//steer away from nearby walls
showIceSpireWarning = false;
var closeCells = Level.Loaded.GetCells(controlledSub.WorldPosition, 4);
foreach (VoronoiCell cell in closeCells)
{
if (cell.DoesDamage)
{
foreach (GraphEdge edge in cell.Edges)
{
Vector2 closestPoint = MathUtils.GetClosestPointOnLineSegment(edge.Point1 + cell.Translation, edge.Point2 + cell.Translation, controlledSub.WorldPosition);
Vector2 diff = closestPoint - controlledSub.WorldPosition;
float dist = diff.Length() - Math.Max(controlledSub.Borders.Width, controlledSub.Borders.Height) / 2;
if (dist > damagingWallAvoidRadius)
{
continue;
}
Vector2 normalizedDiff = Vector2.Normalize(diff);
float dot = Vector2.Dot(normalizedDiff, controlledSub.Velocity);
float avoidStrength = MathHelper.Clamp(MathHelper.Lerp(1.0f, 0.0f, dist / damagingWallAvoidRadius - dot), 0.0f, 1.0f);
Vector2 avoid = -normalizedDiff * avoidStrength;
newAvoidStrength += avoid;
debugDrawObstacles.Add(new ObstacleDebugInfo(edge, edge.Center, 1.0f, avoid, cell.Translation));
if (dot > 0.0f)
{
showIceSpireWarning = true;
}
}
continue;
}
foreach (GraphEdge edge in cell.Edges)
{
if (MathUtils.GetLineIntersection(edge.Point1 + cell.Translation, edge.Point2 + cell.Translation, controlledSub.WorldPosition, cell.Center, out Vector2 intersection))
{
Vector2 diff = controlledSub.WorldPosition - intersection;
//far enough -> ignore
if (Math.Abs(diff.X) > avoidDist.X && Math.Abs(diff.Y) > avoidDist.Y)
{
debugDrawObstacles.Add(new ObstacleDebugInfo(edge, intersection, 0.0f, Vector2.Zero, Vector2.Zero));
continue;
}
if (diff.LengthSquared() < 1.0f)
{
diff = Vector2.UnitY;
}
Vector2 normalizedDiff = Vector2.Normalize(diff);
float dot = controlledSub.Velocity == Vector2.Zero ?
0.0f : Vector2.Dot(controlledSub.Velocity, -normalizedDiff);
//not heading towards the wall -> ignore
if (dot < 1.0)
{
debugDrawObstacles.Add(new ObstacleDebugInfo(edge, intersection, dot, Vector2.Zero, cell.Translation));
continue;
}
Vector2 change = (normalizedDiff * Math.Max((avoidRadius - diff.Length()), 0.0f)) / avoidRadius;
if (change.LengthSquared() < 0.001f)
{
continue;
}
newAvoidStrength += change * (dot - 1.0f);
debugDrawObstacles.Add(new ObstacleDebugInfo(edge, intersection, dot - 1.0f, change * (dot - 1.0f), cell.Translation));
}
}
}
avoidStrength = Vector2.Lerp(avoidStrength, newAvoidStrength, deltaTime * 10.0f);
TargetVelocity = Vector2.Lerp(TargetVelocity, newVelocity + avoidStrength * 100.0f, AutoPilotSteeringLerp);
//steer away from other subs
foreach (Submarine sub in Submarine.Loaded)
{
if (sub == controlledSub || connectedSubs.Contains(sub))
{
continue;
}
Point sizeSum = controlledSub.Borders.Size + sub.Borders.Size;
Vector2 minDist = sizeSum.ToVector2() / 2;
Vector2 diff = controlledSub.WorldPosition - sub.WorldPosition;
float xDist = Math.Abs(diff.X);
float yDist = Math.Abs(diff.Y);
Vector2 maxAvoidDistance = minDist * 2;
if (xDist > maxAvoidDistance.X || yDist > maxAvoidDistance.Y)
{
//far enough -> ignore
continue;
}
float dot = controlledSub.Velocity == Vector2.Zero ? 0.0f : Vector2.Dot(Vector2.Normalize(controlledSub.Velocity), -diff);
if (dot < 0.0f)
{
//heading away -> ignore
continue;
}
float distanceFactor = MathHelper.Lerp(0, 1, MathUtils.InverseLerp(maxAvoidDistance.X + maxAvoidDistance.Y, minDist.X + minDist.Y, xDist + yDist));
float velocityFactor = MathHelper.Lerp(0, 1, MathUtils.InverseLerp(0, 3, controlledSub.Velocity.Length()));
TargetVelocity += 100 * Vector2.Normalize(diff) * distanceFactor * velocityFactor;
}
//clamp velocity magnitude to 100.0f (Is this required? The X and Y components are clamped in the property setter)
float velMagnitude = TargetVelocity.Length();
if (velMagnitude > 100.0f)
{
TargetVelocity *= 100.0f / velMagnitude;
}
#if CLIENT
HintManager.OnAutoPilotPathUpdated(this);
#endif
}