AStarGrid 未完成
This commit is contained in:
parent
206e5622bb
commit
3a3b09c2c5
@ -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
|
||||
|
||||
|
||||
@ -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")
|
||||
|
||||
@ -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<Vector2> _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>("NavigationAgent2D");
|
||||
_animationPlayer = GetNodeOrNull<AnimationPlayer>("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<Vector2> points, int startIndex)
|
||||
{
|
||||
_patrolPoints = points ?? new List<Vector2>();
|
||||
@ -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<Vector2> 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<Vector2>(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<Vector2> 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<Vector2> SimplifyGridPath(List<Vector2> path)
|
||||
{
|
||||
if (path == null || path.Count < 3) return path;
|
||||
|
||||
var simplified = new List<Vector2> { 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,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user