Navigation with plange angle targets now functional again.
parent
c59d92618b
commit
b968f9b3b2
|
@ -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);
|
||||
}
|
||||
}
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
|
@ -5,7 +5,7 @@ public class TaskQueueComponent : Component
|
|||
{
|
||||
[Signal]
|
||||
private delegate void StartInteraction(Entity entity, Entity targetEntity);
|
||||
|
||||
|
||||
public Queue<Task> Queue;
|
||||
|
||||
public TaskQueueComponent()
|
||||
|
@ -51,10 +51,10 @@ public class TaskQueueComponent : Component
|
|||
/// </summary>
|
||||
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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue