martin
/
GodotComponentTest
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
Globals.cs
View File

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

24
components/GroundMotionComponent.cs
View File

@ -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);
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, targetAngle);
CalcAndApplyOrientation(delta, entity, targetPosition, targetOrientation);
CalcPlaneVelocity(delta, entity, targetPosition);
// CalcVerticalVelocity(delta, entity, tileWorld);

83
components/NavigationComponent.cs
View File

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

4
components/TaskQueueComponent.cs
View File

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

2
entities/Entity.cs
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]. */
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);
}

8
entities/Player.cs
View File

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

4
scenes/Game.cs
View File

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

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