From 3a3b09c2c5e6efc09779345238b39e1199816751 Mon Sep 17 00:00:00 2001 From: wjsjwr Date: Sun, 11 Jan 2026 23:29:57 +0800 Subject: [PATCH] =?UTF-8?q?AStarGrid=20=E6=9C=AA=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scenes/campus.tscn | 6 +- scenes/student_16_native.tscn | 1 + scripts/CampusStudent.cs | 617 +++++++++++++++++++++++++++++++++- 3 files changed, 616 insertions(+), 8 deletions(-) diff --git a/scenes/campus.tscn b/scenes/campus.tscn index a69c6a7..608f2f7 100644 --- a/scenes/campus.tscn +++ b/scenes/campus.tscn @@ -7,9 +7,9 @@ [ext_resource type="PackedScene" uid="uid://drmjsqoy8htc8" path="res://scenes/ui-elements/task_list.tscn" id="3_4gjr3"] [sub_resource type="NavigationPolygon" id="NavigationPolygon_8u8vn"] -vertices = PackedVector2Array(956, 540, 4, 540, 420, 516, 460, 516, 380, 516, 204, 356, 204, 260, 228, 284, 228, 468, 4, 500, 100, 500, 380, 468, 300, 468, 148, 212, 148, 196, 156, 196, 156, 212, 244, 212, 244, 20, 268, 20, 268, 236, 436, 236, 436, 196, 444, 196, 444, 236, 612, 236, 612, 20, 636, 20, 636, 244, 708, 244, 708, 196, 732, 196, 732, 172, 756, 172, 756, 196, 772, 196, 772, 20, 796, 20, 796, 228, 844, 228, 844, 196, 852, 196, 852, 228, 860, 228, 860, 284, 828, 284, 804, 284, 916, 308, 916, 292, 940, 292, 948, 332, 940, 188, 948, 188, 828, 332, 828, 308, 828, 516, 804, 516, 572, 284, 572, 300, 548, 300, 548, 284, 956, 516, 116, 364, 124, 364, 124, 396, 196, 396, 196, 356, 180, 436, 180, 412, 140, 412, 292, 468, 292, 452, 300, 452, 140, 436, 4, 284, 4, 4, 28, 4, 28, 212, 100, 284, 116, 260, 460, 284, 420, 284) -polygons = Array[PackedInt32Array]([PackedInt32Array(0, 1, 2, 3), PackedInt32Array(4, 2, 1), PackedInt32Array(5, 6, 7, 8), PackedInt32Array(4, 1, 9, 10), PackedInt32Array(11, 4, 10, 12), PackedInt32Array(13, 14, 15, 16), PackedInt32Array(17, 18, 19, 20), PackedInt32Array(21, 22, 23, 24), PackedInt32Array(25, 26, 27, 28), PackedInt32Array(29, 30, 31), PackedInt32Array(31, 32, 33, 34), PackedInt32Array(35, 36, 37, 38), PackedInt32Array(39, 40, 41, 42), PackedInt32Array(42, 43, 44, 45), PackedInt32Array(39, 42, 45), PackedInt32Array(38, 39, 45, 46), PackedInt32Array(47, 48, 49, 50), PackedInt32Array(49, 51, 52, 50), PackedInt32Array(47, 50, 53, 54), PackedInt32Array(53, 55, 56, 46), PackedInt32Array(54, 53, 46), PackedInt32Array(45, 54, 46), PackedInt32Array(38, 46, 57, 29), PackedInt32Array(57, 58, 59, 60), PackedInt32Array(55, 61, 0), PackedInt32Array(62, 63, 64, 10), PackedInt32Array(65, 66, 5, 8), PackedInt32Array(65, 8, 67, 68), PackedInt32Array(64, 65, 68, 69), PackedInt32Array(70, 71, 72, 12), PackedInt32Array(70, 12, 10), PackedInt32Array(8, 70, 10), PackedInt32Array(67, 8, 10, 73), PackedInt32Array(74, 75, 76, 77), PackedInt32Array(78, 74, 77, 79), PackedInt32Array(29, 31, 34, 38), PackedInt32Array(34, 35, 38), PackedInt32Array(28, 29, 57), PackedInt32Array(24, 25, 28, 57, 60), PackedInt32Array(24, 60, 80, 21), PackedInt32Array(20, 21, 80, 81, 6), PackedInt32Array(64, 69, 73, 10), PackedInt32Array(16, 17, 20, 6), PackedInt32Array(81, 7, 6), PackedInt32Array(62, 10, 78, 79), PackedInt32Array(79, 77, 13, 6), PackedInt32Array(6, 13, 16), PackedInt32Array(81, 80, 3, 2), PackedInt32Array(0, 3, 56), PackedInt32Array(0, 56, 55)]) -outlines = Array[PackedVector2Array]([PackedVector2Array(0, 0, 32, 0, 32, 208, 144, 208, 144, 192, 160, 192, 160, 208, 240, 208, 240, 16, 272, 16, 272, 232, 432, 232, 432, 192, 448, 192, 448, 232, 608, 232, 608, 16, 640, 16, 640, 240, 704, 240, 704, 192, 728, 192, 728, 168, 760, 168, 760, 192, 768, 192, 768, 16, 800, 16, 800, 224, 832, 224, 840, 224, 840, 192, 856, 192, 856, 224, 864, 224, 864, 288, 832, 288, 832, 304, 912, 304, 912, 288, 936, 288, 936, 184, 952, 184, 952, 336, 832, 336, 832, 512, 960, 512, 960, 544, 0, 544, 0, 496, 96, 496, 96, 288, 0, 288), PackedVector2Array(144, 432, 176, 432, 176, 416, 144, 416), PackedVector2Array(128, 392, 192, 392, 192, 352, 200, 352, 200, 264, 120, 264, 120, 360, 128, 360), PackedVector2Array(232, 304, 232, 464, 288, 464, 288, 448, 304, 448, 304, 464, 384, 464, 384, 512, 416, 512, 416, 288, 232, 288), PackedVector2Array(464, 288, 464, 512, 800, 512, 800, 288, 576, 288, 576, 304, 544, 304, 544, 288), PackedVector2Array(144, 464, 176, 464, 176, 480, 144, 480)]) +vertices = PackedVector2Array(956, 540, 4, 540, 420, 516, 460, 516, 380, 516, 204, 356, 204, 260, 228, 284, 228, 468, 4, 500, 100, 500, 380, 468, 300, 468, 148, 212, 148, 196, 156, 196, 156, 212, 244, 212, 244, 20, 268, 20, 268, 236, 436, 236, 436, 196, 444, 196, 444, 236, 612, 236, 612, 20, 636, 20, 636, 244, 708, 244, 708, 196, 732, 196, 732, 172, 756, 172, 756, 196, 772, 196, 772, 20, 796, 20, 796, 228, 844, 228, 844, 196, 852, 196, 852, 228, 860, 228, 860, 284, 828, 284, 804, 284, 932, 308, 932, 180, 956, 180, 956, 332, 828, 332, 828, 308, 828, 516, 804, 516, 572, 284, 572, 300, 548, 300, 548, 284, 956, 516, 116, 364, 124, 364, 124, 396, 196, 396, 196, 356, 180, 436, 180, 412, 140, 412, 292, 468, 292, 452, 300, 452, 140, 436, 4, 284, 4, 4, 28, 4, 28, 212, 100, 284, 116, 260, 460, 284, 420, 284) +polygons = Array[PackedInt32Array]([PackedInt32Array(0, 1, 2, 3), PackedInt32Array(4, 2, 1), PackedInt32Array(5, 6, 7, 8), PackedInt32Array(4, 1, 9, 10), PackedInt32Array(11, 4, 10, 12), PackedInt32Array(13, 14, 15, 16), PackedInt32Array(17, 18, 19, 20), PackedInt32Array(21, 22, 23, 24), PackedInt32Array(25, 26, 27, 28), PackedInt32Array(29, 30, 31), PackedInt32Array(31, 32, 33, 34), PackedInt32Array(35, 36, 37, 38), PackedInt32Array(39, 40, 41, 42), PackedInt32Array(42, 43, 44, 45), PackedInt32Array(39, 42, 45), PackedInt32Array(38, 39, 45, 46), PackedInt32Array(47, 48, 49, 50), PackedInt32Array(47, 50, 51, 52), PackedInt32Array(51, 53, 54, 46), PackedInt32Array(52, 51, 46), PackedInt32Array(45, 52, 46), PackedInt32Array(38, 46, 55, 29), PackedInt32Array(55, 56, 57, 58), PackedInt32Array(53, 59, 0), PackedInt32Array(60, 61, 62, 10), PackedInt32Array(63, 64, 5, 8), PackedInt32Array(63, 8, 65, 66), PackedInt32Array(62, 63, 66, 67), PackedInt32Array(68, 69, 70, 12), PackedInt32Array(68, 12, 10), PackedInt32Array(8, 68, 10), PackedInt32Array(65, 8, 10, 71), PackedInt32Array(72, 73, 74, 75), PackedInt32Array(76, 72, 75, 77), PackedInt32Array(29, 31, 34, 38), PackedInt32Array(34, 35, 38), PackedInt32Array(28, 29, 55), PackedInt32Array(24, 25, 28, 55, 58), PackedInt32Array(24, 58, 78, 21), PackedInt32Array(20, 21, 78, 79, 6), PackedInt32Array(62, 67, 71, 10), PackedInt32Array(16, 17, 20, 6), PackedInt32Array(79, 7, 6), PackedInt32Array(60, 10, 76, 77), PackedInt32Array(77, 75, 13, 6), PackedInt32Array(6, 13, 16), PackedInt32Array(79, 78, 3, 2), PackedInt32Array(0, 3, 54), PackedInt32Array(0, 54, 53)]) +outlines = Array[PackedVector2Array]([PackedVector2Array(0, 0, 32, 0, 32, 208, 144, 208, 144, 192, 160, 192, 160, 208, 240, 208, 240, 16, 272, 16, 272, 232, 432, 232, 432, 192, 448, 192, 448, 232, 608, 232, 608, 16, 640, 16, 640, 240, 704, 240, 704, 192, 728, 192, 728, 168, 760, 168, 760, 192, 768, 192, 768, 16, 800, 16, 800, 224, 832, 224, 840, 224, 840, 192, 856, 192, 856, 224, 864, 224, 864, 288, 832, 288, 832, 304, 912, 304, 928, 304, 928, 288, 928, 176, 960, 176, 960, 336, 832, 336, 832, 512, 960, 512, 960, 544, 0, 544, 0, 496, 96, 496, 96, 288, 0, 288), PackedVector2Array(144, 432, 176, 432, 176, 416, 144, 416), PackedVector2Array(128, 392, 192, 392, 192, 352, 200, 352, 200, 264, 120, 264, 120, 360, 128, 360), PackedVector2Array(232, 304, 232, 464, 288, 464, 288, 448, 304, 448, 304, 464, 384, 464, 384, 512, 416, 512, 416, 288, 232, 288), PackedVector2Array(464, 288, 464, 512, 800, 512, 800, 288, 576, 288, 576, 304, 544, 304, 544, 288), PackedVector2Array(144, 464, 176, 464, 176, 480, 144, 480)]) parsed_collision_mask = 4294967294 agent_radius = 4.0 diff --git a/scenes/student_16_native.tscn b/scenes/student_16_native.tscn index 24c7d9e..9cfed0e 100644 --- a/scenes/student_16_native.tscn +++ b/scenes/student_16_native.tscn @@ -877,6 +877,7 @@ _data = { [node name="Student" type="CharacterBody2D"] z_index = 2 script = ExtResource("1_oesea") +DebugDrawPath = true [node name="CollisionShape2D" type="CollisionShape2D" parent="."] shape = SubResource("RectangleShape2D_opr6h") diff --git a/scripts/CampusStudent.cs b/scripts/CampusStudent.cs index e10ad44..5b81c6a 100644 --- a/scripts/CampusStudent.cs +++ b/scripts/CampusStudent.cs @@ -1,3 +1,4 @@ +using System; using System.Collections.Generic; using System.Diagnostics; using Godot; @@ -30,6 +31,15 @@ public partial class CampusStudent : CharacterBody2D private bool _phoneIdleActive; private bool _phoneExitLocked; private bool _usePhysicsMovement = true; + private readonly List _gridPath = new(); + private int _gridPathIndex; + private bool _gridPathActive; + private bool _gridPathPending; + private AStarGrid2D _astarGrid; + private Rect2I _astarRegion; + private int _astarMapIteration; + private float _gridPathRetryTimer; + private NavigationRegion2D _navigationRegion; [Export] public float MoveSpeed { get; set; } = 60.0f; [Export] public float TargetReachDistance { get; set; } = 6.0f; [Export] public bool Use16X16Sprites { get; set; } = true; @@ -37,6 +47,16 @@ public partial class CampusStudent : CharacterBody2D [Export] public float StuckRepathSeconds { get; set; } = 0.6f; [Export] public float StuckDistanceEpsilon { get; set; } = 2.0f; [Export] public float NavMeshClampDistance { get; set; } = 6.0f; + [Export] public bool UseGridPathfinding { get; set; } = true; + [Export] public float GridCellSize { get; set; } = 8.0f; + [Export] public float GridWalkableTolerance { get; set; } = 2.0f; + [Export] public int GridSearchNodeLimit { get; set; } = 8000; + [Export] public float GridRepathInterval { get; set; } = 0.25f; + [Export] public bool DebugDrawGrid { get; set; } + [Export] public bool DebugDrawSolidOnly { get; set; } = true; + [Export] public bool DebugDrawPath { get; set; } + [Export] public int DebugDrawRadiusCells { get; set; } = 20; + [Export] public bool DebugLogGrid { get; set; } [Export] public uint EnvironmentCollisionMask { get; set; } = 1u; [Export] public uint StudentCollisionLayer { get; set; } = 1u << 1; @@ -44,6 +64,7 @@ public partial class CampusStudent : CharacterBody2D { _navigationAgent = GetNodeOrNull("NavigationAgent2D"); _animationPlayer = GetNodeOrNull("AnimationPlayer"); + _navigationRegion = FindNavigationRegion(); CacheSprites(); ConfigureCollision(); @@ -76,6 +97,18 @@ public partial class CampusStudent : CharacterBody2D public override void _PhysicsProcess(double delta) { _lastDelta = delta; + if (DebugDrawGrid || DebugDrawPath) + { + QueueRedraw(); + } + if ((DebugDrawGrid || DebugDrawPath) && _astarGrid == null) + { + EnsureAStarGrid(); + } + if (_gridPathRetryTimer > 0.0f) + { + _gridPathRetryTimer = Mathf.Max(0.0f, _gridPathRetryTimer - (float)delta); + } if (_phoneExitLocked) { Velocity = Vector2.Zero; @@ -85,6 +118,33 @@ public partial class CampusStudent : CharacterBody2D } return; } + + if (_behaviorControlEnabled && _behaviorHasTarget) + { + TryBuildPendingGridPath(); + if (_gridPathActive) + { + if (ProcessGridPathMovement(delta)) + { + return; + } + } + else if (UseGridPathfinding && _gridPathPending) + { + Velocity = Vector2.Zero; + if (!EnableAvoidance && _usePhysicsMovement) + { + MoveAndSlide(); + } + + if (!_phoneIdleActive && !_phoneExitLocked) + { + PlayIdleAnimation(); + } + UpdateStuckTimer(delta); + return; + } + } if (_navigationAgent == null || (!_behaviorControlEnabled && _patrolPoints.Count == 0) || (_behaviorControlEnabled && !_behaviorHasTarget)) { if (!_phoneIdleActive && !_phoneExitLocked) @@ -152,6 +212,63 @@ public partial class CampusStudent : CharacterBody2D } } + public override void _Draw() + { + if (!DebugDrawGrid && !DebugDrawPath) return; + if (_astarGrid == null || GridCellSize <= 0.0f) return; + + var cellSize = Mathf.Max(1.0f, GridCellSize); + var half = new Vector2(cellSize * 0.5f, cellSize * 0.5f); + + if (DebugDrawGrid) + { + var radius = Mathf.Max(1, DebugDrawRadiusCells); + var originCell = WorldToGrid(GlobalPosition); + var minX = Math.Max(_astarRegion.Position.X, originCell.X - radius); + var maxX = Math.Min(_astarRegion.Position.X + _astarRegion.Size.X - 1, originCell.X + radius); + var minY = Math.Max(_astarRegion.Position.Y, originCell.Y - radius); + var maxY = Math.Min(_astarRegion.Position.Y + _astarRegion.Size.Y - 1, originCell.Y + radius); + + var solidFill = new Color(1.0f, 0.3f, 0.3f, 0.25f); + var solidOutline = new Color(1.0f, 0.3f, 0.3f, 0.6f); + var walkFill = new Color(0.2f, 0.8f, 0.3f, 0.08f); + var walkOutline = new Color(0.2f, 0.8f, 0.3f, 0.2f); + + for (var x = minX; x <= maxX; x++) + { + for (var y = minY; y <= maxY; y++) + { + var cell = new Vector2I(x, y); + var isSolid = _astarGrid.IsPointSolid(cell); + if (DebugDrawSolidOnly && !isSolid) + { + continue; + } + + var worldCenter = GridToWorld(cell); + // Draw around the cell center to verify path alignment. + var localTopLeft = ToLocal(worldCenter) - half; + var rect = new Rect2(localTopLeft, new Vector2(cellSize, cellSize)); + DrawRect(rect, isSolid ? solidFill : walkFill); + DrawRect(rect, isSolid ? solidOutline : walkOutline, false, 1.0f); + } + } + } + + if (DebugDrawPath && _gridPath.Count > 0 && _gridPathIndex < _gridPath.Count) + { + var pathColor = new Color(0.2f, 0.7f, 1.0f, 0.9f); + var current = ToLocal(GlobalPosition); + for (var i = _gridPathIndex; i < _gridPath.Count; i++) + { + var next = ToLocal(_gridPath[i]); + DrawLine(current, next, pathColor, 2.0f); + DrawCircle(next, 2.5f, pathColor); + current = next; + } + } + } + public void ConfigurePatrol(List points, int startIndex) { _patrolPoints = points ?? new List(); @@ -175,13 +292,25 @@ public partial class CampusStudent : CharacterBody2D public void SetBehaviorTarget(Vector2 target) { _behaviorControlEnabled = true; + target = ClampTargetToNavMesh(target); _behaviorTarget = target; _behaviorHasTarget = true; _currentTarget = target; _hasTarget = true; _stuckTimer = 0.0f; + _gridPathActive = false; + _gridPathPending = false; + _gridPathIndex = 0; + _gridPathRetryTimer = 0.0f; + _gridPath.Clear(); - if (_navigationAgent != null) + if (UseGridPathfinding) + { + _gridPathPending = true; + TryBuildPendingGridPath(); + } + + if (!UseGridPathfinding && _navigationAgent != null) { _navigationAgent.TargetPosition = target; } @@ -193,11 +322,25 @@ public partial class CampusStudent : CharacterBody2D _hasTarget = false; _currentTarget = Vector2.Zero; _stuckTimer = 0.0f; + _gridPathActive = false; + _gridPathPending = false; + _gridPathIndex = 0; + _gridPathRetryTimer = 0.0f; + _gridPath.Clear(); } public bool HasReachedBehaviorTarget() { - if (!_behaviorHasTarget || _navigationAgent == null) return true; + if (!_behaviorHasTarget) return true; + if (_gridPathActive) + { + return _gridPathIndex >= _gridPath.Count; + } + if (_gridPathPending) + { + return false; + } + if (_navigationAgent == null) return true; return _navigationAgent.IsNavigationFinished(); } @@ -247,6 +390,10 @@ public partial class CampusStudent : CharacterBody2D { // 由校园控制器传入导航地图，供本地边界夹紧使用 _navigationMap = map; + _astarGrid = null; + _astarRegion = new Rect2I(); + _astarMapIteration = 0; + _navigationRegion = FindNavigationRegion(); } public void ApplyRandomTheme() @@ -351,16 +498,56 @@ public partial class CampusStudent : CharacterBody2D private void RepathToCurrentTarget() { - if (_navigationAgent == null || !_hasTarget) return; + if (!_hasTarget) return; + + if (UseGridPathfinding) + { + _gridPathActive = false; + _gridPathPending = true; + _gridPathIndex = 0; + _gridPath.Clear(); + TryBuildPendingGridPath(); + return; + } + + if (_navigationAgent == null) return; // 重新请求到同一目标点的路径，避免“固定时间换目的地” _navigationAgent.TargetPosition = _currentTarget; } + private void TryBuildPendingGridPath() + { + if (!UseGridPathfinding || !_gridPathPending) return; + if (_gridPathRetryTimer > 0.0f) return; + if (!IsNavigationMapReady()) return; + + var path = BuildGridPath(GlobalPosition, _currentTarget); + if (path == null || path.Count == 0) + { + _gridPathRetryTimer = Mathf.Max(0.1f, GridRepathInterval); + return; + } + + _gridPath.Clear(); + _gridPath.AddRange(path); + _gridPathIndex = 0; + _gridPathActive = true; + _gridPathPending = false; + if (DebugDrawGrid || DebugDrawPath) + { + QueueRedraw(); + } + } + + private bool IsNavigationMapReady() + { + return _navigationMap.IsValid && NavigationServer2D.MapGetIterationId(_navigationMap) > 0; + } + private Vector2 ClampVelocityToNavMesh(Vector2 velocity) { - if (!_navigationMap.IsValid) return velocity; - if (NavigationServer2D.MapGetIterationId(_navigationMap) == 0) return velocity; + if (!IsNavigationMapReady()) return velocity; if (_lastDelta <= 0.0) return velocity; // 预测下一帧位置，若偏离导航网格过远则夹回网格内 @@ -379,6 +566,19 @@ public partial class CampusStudent : CharacterBody2D return velocity; } + private Vector2 ClampTargetToNavMesh(Vector2 target) + { + if (!IsNavigationMapReady()) return target; + + var closest = NavigationServer2D.MapGetClosestPoint(_navigationMap, target); + if (closest.DistanceTo(target) <= 0.01f) + { + return target; + } + + return closest; + } + private void ApplyMovement(double delta) { if (_usePhysicsMovement) @@ -477,6 +677,413 @@ public partial class CampusStudent : CharacterBody2D } } + private bool ProcessGridPathMovement(double delta) + { + if (_gridPathIndex >= _gridPath.Count) + { + Velocity = Vector2.Zero; + if (!EnableAvoidance && _usePhysicsMovement) + { + MoveAndSlide(); + } + + if (!_phoneIdleActive && !_phoneExitLocked) + { + PlayIdleAnimation(); + } + UpdateStuckTimer(delta); + return true; + } + + var target = _gridPath[_gridPathIndex]; + var toNext = target - GlobalPosition; + if (toNext.LengthSquared() <= TargetReachDistance * TargetReachDistance) + { + _gridPathIndex += 1; + return true; + } + + if (delta <= 0.0) + { + return true; + } + + var axisVelocity = ToAxisVelocity(toNext); + var step = axisVelocity * MoveSpeed * (float)delta; + var candidate = GlobalPosition + step; + if (!IsWorldWalkable(candidate)) + { + Velocity = Vector2.Zero; + if (!EnableAvoidance && _usePhysicsMovement) + { + MoveAndSlide(); + } + RepathToCurrentTarget(); + UpdateStuckTimer(delta); + return true; + } + + Velocity = step / (float)delta; + ApplyMovement(delta); + UpdateFacingAnimation(Velocity); + UpdateStuckTimer(delta); + return true; + } + + private Vector2 ToAxisVelocity(Vector2 delta) + { + if (Mathf.Abs(delta.X) >= Mathf.Abs(delta.Y)) + { + return new Vector2(Mathf.Sign(delta.X), 0); + } + + return new Vector2(0, Mathf.Sign(delta.Y)); + } + + private List BuildGridPath(Vector2 start, Vector2 target) + { + if (!IsNavigationMapReady() || GridCellSize <= 0.0f) + { + return null; + } + + EnsureAStarGrid(); + if (_astarGrid == null) + { + return null; + } + + var startCell = WorldToGrid(start); + var targetCell = WorldToGrid(target); + if (!IsCellInBounds(startCell) || !IsCellInBounds(targetCell)) + { + if (DebugLogGrid) + { + GD.Print($"[AStarGrid] out_of_bounds start={startCell} target={targetCell} region={_astarRegion}"); + } + return null; + } + + startCell = FindNearestOpenCell(startCell, 6); + targetCell = FindNearestOpenCell(targetCell, 6); + if (!IsCellInBounds(startCell) || !IsCellInBounds(targetCell)) + { + if (DebugLogGrid) + { + GD.Print($"[AStarGrid] no_open_cell start={startCell} target={targetCell}"); + } + return null; + } + + if (_astarGrid.IsPointSolid(startCell) || _astarGrid.IsPointSolid(targetCell)) + { + if (DebugLogGrid) + { + GD.Print($"[AStarGrid] solid start={startCell} target={targetCell}"); + } + return null; + } + + var path = _astarGrid.GetIdPath(startCell, targetCell); + if (path.Count == 0) + { + if (DebugLogGrid) + { + GD.Print($"[AStarGrid] empty_path start={startCell} target={targetCell}"); + } + return null; + } + + var result = new List(path.Count); + foreach (Vector2I cell in path) + { + result.Add(GridToWorld(cell)); + } + + if (result.Count > 0) + { + result.RemoveAt(0); + } + + RemoveImmediateBacktracks(result); + return SimplifyGridPath(result); + } + + private void RemoveImmediateBacktracks(List path) + { + if (path == null || path.Count < 3) return; + + var index = 2; + while (index < path.Count) + { + if (path[index].DistanceTo(path[index - 2]) <= 0.01f) + { + path.RemoveAt(index - 1); + index = Math.Max(2, index - 1); + continue; + } + index += 1; + } + } + + private List SimplifyGridPath(List path) + { + if (path == null || path.Count < 3) return path; + + var simplified = new List { path[0] }; + var lastDir = Vector2.Zero; + + for (var i = 1; i < path.Count; i++) + { + var delta = path[i] - path[i - 1]; + if (delta.LengthSquared() < 0.01f) + { + continue; + } + + var axisDir = ToAxisVelocity(delta); + if (simplified.Count == 1) + { + simplified.Add(path[i]); + lastDir = axisDir; + continue; + } + + if (axisDir == lastDir) + { + simplified[simplified.Count - 1] = path[i]; + } + else + { + simplified.Add(path[i]); + lastDir = axisDir; + } + } + + return simplified; + } + + private void EnsureAStarGrid() + { + if (!IsNavigationMapReady()) return; + + var region = BuildGridRegion(); + if (region.Size.X <= 0 || region.Size.Y <= 0) + { + return; + } + + var cellSize = new Vector2(Mathf.Max(1.0f, GridCellSize), Mathf.Max(1.0f, GridCellSize)); + var mapIteration = (int)NavigationServer2D.MapGetIterationId(_navigationMap); + var needsRebuild = _astarGrid == null + || _astarMapIteration != mapIteration + || _astarRegion.Position != region.Position + || _astarRegion.Size != region.Size + || _astarGrid.CellSize != cellSize; + + if (!needsRebuild) + { + return; + } + + _astarGrid ??= new AStarGrid2D(); + _astarGrid.Region = region; + _astarGrid.CellSize = cellSize; + _astarGrid.Offset = Vector2.Zero; + _astarGrid.DiagonalMode = AStarGrid2D.DiagonalModeEnum.Never; + _astarGrid.Update(); + + for (var x = region.Position.X; x < region.Position.X + region.Size.X; x++) + { + for (var y = region.Position.Y; y < region.Position.Y + region.Size.Y; y++) + { + var cell = new Vector2I(x, y); + var center = GridToWorld(cell); + var walkable = IsCellWalkable(center); + _astarGrid.SetPointSolid(cell, !walkable); + } + } + + _astarRegion = region; + _astarMapIteration = mapIteration; + if (DebugLogGrid) + { + GD.Print($"[AStarGrid] region={region} cell={cellSize}"); + } + if (DebugDrawGrid || DebugDrawPath) + { + QueueRedraw(); + } + } + + private Rect2I BuildGridRegion() + { + var bounds = BuildWorldBounds(); + var cell = Mathf.Max(1.0f, GridCellSize); + var minX = Mathf.FloorToInt(bounds.Position.X / cell); + var minY = Mathf.FloorToInt(bounds.Position.Y / cell); + var maxX = Mathf.CeilToInt((bounds.Position.X + bounds.Size.X) / cell); + var maxY = Mathf.CeilToInt((bounds.Position.Y + bounds.Size.Y) / cell); + var sizeX = Math.Max(1, maxX - minX); + var sizeY = Math.Max(1, maxY - minY); + return new Rect2I(new Vector2I(minX, minY), new Vector2I(sizeX, sizeY)); + } + + private Rect2 BuildWorldBounds() + { + var viewport = GetViewportRect(); + var bounds = new Rect2(viewport.Position, viewport.Size); + + var navRegion = _navigationRegion ?? FindNavigationRegion(); + if (navRegion == null || navRegion.NavigationPolygon == null) + { + return bounds; + } + + var polygon = navRegion.NavigationPolygon; + var outlineCount = polygon.GetOutlineCount(); + if (outlineCount == 0) + { + return bounds; + } + + var min = Vector2.Zero; + var max = Vector2.Zero; + var hasPoint = false; + + for (var i = 0; i < outlineCount; i++) + { + var outline = polygon.GetOutline(i); + for (var j = 0; j < outline.Length; j++) + { + var world = navRegion.ToGlobal(outline[j]); + if (!hasPoint) + { + min = world; + max = world; + hasPoint = true; + continue; + } + + min = new Vector2(Mathf.Min(min.X, world.X), Mathf.Min(min.Y, world.Y)); + max = new Vector2(Mathf.Max(max.X, world.X), Mathf.Max(max.Y, world.Y)); + } + } + + if (!hasPoint) + { + return bounds; + } + + var navBounds = new Rect2(min, max - min); + return bounds.Merge(navBounds); + } + + private bool IsCellInRegion(Vector2I cell) + { + return cell.X >= _astarRegion.Position.X + && cell.X < _astarRegion.Position.X + _astarRegion.Size.X + && cell.Y >= _astarRegion.Position.Y + && cell.Y < _astarRegion.Position.Y + _astarRegion.Size.Y; + } + + private bool IsCellInBounds(Vector2I cell) + { + if (_astarGrid != null) + { + return _astarGrid.IsInBounds(cell.X, cell.Y); + } + + return IsCellInRegion(cell); + } + + private Vector2I WorldToGrid(Vector2 world) + { + var size = Mathf.Max(1.0f, GridCellSize); + return new Vector2I( + Mathf.FloorToInt(world.X / size), + Mathf.FloorToInt(world.Y / size)); + } + + private Vector2 GridToWorld(Vector2I grid) + { + var size = Mathf.Max(1.0f, GridCellSize); + return new Vector2((grid.X + 0.5f) * size, (grid.Y + 0.5f) * size); + } + + private Vector2I FindNearestOpenCell(Vector2I origin, int radius) + { + if (_astarGrid == null) return origin; + if (IsCellInBounds(origin) && !_astarGrid.IsPointSolid(origin)) return origin; + + for (var r = 1; r <= radius; r++) + { + for (var dx = -r; dx <= r; dx++) + { + var dy = r - Math.Abs(dx); + var candidateA = new Vector2I(origin.X + dx, origin.Y + dy); + if (IsCellInBounds(candidateA) && !_astarGrid.IsPointSolid(candidateA)) + { + return candidateA; + } + + if (dy == 0) continue; + var candidateB = new Vector2I(origin.X + dx, origin.Y - dy); + if (IsCellInBounds(candidateB) && !_astarGrid.IsPointSolid(candidateB)) + { + return candidateB; + } + } + } + + return origin; + } + + private bool IsWorldWalkable(Vector2 world) + { + if (!IsNavigationMapReady()) return false; + var closest = NavigationServer2D.MapGetClosestPoint(_navigationMap, world); + return closest.DistanceTo(world) <= GetGridTolerance(); + } + + private NavigationRegion2D FindNavigationRegion() + { + return GetTree()?.CurrentScene?.FindChild("NavigationRegion2D", true, false) as NavigationRegion2D; + } + + private bool IsCellWalkable(Vector2 center) + { + if (!IsWorldWalkable(center)) return false; + + var half = Mathf.Max(1.0f, GridCellSize * 0.45f); + var offsets = new[] + { + new Vector2(half, 0), + new Vector2(-half, 0), + new Vector2(0, half), + new Vector2(0, -half), + new Vector2(half, half), + new Vector2(half, -half), + new Vector2(-half, half), + new Vector2(-half, -half) + }; + + for (var i = 0; i < offsets.Length; i++) + { + if (!IsWorldWalkable(center + offsets[i])) + { + return false; + } + } + + return true; + } + + private float GetGridTolerance() + { + return Mathf.Max(1.0f, GridWalkableTolerance); + } + private enum FacingDirection { Up,