lab4

Assets/Code/Lab4/BlendedPathFollower.cs

@@ -0,0 +1,220 @@
+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);
+                }
+            }
+        }
+    }
+}