ID/Assets/Code/Lab4/BlendedPathFollower.cs

using Code.Base;
using Code.Lab1;
using System.Collections.Generic;
using System.Linq;
using Code.Lab3;
using UnityEngine;

namespace Code.Lab4
{
    public class BlendedPathFollower : Seek
    {
        public Path path;
        public float pathOffset = 1.0f;
        
        [Range(0, 1)]
        public float pathFollowingWeight = 0.7f;
        [Range(0, 1)]
        public float optimalPathWeight = 0.3f;

        private float _currentParam;

        private List<Vector3> _waypoints;
        private int _currentTargetIndex = -1;
        private List<int> _visitedIndices;
        private bool _reachedLastPoint;
        
        public override void Awake()
        {
            base.Awake();
            target = new GameObject("BlendedPathFollower_Target");
            _visitedIndices = new List<int>();
            InitializeWaypoints();
        }
        
        private void InitializeWaypoints()
        {
            if (path == null || path.nodes == null || path.nodes.Count == 0) return;
            
            _waypoints = new List<Vector3>();
            
            foreach (var node in path.nodes.Where(node => node != null))
            {
                _waypoints.Add(node.transform.position);
            }
            
            FindClosestStartPoint();
        }
        
        private void FindClosestStartPoint()
        {
            var minDistance = float.MaxValue;
            var closestIndex = 0;
            
            for (var i = 0; i < _waypoints.Count; i++)
            {
                var distance = Vector3.Distance(transform.position, _waypoints[i]);
                if (distance < minDistance)
                {
                    minDistance = distance;
                    closestIndex = i;
                }
            }
            
            _currentTargetIndex = closestIndex;
            _currentParam = 0;
            _visitedIndices.Clear();
            _reachedLastPoint = false;
        }
        
        public override Steering GetSteering()
        {
            if (_waypoints == null || _waypoints.Count == 0 || _reachedLastPoint)
            {
                return new Steering();
            }

            var pathSteering = GetPathFollowingSteering();
            var optimalSteering = GetOptimalPathSteering();

            var blendedSteering = new Steering
            {
                Linear = (pathFollowingWeight * pathSteering.Linear) + (optimalPathWeight * optimalSteering.Linear),
                Angular = (pathFollowingWeight * pathSteering.Angular) + (optimalPathWeight * optimalSteering.Angular)
            };

            if (blendedSteering.Linear.magnitude > Agent.maxAcceleration)
            {
                blendedSteering.Linear = blendedSteering.Linear.normalized * Agent.maxAcceleration;
            }
            
            return blendedSteering;
        }
        
        private Steering GetPathFollowingSteering()
        {
            if (!path) return new Steering();
            
            _currentParam = path.GetParam(transform.position, _currentParam);
            var targetParam = _currentParam + pathOffset;
            target.transform.position = path.GetPosition(targetParam);
            
            return base.GetSteering();
        }
        
        private Steering GetOptimalPathSteering()
        {
            if (_waypoints == null || _waypoints.Count == 0) return new Steering();
            
            var distanceToTarget = Vector3.Distance(transform.position, _waypoints[_currentTargetIndex]);
            
            if (distanceToTarget < 1f)
            {
                if (!_visitedIndices.Contains(_currentTargetIndex))
                {
                    _visitedIndices.Add(_currentTargetIndex);
                }
                
                if (_visitedIndices.Count >= _waypoints.Count)
                {
                    _reachedLastPoint = true;
                    return new Steering();
                }
                
                FindNextTarget();
            }

            var tempTarget = target.transform.position;
            target.transform.position = _waypoints[_currentTargetIndex];
            var steering = base.GetSteering();
            target.transform.position = tempTarget;

            return steering;
        }
        
        private void FindNextTarget()
        {
            var shortestDistance = float.MaxValue;
            var bestTargetIndex = -1;
            
            for (var i = 0; i < _waypoints.Count; i++)
            {
                if (_visitedIndices.Contains(i))
                    continue;
                    
                var distance = Vector3.Distance(transform.position, _waypoints[i]);
                if (distance < shortestDistance)
                {
                    shortestDistance = distance;
                    bestTargetIndex = i;
                }
            }
            
            if (bestTargetIndex != -1)
            {
                _currentTargetIndex = bestTargetIndex;
            }
            else
            {
                _reachedLastPoint = true;
            }
        }
        
        public void ResetPathFollowing()
        {
            _visitedIndices.Clear();
            FindClosestStartPoint();
            _reachedLastPoint = false;
        }
        
        public void SetWeights(float pathWeight, float optimalWeight)
        {
            pathFollowingWeight = Mathf.Clamp01(pathWeight);
            optimalPathWeight = Mathf.Clamp01(optimalWeight);

            float total = pathFollowingWeight + optimalPathWeight;
            if (total > 1f)
            {
                pathFollowingWeight /= total;
                optimalPathWeight /= total;
            }
        }
        
        private void OnDrawGizmosSelected()
        {
            if (_waypoints == null) return;

            for (var i = 0; i < _waypoints.Count; i++)
            {
                if (_reachedLastPoint)
                {
                    Gizmos.color = Color.gray;
                }
                else if (i == _currentTargetIndex)
                {
                    Gizmos.color = Color.green;
                }
                else if (_visitedIndices != null && _visitedIndices.Contains(i))
                {
                    Gizmos.color = Color.blue;
                }
                else
                {
                    Gizmos.color = Color.yellow;
                }
                
                Gizmos.DrawSphere(_waypoints[i], 0.3f);
            }

            if (path != null)
            {
                Gizmos.color = Color.magenta;
                var segments = path.GetSegments();
                foreach (var segment in segments)
                {
                    Gizmos.DrawLine(segment.A, segment.B);
                }
            }
        }
    }
}