示例#1
0
        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
        }