GodotComponentTest/components/NavigationComponent.cs

466 lines
17 KiB
C#

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Numerics;
using Godot;
using GodotComponentTest.utils;
using Vector2 = Godot.Vector2;
using Vector3 = Godot.Vector3;
using GoDotLog;
/// <summary>
/// </summary>
public class NavigationComponent : Spatial
{
public class NavigationPoint
{
[Flags]
public enum NavigationFlags
{
Position = 1,
Orientation = 2
}
public Vector3 WorldPosition = Vector3.Zero;
public Quat WorldOrientation = Quat.Identity;
public NavigationFlags Flags = NavigationFlags.Position | NavigationFlags.Orientation;
public NavigationPoint(Vector3 worldPosition)
{
WorldPosition = worldPosition;
Flags = NavigationFlags.Position;
}
public NavigationPoint(Quat worldOrientation)
{
WorldOrientation = worldOrientation;
Flags = NavigationFlags.Orientation;
}
public NavigationPoint(Transform worldTransform)
{
WorldPosition = worldTransform.origin;
WorldOrientation = worldTransform.basis.Quat();
Flags = NavigationFlags.Position | NavigationFlags.Orientation;
}
public bool IsReached(Transform worldTransform)
{
bool goalReached = false;
float positionErrorSquared = (worldTransform.origin - WorldPosition).LengthSquared();
Quat own_orientation = worldTransform.basis.Quat();
float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));
if (Flags.HasFlag(NavigationPoint.NavigationFlags.Position)
&& Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation)
&& positionErrorSquared < Globals.EpsPositionSquared
&& orientationError < Globals.EpsRadians)
{
goalReached = true;
}
else if (Flags == NavigationPoint.NavigationFlags.Position &&
positionErrorSquared < Globals.EpsPositionSquared)
{
goalReached = true;
}
else if (Flags == NavigationPoint.NavigationFlags.Orientation &&
orientationError < Globals.EpsRadians)
{
goalReached = true;
}
return goalReached;
}
}
public TileWorld TileWorld { set; get; }
public Vector3 CurrentGoalPositionWorld => _currentGoalPositionWorld;
public Quat CurrentGoalOrientationWorld => _currentGoalOrientationWorld;
private NavigationPoint _currentGoal;
private Vector3 _currentGoalPositionWorld = Vector3.Zero;
private Quat _currentGoalOrientationWorld = Quat.Identity;
private List<NavigationPoint> _planningPathWorldNavigationPoints = new List<NavigationPoint>();
private List<NavigationPoint> _planningPathSmoothedWorldNavigationPoints = new List<NavigationPoint>();
private List<NavigationPoint> _pathWorldNavigationPoints = new List<NavigationPoint>();
private List<NavigationPoint> _smoothedPathWorldNavigationPoints = new List<NavigationPoint>();
private HexCell[] _path;
public override void _Ready()
{
base._Ready();
_pathWorldNavigationPoints = new List<NavigationPoint>();
}
public override void _Process(float delta)
{
Debug.Assert(TileWorld != null);
}
public void FindPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
{
HexCell fromCell = TileWorld.HexGrid.GetHexAt(new Vector2(fromPositionWorld.x, fromPositionWorld.z));
if (TileWorld.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;
}
HexCell toCell = TileWorld.HexGrid.GetHexAt(new Vector2(toPositionWorld.x, toPositionWorld.z));
toCell = TileWorld.HexGrid.GetClosestWalkableCell(fromCell, toCell);
if (TileWorld.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;
}
List<HexCell> path = TileWorld.HexGrid.FindPath(fromCell, toCell);
// GD.Print("Planning path and have " + TileWorld.HexGrid.Obstacles.Count + " obstacles:");
// GD.Print(TileWorld.HexGrid.Obstacles);
// foreach (Vector2 coord in TileWorld.HexGrid.Obstacles.Keys)
// {
// HexCell cell = new HexCell(coord);
// GD.Print(" " + cell.OffsetCoords);
// }
_planningPathWorldNavigationPoints = new List<NavigationPoint>();
foreach (int index in Enumerable.Range(0, path.Count))
{
// GD.Print("Step " + index + ": " + path[index].OffsetCoords);
_planningPathWorldNavigationPoints.Add(
new NavigationPoint(TileWorld.GetHexCenterFromOffset(path[index].OffsetCoords)));
}
_planningPathSmoothedWorldNavigationPoints = SmoothPath(body, _planningPathWorldNavigationPoints);
}
public void PlanGridPath(KinematicBody body, Vector3 fromPositionWorld, Vector3 toPositionWorld)
{
Vector2 fromPositionOffset = TileWorld.WorldToOffsetCoords(fromPositionWorld);
Vector2 toPositionOffset = TileWorld.WorldToOffsetCoords(toPositionWorld);
HexCell fromCell = new HexCell();
fromCell.OffsetCoords = fromPositionOffset;
HexCell toCell = new HexCell();
toCell.OffsetCoords = toPositionOffset;
_path = fromCell.LineTo(toCell);
Debug.Assert(_path.Length > 0);
_pathWorldNavigationPoints = new List<NavigationPoint>();
_pathWorldNavigationPoints.Add(
new NavigationPoint(TileWorld.GetHexCenterFromOffset(fromPositionOffset)));
foreach (int index in Enumerable.Range(1, _path.Length - 1))
{
_pathWorldNavigationPoints.Add(
new NavigationPoint(TileWorld.GetHexCenterFromOffset(_path[index].OffsetCoords)));
}
if ((fromPositionWorld - TileWorld.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 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 (HasPathCollision(body, startPoint, endPoint))
{
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;
_currentGoalOrientationWorld = _pathWorldNavigationPoints[0].WorldOrientation;
// GD.Print("Navigation Goal: pos " + _currentGoal.WorldPosition + " " + " rot: " + _currentGoal.WorldOrientation +
// " flags: " + _currentGoal.Flags + " path length: " +
// _pathWorldNavigationPoints.Count);
}
private void ApplyExistingTransform(Transform worldTransform)
{
if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Orientation)
{
_currentGoalPositionWorld = worldTransform.origin;
}
else if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Position)
{
_currentGoalOrientationWorld = worldTransform.basis.Quat();
}
}
public void UpdateCurrentGoal(Transform currentTransformWorld)
{
if (_currentGoal == null)
{
_currentGoal = new NavigationPoint(currentTransformWorld);
}
if (_pathWorldNavigationPoints.Count == 0)
{
_currentGoalOrientationWorld = currentTransformWorld.basis.Quat();
_currentGoalPositionWorld = currentTransformWorld.origin;
return;
}
if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Position))
{
_currentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
}
else
{
_currentGoalPositionWorld = currentTransformWorld.origin;
}
if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
{
_currentGoalOrientationWorld = _currentGoal.WorldOrientation;
}
else
{
_currentGoalOrientationWorld = currentTransformWorld.basis.Quat();
}
if (_currentGoal.IsReached(currentTransformWorld))
{
_pathWorldNavigationPoints.RemoveAt(0);
UpdateCurrentGoal();
ApplyExistingTransform(currentTransformWorld);
}
if (_pathWorldNavigationPoints.Count == 0)
{
_currentGoalOrientationWorld = currentTransformWorld.basis.Quat();
_currentGoalPositionWorld = currentTransformWorld.origin;
}
}
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, 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, 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, 1));
debugGeometry.AddVertex(previousPoint + yOffset);
debugGeometry.AddVertex(point.WorldPosition + yOffset);
previousPoint = point.WorldPosition;
}
debugGeometry.End();
}
}