471 lines
17 KiB
C#
471 lines
17 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Linq;
|
|
using Godot;
|
|
using GodotComponentTest.utils;
|
|
|
|
/// <summary>
|
|
/// </summary>
|
|
public class NavigationComponent : Spatial
|
|
{
|
|
public World World { set; get; }
|
|
public Vector3 CurrentGoalPositionWorld { get; private set; } = Vector3.Zero;
|
|
public float CurrentGoalAngleWorld { get; private set; }
|
|
|
|
private NavigationPoint _currentGoal;
|
|
private HexCell[] _path;
|
|
private List<NavigationPoint> _pathWorldNavigationPoints = new();
|
|
private List<NavigationPoint> _planningPathSmoothedWorldNavigationPoints = new();
|
|
|
|
private List<NavigationPoint> _planningPathWorldNavigationPoints = new();
|
|
private List<NavigationPoint> _smoothedPathWorldNavigationPoints = new();
|
|
|
|
public override void _Ready()
|
|
{
|
|
base._Ready();
|
|
_pathWorldNavigationPoints = new List<NavigationPoint>();
|
|
}
|
|
|
|
public override void _Process(float delta)
|
|
{
|
|
Debug.Assert(World != null);
|
|
}
|
|
|
|
public void PlanSmoothedPath(KinematicBody body, Transform fromTransformWorld, NavigationPoint navigationPoint)
|
|
{
|
|
if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
|
|
&& navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
|
|
{
|
|
FindPath(body, fromTransformWorld.origin, navigationPoint);
|
|
}
|
|
else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
|
|
{
|
|
FindPath(body, fromTransformWorld.origin, navigationPoint.WorldPosition);
|
|
}
|
|
else
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
}
|
|
|
|
public void FindPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
|
|
{
|
|
var fromCell = World.HexGrid.GetHexAt(new Vector2(fromPositionWorld.x, fromPositionWorld.z));
|
|
if (World.HexGrid.GetHexCost(fromCell) == 0)
|
|
{
|
|
GD.Print("Invalid starting point for FindPath(): returning empty path.");
|
|
_planningPathWorldNavigationPoints = new List<NavigationPoint>();
|
|
_planningPathWorldNavigationPoints.Add(new NavigationPoint(fromPositionWorld));
|
|
_planningPathSmoothedWorldNavigationPoints = _planningPathWorldNavigationPoints;
|
|
return;
|
|
}
|
|
|
|
var toCell = World.HexGrid.GetHexAt(new Vector2(toPositionWorld.x, toPositionWorld.z));
|
|
toCell = World.HexGrid.GetClosestWalkableCell(fromCell, toCell);
|
|
|
|
if (World.HexGrid.GetHexCost(toCell) == 0)
|
|
{
|
|
GD.Print("Invalid target point for FindPath(): returning empty path.");
|
|
_planningPathWorldNavigationPoints = new List<NavigationPoint>();
|
|
_planningPathWorldNavigationPoints.Add(new NavigationPoint(fromPositionWorld));
|
|
_planningPathSmoothedWorldNavigationPoints = _planningPathWorldNavigationPoints;
|
|
return;
|
|
}
|
|
|
|
var path = World.HexGrid.FindPath(fromCell, toCell);
|
|
|
|
// Generate grid navigation points
|
|
_planningPathWorldNavigationPoints = new List<NavigationPoint>();
|
|
foreach (var index in Enumerable.Range(0, path.Count))
|
|
{
|
|
_planningPathWorldNavigationPoints.Add(
|
|
new NavigationPoint(World.HexGrid.GetHexCenterVec3FromOffset(path[index].OffsetCoords)));
|
|
}
|
|
|
|
// Ensure the last point coincides with the target position
|
|
if (_planningPathWorldNavigationPoints.Count > 0 &&
|
|
(_planningPathWorldNavigationPoints.Last().WorldPosition - toPositionWorld).LengthSquared() <
|
|
0.5f * 0.5f)
|
|
{
|
|
_planningPathWorldNavigationPoints[_planningPathWorldNavigationPoints.Count - 1].WorldPosition =
|
|
toPositionWorld;
|
|
}
|
|
|
|
// Perform smoothing
|
|
_planningPathSmoothedWorldNavigationPoints = SmoothPath(body, _planningPathWorldNavigationPoints);
|
|
|
|
// Ensure starting point is the current position
|
|
if (_planningPathSmoothedWorldNavigationPoints.Count > 0)
|
|
{
|
|
_planningPathSmoothedWorldNavigationPoints[0] = new NavigationPoint(fromPositionWorld);
|
|
}
|
|
}
|
|
|
|
public void FindPath(KinematicBody body, Vector3 fromPositionWorld, NavigationPoint navigationPoint)
|
|
{
|
|
FindPath(body, fromPositionWorld, navigationPoint.WorldPosition);
|
|
|
|
_planningPathWorldNavigationPoints[_planningPathWorldNavigationPoints.Count - 1] = navigationPoint;
|
|
_planningPathSmoothedWorldNavigationPoints[_planningPathSmoothedWorldNavigationPoints.Count - 1] =
|
|
navigationPoint;
|
|
}
|
|
|
|
public void PlanGridPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
|
|
{
|
|
var fromPositionOffset = World.WorldToOffsetCoords(fromPositionWorld);
|
|
var toPositionOffset = World.WorldToOffsetCoords(toPositionWorld);
|
|
|
|
var fromCell = new HexCell();
|
|
fromCell.OffsetCoords = fromPositionOffset;
|
|
|
|
var toCell = new HexCell();
|
|
toCell.OffsetCoords = toPositionOffset;
|
|
|
|
_path = fromCell.LineTo(toCell);
|
|
Debug.Assert(_path.Length > 0);
|
|
|
|
_pathWorldNavigationPoints = new List<NavigationPoint>();
|
|
_pathWorldNavigationPoints.Add(
|
|
new NavigationPoint(World.HexGrid.GetHexCenterVec3FromOffset(fromPositionOffset)));
|
|
|
|
foreach (var index in Enumerable.Range(1, _path.Length - 1))
|
|
{
|
|
_pathWorldNavigationPoints.Add(
|
|
new NavigationPoint(World.GetHexCenterFromOffset(_path[index].OffsetCoords)));
|
|
}
|
|
|
|
if ((fromPositionWorld - World.GetHexCenterFromOffset(toCell.OffsetCoords)).LengthSquared() >
|
|
Globals.EpsPositionSquared)
|
|
{
|
|
// Remove the last one, because it is only the position rounded to HexGrid coordinates.
|
|
if (_pathWorldNavigationPoints.Count > 0)
|
|
{
|
|
_pathWorldNavigationPoints.RemoveAt(_pathWorldNavigationPoints.Count - 1);
|
|
}
|
|
}
|
|
|
|
_pathWorldNavigationPoints.Add(new NavigationPoint(toPositionWorld));
|
|
if (_pathWorldNavigationPoints.Count > 2)
|
|
{
|
|
_smoothedPathWorldNavigationPoints = SmoothPath(body, _pathWorldNavigationPoints);
|
|
_pathWorldNavigationPoints = _smoothedPathWorldNavigationPoints;
|
|
}
|
|
|
|
UpdateCurrentGoal();
|
|
}
|
|
|
|
|
|
public void PlanGridPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld,
|
|
Quat toWorldOrientation)
|
|
{
|
|
PlanGridPath(body, fromPositionWorld, toPositionWorld);
|
|
|
|
_pathWorldNavigationPoints.Add(new NavigationPoint(toWorldOrientation));
|
|
}
|
|
|
|
|
|
public void PlanGridPath(KinematicBody body, Transform fromTransformWorld, NavigationPoint navigationPoint)
|
|
{
|
|
if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
|
|
&& navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
|
|
{
|
|
PlanGridPath(body, fromTransformWorld.origin, navigationPoint.WorldPosition,
|
|
navigationPoint.WorldOrientation);
|
|
}
|
|
else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
|
|
{
|
|
PlanGridPath(body, fromTransformWorld.origin, navigationPoint.WorldPosition);
|
|
}
|
|
else
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
}
|
|
|
|
|
|
public void PlanDirectPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
|
|
{
|
|
_pathWorldNavigationPoints.Clear();
|
|
_pathWorldNavigationPoints.Add(new NavigationPoint(toPositionWorld));
|
|
|
|
UpdateCurrentGoal();
|
|
}
|
|
|
|
|
|
public void PlanDirectPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld,
|
|
Quat toWorldOrientation)
|
|
{
|
|
PlanDirectPath(body, fromPositionWorld, toPositionWorld);
|
|
|
|
_pathWorldNavigationPoints.Add(new NavigationPoint(toWorldOrientation));
|
|
}
|
|
|
|
|
|
public bool HasPathCollision(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
|
|
{
|
|
Vector3 fromPositionLocal = GlobalTransform.XformInv(fromPositionWorld);
|
|
Vector3 toPositionLocal = GlobalTransform.XformInv(toPositionWorld);
|
|
Vector3 relativeVelocity = GlobalTransform.basis.Xform(toPositionLocal - fromPositionLocal);
|
|
|
|
KinematicCollision moveCollision = body.MoveAndCollide(relativeVelocity, true, true, true);
|
|
if (moveCollision != null)
|
|
{
|
|
Spatial colliderSpatial = moveCollision.Collider as Spatial;
|
|
// GD.Print("Found collision: " + moveCollision.Collider + " (" + colliderSpatial.Name + ")");
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
public 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<HexCell> cells =
|
|
World.HexGrid.GetCellsForLine(startPlane + directionPlane * radius, endPlane + directionPlane * radius);
|
|
foreach (HexCell cell in cells)
|
|
{
|
|
if (World.HexGrid.GetHexCost(cell) == 0)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
cells = World.HexGrid.GetCellsForLine(startPlane + sidePlane * radius, endPlane + sidePlane * radius);
|
|
foreach (HexCell cell in cells)
|
|
{
|
|
if (World.HexGrid.GetHexCost(cell) == 0)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
cells = World.HexGrid.GetCellsForLine(startPlane - sidePlane * radius, endPlane - sidePlane * radius);
|
|
foreach (HexCell cell in cells)
|
|
{
|
|
if (World.HexGrid.GetHexCost(cell) == 0)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
public List<NavigationPoint> SmoothPath(KinematicBody body, List<NavigationPoint> navigationPoints)
|
|
{
|
|
if (navigationPoints.Count <= 2)
|
|
{
|
|
return navigationPoints;
|
|
}
|
|
|
|
Vector3 bodyGlobalTranslation = body.GlobalTranslation;
|
|
List<NavigationPoint> smoothedPath = new List<NavigationPoint>();
|
|
|
|
int startIndex = 0;
|
|
int endIndex = navigationPoints.Count > 1 ? 1 : 0;
|
|
smoothedPath.Add(navigationPoints[startIndex]);
|
|
Vector3 startPoint = navigationPoints[startIndex].WorldPosition;
|
|
|
|
while (endIndex != navigationPoints.Count)
|
|
{
|
|
Vector3 endPoint = navigationPoints[endIndex].WorldPosition;
|
|
|
|
if (CheckSweptTriangleCellCollision(startPoint, endPoint, 0.27f))
|
|
{
|
|
if (endIndex - startIndex == 1)
|
|
{
|
|
GD.Print("Aborting SmoothPath: input path passes through collision geometry.");
|
|
body.GlobalTranslation = bodyGlobalTranslation;
|
|
return smoothedPath;
|
|
}
|
|
|
|
smoothedPath.Add(navigationPoints[endIndex - 1]);
|
|
startIndex = endIndex - 1;
|
|
startPoint = navigationPoints[startIndex].WorldPosition;
|
|
body.GlobalTranslation = startPoint;
|
|
|
|
continue;
|
|
}
|
|
|
|
if (endIndex == navigationPoints.Count - 1)
|
|
{
|
|
break;
|
|
}
|
|
|
|
endIndex += 1;
|
|
}
|
|
|
|
smoothedPath.Add(navigationPoints[endIndex]);
|
|
body.GlobalTranslation = bodyGlobalTranslation;
|
|
|
|
return smoothedPath;
|
|
}
|
|
|
|
public void PlanDirectPath(KinematicBody body, Transform fromTransformWorld, NavigationPoint navigationPoint)
|
|
{
|
|
if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
|
|
&& navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
|
|
{
|
|
PlanDirectPath(body, fromTransformWorld.origin, navigationPoint.WorldPosition,
|
|
navigationPoint.WorldOrientation);
|
|
}
|
|
else if (navigationPoint.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
|
|
{
|
|
PlanDirectPath(body, fromTransformWorld.origin, navigationPoint.WorldPosition);
|
|
}
|
|
else
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
}
|
|
|
|
public void ActivatePlannedPath()
|
|
{
|
|
_pathWorldNavigationPoints = _planningPathSmoothedWorldNavigationPoints;
|
|
UpdateCurrentGoal();
|
|
}
|
|
|
|
private void UpdateCurrentGoal()
|
|
{
|
|
if (_pathWorldNavigationPoints.Count == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_currentGoal = _pathWorldNavigationPoints[0];
|
|
CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
|
|
CurrentGoalAngleWorld = _pathWorldNavigationPoints[0].WorldAngle;
|
|
}
|
|
|
|
private void ApplyExistingTransform(Transform worldTransform)
|
|
{
|
|
if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Orientation)
|
|
{
|
|
CurrentGoalPositionWorld = worldTransform.origin;
|
|
}
|
|
else if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Position)
|
|
{
|
|
CurrentGoalAngleWorld = Globals.CalcPlaneAngle(worldTransform);
|
|
}
|
|
}
|
|
|
|
|
|
public void UpdateCurrentGoal(Transform currentTransformWorld)
|
|
{
|
|
if (_currentGoal == null)
|
|
{
|
|
_currentGoal = new NavigationPoint(currentTransformWorld);
|
|
}
|
|
|
|
if (_pathWorldNavigationPoints.Count == 0)
|
|
{
|
|
CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
|
|
CurrentGoalPositionWorld = currentTransformWorld.origin;
|
|
return;
|
|
}
|
|
|
|
if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
|
|
{
|
|
CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
|
|
}
|
|
else
|
|
{
|
|
CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
|
|
}
|
|
|
|
if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
|
|
{
|
|
CurrentGoalAngleWorld = _currentGoal.WorldAngle;
|
|
}
|
|
else
|
|
{
|
|
CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
|
|
}
|
|
|
|
if (_currentGoal.IsReached(currentTransformWorld))
|
|
{
|
|
_pathWorldNavigationPoints.RemoveAt(0);
|
|
|
|
UpdateCurrentGoal();
|
|
ApplyExistingTransform(currentTransformWorld);
|
|
}
|
|
|
|
if (_pathWorldNavigationPoints.Count == 0)
|
|
{
|
|
CurrentGoalPositionWorld = currentTransformWorld.origin;
|
|
CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
|
|
}
|
|
}
|
|
|
|
public bool IsGoalReached()
|
|
{
|
|
return _pathWorldNavigationPoints.Count == 0;
|
|
}
|
|
|
|
public void DebugDraw(Spatial parentNode, DebugGeometry debugGeometry)
|
|
{
|
|
Vector3 yOffset = Vector3.Up * 0.1f;
|
|
|
|
debugGeometry.GlobalTransform = Transform.Identity;
|
|
debugGeometry.Begin(Mesh.PrimitiveType.Lines);
|
|
Color pinkColor = Colors.Pink;
|
|
pinkColor.a = 1;
|
|
debugGeometry.SetColor(pinkColor);
|
|
debugGeometry.AddVertex(parentNode.GlobalTranslation + yOffset);
|
|
debugGeometry.SetColor(pinkColor);
|
|
debugGeometry.AddVertex(CurrentGoalPositionWorld + yOffset);
|
|
debugGeometry.SetColor(pinkColor);
|
|
|
|
debugGeometry.PushTranslated(CurrentGoalPositionWorld);
|
|
debugGeometry.AddBox(Vector3.One * 1);
|
|
debugGeometry.PopTransform();
|
|
|
|
Vector3 previousPoint = parentNode.GlobalTranslation;
|
|
foreach (NavigationPoint point in _pathWorldNavigationPoints)
|
|
{
|
|
debugGeometry.AddVertex(previousPoint + yOffset);
|
|
debugGeometry.AddVertex(point.WorldPosition + yOffset);
|
|
|
|
previousPoint = point.WorldPosition;
|
|
}
|
|
|
|
previousPoint = parentNode.GlobalTranslation;
|
|
foreach (NavigationPoint point in _smoothedPathWorldNavigationPoints)
|
|
{
|
|
debugGeometry.SetColor(new Color(0, 0, 1));
|
|
debugGeometry.AddVertex(previousPoint + yOffset);
|
|
debugGeometry.AddVertex(point.WorldPosition + yOffset);
|
|
|
|
previousPoint = point.WorldPosition;
|
|
}
|
|
|
|
previousPoint = parentNode.GlobalTranslation;
|
|
foreach (NavigationPoint point in _planningPathWorldNavigationPoints)
|
|
{
|
|
debugGeometry.SetColor(new Color(1, 0, 1));
|
|
debugGeometry.AddVertex(previousPoint + yOffset);
|
|
debugGeometry.AddVertex(point.WorldPosition + yOffset);
|
|
|
|
previousPoint = point.WorldPosition;
|
|
}
|
|
|
|
previousPoint = parentNode.GlobalTranslation;
|
|
foreach (NavigationPoint point in _planningPathSmoothedWorldNavigationPoints)
|
|
{
|
|
debugGeometry.SetColor(new Color(1, 1, 0));
|
|
debugGeometry.AddVertex(previousPoint + yOffset);
|
|
debugGeometry.AddVertex(point.WorldPosition + yOffset);
|
|
|
|
previousPoint = point.WorldPosition;
|
|
}
|
|
|
|
debugGeometry.End();
|
|
}
|
|
} |