diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs index 0c4d374..5bee713 100644 --- a/components/NavigationComponent.cs +++ b/components/NavigationComponent.cs @@ -252,6 +252,44 @@ public class NavigationComponent : Spatial return false; } + + bool CheckSweptTriangleCellCollision(Vector3 startWorld, Vector3 endWorld, float radius) + { + Vector2 startPlane = new Vector2(startWorld.x, startWorld.z); + Vector2 endPlane = new Vector2(endWorld.x, endWorld.z); + Vector2 directionPlane = (endPlane - startPlane).Normalized(); + Vector2 sidePlane = directionPlane.Rotated(Mathf.Pi * 0.5f); + + List cells = TileWorld.HexGrid.GetCellsForLine(startPlane + directionPlane * radius, endPlane + directionPlane * radius); + foreach (HexCell cell in cells) + { + if (TileWorld.HexGrid.GetHexCost(cell) == 0) + { + return true; + } + } + + cells = TileWorld.HexGrid.GetCellsForLine(startPlane + sidePlane * radius, endPlane + sidePlane * radius); + foreach (HexCell cell in cells) + { + if (TileWorld.HexGrid.GetHexCost(cell) == 0) + { + return true; + } + } + + cells = TileWorld.HexGrid.GetCellsForLine(startPlane - sidePlane * radius, endPlane - sidePlane * radius); + foreach (HexCell cell in cells) + { + if (TileWorld.HexGrid.GetHexCost(cell) == 0) + { + return true; + } + } + + return false; + } + public List SmoothPath(KinematicBody body, List navigationPoints) { if (navigationPoints.Count <= 2) @@ -271,7 +309,7 @@ public class NavigationComponent : Spatial { Vector3 endPoint = navigationPoints[endIndex].WorldPosition; - if (HasPathCollision(body, startPoint, endPoint)) + if (CheckSweptTriangleCellCollision(startPoint, endPoint, 0.27f)) { if (endIndex - startIndex == 1) {