From b968f9b3b27e6717b05b0a875af926eabb477263 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Fri, 10 Nov 2023 12:22:17 +0100 Subject: [PATCH] Navigation with plange angle targets now functional again. --- Globals.cs | 9 +++- components/GroundMotionComponent.cs | 26 ++++++--- components/NavigationComponent.cs | 83 +++-------------------------- components/TaskQueueComponent.cs | 6 +-- entities/Entity.cs | 2 +- entities/Player.cs | 8 +-- scenes/Game.cs | 4 +- utils/NavigationPoint.cs | 70 ++++++++++++++++++++++++ 8 files changed, 112 insertions(+), 96 deletions(-) create mode 100644 utils/NavigationPoint.cs diff --git a/Globals.cs b/Globals.cs index 72c9641..445f192 100644 --- a/Globals.cs +++ b/Globals.cs @@ -1,9 +1,14 @@ -using System; +using Godot; public static class Globals { public const float EpsPosition = 0.01f; public const float EpsPositionSquared = EpsPosition * EpsPosition; - public const float EpsRadians = 0.1f * Godot.Mathf.Pi / 180f; + public const float EpsRadians = 0.1f * Mathf.Pi / 180f; public const float EpsRadiansSquared = EpsRadians * EpsRadians; + + public static float CalcPlaneAngle(Transform worldTransform) + { + return worldTransform.basis.x.SignedAngleTo(Vector3.Right.Rotated(Vector3.Up, Mathf.Pi * 0.5f), -Vector3.Up); + } } \ No newline at end of file diff --git a/components/GroundMotionComponent.cs b/components/GroundMotionComponent.cs index b2cba84..60024e1 100644 --- a/components/GroundMotionComponent.cs +++ b/components/GroundMotionComponent.cs @@ -14,7 +14,7 @@ public class GroundMotionComponent : Component public float TargetAngle; public float TargetDeltaAngle; - private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation) + private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, float targetAngle) { float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle); if (deltaAngleAbsolute > 0.001) @@ -104,7 +104,7 @@ public class GroundMotionComponent : Component entity.Velocity = entityVelocity; } - public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation, + public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, float targetAngle, World world) { DirectionToTarget = targetPosition - entity.GlobalTranslation; @@ -119,16 +119,28 @@ public class GroundMotionComponent : Component } else { - GD.Print("Target Position reached!"); DirectionToTarget = Vector3.Right; - TargetAngle = entity.PlaneAngle; - TargetDeltaAngle = 0; entity.GlobalTranslation = targetPosition; entity.Velocity = new Vector3(0, entity.Velocity.y, 0); - return; + + if (entity.PlaneAngle != targetAngle) + { + Vector3 directionToTarget = Vector3.Right.Rotated(Vector3.Up, targetAngle); + TargetAngle = targetAngle; + TargetDeltaAngle = entity.CalcShortestPlaneRotationToTargetDirection(directionToTarget); + + if (Mathf.Abs(TargetDeltaAngle) < Globals.EpsRadians) + { + TargetAngle = entity.PlaneAngle; + TargetDeltaAngle = 0; + + return; + } + } } - CalcAndApplyOrientation(delta, entity, targetPosition, targetOrientation); + CalcAndApplyOrientation(delta, entity, targetPosition, targetAngle); + CalcPlaneVelocity(delta, entity, targetPosition); // CalcVerticalVelocity(delta, entity, tileWorld); diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs index e76a62b..9323f65 100644 --- a/components/NavigationComponent.cs +++ b/components/NavigationComponent.cs @@ -11,8 +11,7 @@ public class NavigationComponent : Spatial { public World World { set; get; } public Vector3 CurrentGoalPositionWorld { get; private set; } = Vector3.Zero; - public float CurrentGoalAngleWorld { get; } = 0; - public Quat CurrentGoalOrientationWorld { get; private set; } = Quat.Identity; + public float CurrentGoalAngleWorld { get; private set; } private NavigationPoint _currentGoal; private HexCell[] _path; @@ -342,11 +341,7 @@ public class NavigationComponent : Spatial _currentGoal = _pathWorldNavigationPoints[0]; CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition; - //_currentGoalOrientationWorld = Vector3.Right.SignedAngleTo(_pathWorldNavigationPoints[0].WorldOrientation); - -// GD.Print("Navigation Goal: pos " + _currentGoal.WorldPosition + " " + " rot: " + _currentGoal.WorldOrientation + -// " flags: " + _currentGoal.Flags + " path length: " + -// _pathWorldNavigationPoints.Count); + CurrentGoalAngleWorld = _pathWorldNavigationPoints[0].WorldAngle; } private void ApplyExistingTransform(Transform worldTransform) @@ -357,7 +352,7 @@ public class NavigationComponent : Spatial } else if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Position) { - CurrentGoalOrientationWorld = worldTransform.basis.Quat(); + CurrentGoalAngleWorld = Globals.CalcPlaneAngle(worldTransform); } } @@ -371,7 +366,7 @@ public class NavigationComponent : Spatial if (_pathWorldNavigationPoints.Count == 0) { - CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat(); + CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld); CurrentGoalPositionWorld = currentTransformWorld.origin; return; } @@ -382,22 +377,18 @@ public class NavigationComponent : Spatial } else { - CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat(); + CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld); } if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation)) { - CurrentGoalOrientationWorld = _currentGoal.WorldOrientation; + CurrentGoalAngleWorld = _currentGoal.WorldAngle; } else { - CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat(); + CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld); } - // Vector3 currentWorldXAxis = currentTransformWorld.basis.x; - // Debug.Assert(Mathf.Abs(currentWorldXAxis.y) < 0.9); - // float angle = Mathf.Atan2(currentWorldXAxis.y, currentWorldXAxis.x); - // _currentGoalOrientationWorld = Basis.Identity.Rotated(Vector3.Up, angle).Quat(); if (_currentGoal.IsReached(currentTransformWorld)) { _pathWorldNavigationPoints.RemoveAt(0); @@ -408,8 +399,8 @@ public class NavigationComponent : Spatial if (_pathWorldNavigationPoints.Count == 0) { - CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat(); CurrentGoalPositionWorld = currentTransformWorld.origin; + CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld); } } @@ -477,62 +468,4 @@ public class NavigationComponent : Spatial debugGeometry.End(); } - - public class NavigationPoint - { - [Flags] - public enum NavigationFlags - { - Position = 1, - Orientation = 2 - } - - public readonly NavigationFlags Flags; - public Quat WorldOrientation = Quat.Identity; - - public Vector3 WorldPosition = Vector3.Zero; - - 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) - { - var goalReached = false; - var positionError = new Vector2(WorldPosition.x - worldTransform.origin.x, - WorldPosition.z - worldTransform.origin.z); - var positionErrorSquared = positionError.LengthSquared(); - worldTransform.basis.Quat(); - var orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation)); - - if (Flags.HasFlag(NavigationFlags.Position) - && Flags.HasFlag(NavigationFlags.Orientation) - && positionErrorSquared < Globals.EpsPositionSquared - && orientationError < Globals.EpsRadians) - goalReached = true; - else if (Flags == NavigationFlags.Position && - positionErrorSquared < Globals.EpsPositionSquared) - goalReached = true; - else if (Flags == NavigationFlags.Orientation && - orientationError < Globals.EpsRadians) - goalReached = true; - - return goalReached; - } - } } \ No newline at end of file diff --git a/components/TaskQueueComponent.cs b/components/TaskQueueComponent.cs index 885dc08..8a9f4f3 100644 --- a/components/TaskQueueComponent.cs +++ b/components/TaskQueueComponent.cs @@ -5,7 +5,7 @@ public class TaskQueueComponent : Component { [Signal] private delegate void StartInteraction(Entity entity, Entity targetEntity); - + public Queue Queue; public TaskQueueComponent() @@ -51,10 +51,10 @@ public class TaskQueueComponent : Component /// public class NavigationTask : Task { - public NavigationComponent.NavigationPoint NavigationPoint; + public NavigationPoint NavigationPoint; public bool PlanningComplete = false; - public NavigationTask(NavigationComponent.NavigationPoint navigationPoint) + public NavigationTask(NavigationPoint navigationPoint) { NavigationPoint = navigationPoint; } diff --git a/entities/Entity.cs b/entities/Entity.cs index e922066..3f13a0c 100644 --- a/entities/Entity.cs +++ b/entities/Entity.cs @@ -9,7 +9,7 @@ public class Entity : KinematicBody /** Defines the angle in plane coordinates, 0 => pointing to the right/east, pi/2 pointing up/north, range [-pi,pi]. */ public float PlaneAngle { - get => GlobalTransform.basis.x.SignedAngleTo(Vector3.Right.Rotated(Vector3.Up, Mathf.Pi * 0.5f), -Vector3.Up); + get => Globals.CalcPlaneAngle(GlobalTransform); set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation); } diff --git a/entities/Player.cs b/entities/Player.cs index b0de207..410f849 100644 --- a/entities/Player.cs +++ b/entities/Player.cs @@ -98,10 +98,10 @@ public class Player : Entity, IInteractionInterface NavigationComponent.UpdateCurrentGoal(GlobalTransform); _groundMotion.PhysicsProcess(delta, this, NavigationComponent.CurrentGoalPositionWorld, - NavigationComponent.CurrentGoalOrientationWorld, _worldInfo.World); + NavigationComponent.CurrentGoalAngleWorld, _worldInfo.World); if (NavigationComponent.IsGoalReached()) - navigationTask.NavigationPoint = new NavigationComponent.NavigationPoint(GlobalTransform); + navigationTask.NavigationPoint = new NavigationPoint(GlobalTransform); } } } @@ -139,20 +139,16 @@ public class Player : Entity, IInteractionInterface if (NavigationComponent != null) { NavigationComponent.DebugDraw(this, _debugGeometry); - return; } } public void OnItemAttractorBodyEntered(Node node) { - GD.Print("Item entered " + node); _attractedItemList.Add(node); } public void OnItemAttractorBodyExited(Node node) { - GD.Print("Item exited " + node); - if (node is GoldBar) { GoldBar bar = (GoldBar)node; diff --git a/scenes/Game.cs b/scenes/Game.cs index e8fe5a8..60df2b4 100644 --- a/scenes/Game.cs +++ b/scenes/Game.cs @@ -206,7 +206,7 @@ public class Game : Spatial _player.TaskQueueComponent.Reset(); _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask( - new NavigationComponent.NavigationPoint(tile.GlobalTranslation))); + new NavigationPoint(tile.GlobalTranslation))); } public void OnTileHovered(HexTile3D tile) @@ -229,7 +229,7 @@ public class Game : Spatial { _player.TaskQueueComponent.Reset(); _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask( - new NavigationComponent.NavigationPoint(mountPoint.GlobalTransform))); + new NavigationPoint(mountPoint.GlobalTransform))); _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.InteractionTask(entity)); } } diff --git a/utils/NavigationPoint.cs b/utils/NavigationPoint.cs new file mode 100644 index 0000000..43d1fcb --- /dev/null +++ b/utils/NavigationPoint.cs @@ -0,0 +1,70 @@ +using System; +using Godot; + +public class NavigationPoint +{ + [Flags] + public enum NavigationFlags + { + Position = 1, + Orientation = 2 + } + + public readonly NavigationFlags Flags; + public Quat WorldOrientation = Quat.Identity; + public float WorldAngle; + + public Vector3 WorldPosition = Vector3.Zero; + + 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(); + WorldAngle = Globals.CalcPlaneAngle(worldTransform); + + Flags = NavigationFlags.Position | NavigationFlags.Orientation; + } + + public bool IsReached(Transform worldTransform) + { + var goalReached = false; + var positionError = new Vector2(WorldPosition.x - worldTransform.origin.x, + WorldPosition.z - worldTransform.origin.z); + var positionErrorSquared = positionError.LengthSquared(); + worldTransform.basis.Quat(); + var orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation)); + var angleError = Mathf.Abs(Globals.CalcPlaneAngle(worldTransform) - WorldAngle); + + if (Flags.HasFlag(NavigationFlags.Position) + && Flags.HasFlag(NavigationFlags.Orientation) + && positionErrorSquared < Globals.EpsPositionSquared + && angleError < Globals.EpsRadians) + { + goalReached = true; + } + else if (Flags == NavigationFlags.Position && + positionErrorSquared < Globals.EpsPositionSquared) + { + goalReached = true; + } + else if (Flags == NavigationFlags.Orientation && + angleError < Globals.EpsRadians) + { + goalReached = true; + } + + return goalReached; + } +} \ No newline at end of file