Navigation with plange angle targets now functional again.

WorldChunkRefactoring
Martin Felis 2023-11-10 12:22:17 +01:00
parent c59d92618b
commit b968f9b3b2
8 changed files with 112 additions and 96 deletions

View File

@ -1,9 +1,14 @@
using System; using Godot;
public static class Globals public static class Globals
{ {
public const float EpsPosition = 0.01f; public const float EpsPosition = 0.01f;
public const float EpsPositionSquared = EpsPosition * EpsPosition; 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 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);
}
} }

View File

@ -14,7 +14,7 @@ public class GroundMotionComponent : Component
public float TargetAngle; public float TargetAngle;
public float TargetDeltaAngle; 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); float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle);
if (deltaAngleAbsolute > 0.001) if (deltaAngleAbsolute > 0.001)
@ -104,7 +104,7 @@ public class GroundMotionComponent : Component
entity.Velocity = entityVelocity; 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) World world)
{ {
DirectionToTarget = targetPosition - entity.GlobalTranslation; DirectionToTarget = targetPosition - entity.GlobalTranslation;
@ -119,16 +119,28 @@ public class GroundMotionComponent : Component
} }
else else
{ {
GD.Print("Target Position reached!");
DirectionToTarget = Vector3.Right; DirectionToTarget = Vector3.Right;
TargetAngle = entity.PlaneAngle;
TargetDeltaAngle = 0;
entity.GlobalTranslation = targetPosition; entity.GlobalTranslation = targetPosition;
entity.Velocity = new Vector3(0, entity.Velocity.y, 0); 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); CalcPlaneVelocity(delta, entity, targetPosition);
// CalcVerticalVelocity(delta, entity, tileWorld); // CalcVerticalVelocity(delta, entity, tileWorld);

View File

@ -11,8 +11,7 @@ public class NavigationComponent : Spatial
{ {
public World World { set; get; } public World World { set; get; }
public Vector3 CurrentGoalPositionWorld { get; private set; } = Vector3.Zero; public Vector3 CurrentGoalPositionWorld { get; private set; } = Vector3.Zero;
public float CurrentGoalAngleWorld { get; } = 0; public float CurrentGoalAngleWorld { get; private set; }
public Quat CurrentGoalOrientationWorld { get; private set; } = Quat.Identity;
private NavigationPoint _currentGoal; private NavigationPoint _currentGoal;
private HexCell[] _path; private HexCell[] _path;
@ -342,11 +341,7 @@ public class NavigationComponent : Spatial
_currentGoal = _pathWorldNavigationPoints[0]; _currentGoal = _pathWorldNavigationPoints[0];
CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition; CurrentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
//_currentGoalOrientationWorld = Vector3.Right.SignedAngleTo(_pathWorldNavigationPoints[0].WorldOrientation); CurrentGoalAngleWorld = _pathWorldNavigationPoints[0].WorldAngle;
// GD.Print("Navigation Goal: pos " + _currentGoal.WorldPosition + " " + " rot: " + _currentGoal.WorldOrientation +
// " flags: " + _currentGoal.Flags + " path length: " +
// _pathWorldNavigationPoints.Count);
} }
private void ApplyExistingTransform(Transform worldTransform) private void ApplyExistingTransform(Transform worldTransform)
@ -357,7 +352,7 @@ public class NavigationComponent : Spatial
} }
else if (_currentGoal.Flags == NavigationPoint.NavigationFlags.Position) 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) if (_pathWorldNavigationPoints.Count == 0)
{ {
CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat(); CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
CurrentGoalPositionWorld = currentTransformWorld.origin; CurrentGoalPositionWorld = currentTransformWorld.origin;
return; return;
} }
@ -382,22 +377,18 @@ public class NavigationComponent : Spatial
} }
else else
{ {
CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat(); CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
} }
if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation)) if (_currentGoal.Flags.HasFlag(NavigationPoint.NavigationFlags.Orientation))
{ {
CurrentGoalOrientationWorld = _currentGoal.WorldOrientation; CurrentGoalAngleWorld = _currentGoal.WorldAngle;
} }
else 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)) if (_currentGoal.IsReached(currentTransformWorld))
{ {
_pathWorldNavigationPoints.RemoveAt(0); _pathWorldNavigationPoints.RemoveAt(0);
@ -408,8 +399,8 @@ public class NavigationComponent : Spatial
if (_pathWorldNavigationPoints.Count == 0) if (_pathWorldNavigationPoints.Count == 0)
{ {
CurrentGoalOrientationWorld = currentTransformWorld.basis.Quat();
CurrentGoalPositionWorld = currentTransformWorld.origin; CurrentGoalPositionWorld = currentTransformWorld.origin;
CurrentGoalAngleWorld = Globals.CalcPlaneAngle(currentTransformWorld);
} }
} }
@ -477,62 +468,4 @@ public class NavigationComponent : Spatial
debugGeometry.End(); 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;
}
}
} }

View File

@ -51,10 +51,10 @@ public class TaskQueueComponent : Component
/// </summary> /// </summary>
public class NavigationTask : Task public class NavigationTask : Task
{ {
public NavigationComponent.NavigationPoint NavigationPoint; public NavigationPoint NavigationPoint;
public bool PlanningComplete = false; public bool PlanningComplete = false;
public NavigationTask(NavigationComponent.NavigationPoint navigationPoint) public NavigationTask(NavigationPoint navigationPoint)
{ {
NavigationPoint = navigationPoint; NavigationPoint = navigationPoint;
} }

View File

@ -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]. */ /** Defines the angle in plane coordinates, 0 => pointing to the right/east, pi/2 pointing up/north, range [-pi,pi]. */
public float PlaneAngle 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); set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation);
} }

View File

@ -98,10 +98,10 @@ public class Player : Entity, IInteractionInterface
NavigationComponent.UpdateCurrentGoal(GlobalTransform); NavigationComponent.UpdateCurrentGoal(GlobalTransform);
_groundMotion.PhysicsProcess(delta, this, NavigationComponent.CurrentGoalPositionWorld, _groundMotion.PhysicsProcess(delta, this, NavigationComponent.CurrentGoalPositionWorld,
NavigationComponent.CurrentGoalOrientationWorld, _worldInfo.World); NavigationComponent.CurrentGoalAngleWorld, _worldInfo.World);
if (NavigationComponent.IsGoalReached()) 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) if (NavigationComponent != null)
{ {
NavigationComponent.DebugDraw(this, _debugGeometry); NavigationComponent.DebugDraw(this, _debugGeometry);
return;
} }
} }
public void OnItemAttractorBodyEntered(Node node) public void OnItemAttractorBodyEntered(Node node)
{ {
GD.Print("Item entered " + node);
_attractedItemList.Add(node); _attractedItemList.Add(node);
} }
public void OnItemAttractorBodyExited(Node node) public void OnItemAttractorBodyExited(Node node)
{ {
GD.Print("Item exited " + node);
if (node is GoldBar) if (node is GoldBar)
{ {
GoldBar bar = (GoldBar)node; GoldBar bar = (GoldBar)node;

View File

@ -206,7 +206,7 @@ public class Game : Spatial
_player.TaskQueueComponent.Reset(); _player.TaskQueueComponent.Reset();
_player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask( _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask(
new NavigationComponent.NavigationPoint(tile.GlobalTranslation))); new NavigationPoint(tile.GlobalTranslation)));
} }
public void OnTileHovered(HexTile3D tile) public void OnTileHovered(HexTile3D tile)
@ -229,7 +229,7 @@ public class Game : Spatial
{ {
_player.TaskQueueComponent.Reset(); _player.TaskQueueComponent.Reset();
_player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask( _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.NavigationTask(
new NavigationComponent.NavigationPoint(mountPoint.GlobalTransform))); new NavigationPoint(mountPoint.GlobalTransform)));
_player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.InteractionTask(entity)); _player.TaskQueueComponent.Queue.Enqueue(new TaskQueueComponent.InteractionTask(entity));
} }
} }

70
utils/NavigationPoint.cs Normal file
View File

@ -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;
}
}