From 0160e72eeca416148758b87c878b0e80222ee901 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Wed, 1 Nov 2023 12:33:04 +0100 Subject: [PATCH] Fixed angle stuttering when reaching target. --- components/GroundMotionComponent.cs | 5 ++--- components/NavigationComponent.cs | 4 +++- entities/Entity.cs | 5 +++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/components/GroundMotionComponent.cs b/components/GroundMotionComponent.cs index d81901a..916be89 100644 --- a/components/GroundMotionComponent.cs +++ b/components/GroundMotionComponent.cs @@ -24,10 +24,9 @@ public class GroundMotionComponent : Component float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle); if (deltaAngleAbsolute > 0.001) { - Transform entityTransform = entity.Transform; if (RotationSpeedRadPerSecond * delta + 0.001 >= deltaAngleAbsolute) { - GD.Print("Target Angle " + TargetAngle + " reached! Current Angle: " + entity.PlaneAngle); + GD.Print("Target Angle " + TargetAngle + " reached! Current Angle: " + entity.PlaneAngle + " TargetDeltaAngle = " + TargetDeltaAngle); entity.PlaneAngle = TargetAngle; TargetDeltaAngle = 0; } @@ -123,7 +122,7 @@ public class GroundMotionComponent : Component { DirectionToTarget = DirectionToTarget.Normalized(); TargetAngle = Vector3.Right.SignedAngleTo(DirectionToTarget, Vector3.Up); - TargetDeltaAngle = entity.CalcShortestPlaneRotationToTarget(targetPosition); + TargetDeltaAngle = entity.CalcShortestPlaneRotationToTargetDirection(DirectionToTarget); } else { diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs index 0465ca1..56f487c 100644 --- a/components/NavigationComponent.cs +++ b/components/NavigationComponent.cs @@ -46,7 +46,9 @@ public class NavigationComponent : Spatial public bool IsReached(Transform worldTransform) { bool goalReached = false; - float positionErrorSquared = (worldTransform.origin - WorldPosition).LengthSquared(); + Vector2 positionError = new Vector2(WorldPosition.x - worldTransform.origin.x, + WorldPosition.z - worldTransform.origin.z); + float positionErrorSquared = positionError.LengthSquared(); worldTransform.basis.Quat(); float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation)); diff --git a/entities/Entity.cs b/entities/Entity.cs index b10dd7b..e922066 100644 --- a/entities/Entity.cs +++ b/entities/Entity.cs @@ -13,12 +13,13 @@ public class Entity : KinematicBody set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation); } - public float CalcShortestPlaneRotationToTarget(Vector3 globalTargetPosition) + public float CalcShortestPlaneRotationToTargetDirection(Vector3 globalTargetDirection) { - float angleToTarget = Vector3.Right.SignedAngleTo(globalTargetPosition - GlobalTranslation, Vector3.Up); + float angleToTarget = Vector3.Right.SignedAngleTo(globalTargetDirection, Vector3.Up); float currentAngle = PlaneAngle; float delta = angleToTarget - currentAngle; + delta += (delta > Mathf.Pi) ? -Mathf.Pi * 2 : (delta < -Mathf.Pi) ? Mathf.Pi * 2 : 0; return delta; }