Somewhat improved GroundMotionComponent. Still buggy, though.

components/GroundMotionComponent.cs

@@ -13,33 +13,56 @@ public class GroundMotionComponent : Component
     public float MaxSpeed = 8;
     
     public float RotationSpeedRadPerSecond = 270 * Mathf.Pi / 180;
+    public Vector3 DirectionToTarget = Vector3.Zero;
+    public float DistanceToTarget = 0;
+    public float TargetAngle = 0;
+    public float TargetDeltaAngle = 0;
     
     
+    private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
+    {
+        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);
+                entity.PlaneAngle = TargetAngle;
+                TargetDeltaAngle = 0;
+            }
+            else
+            {
+                entity.PlaneAngle += Mathf.Sign(TargetDeltaAngle) * RotationSpeedRadPerSecond * delta;
+            }
+        }
+        else
+        {
+            TargetDeltaAngle = 0;
+        }
+    }
+    
     private void CalcPlaneVelocity(float delta, Entity entity, Vector3 targetPosition)
     {
-        Vector2 planeTargetVector = new Vector2(targetPosition.x - entity.GlobalTranslation.x,
+        Vector2 planeTargetDirection = new Vector2(DirectionToTarget.x, DirectionToTarget.z);
-            targetPosition.z - entity.GlobalTranslation.z);
-        float targetDistance = planeTargetVector.Length();
-        Vector2 planeTargetDirection = planeTargetVector / targetDistance;
-        Vector2 planeOrientation = new Vector2(entity.GlobalTransform.basis.z[0], entity.GlobalTransform.basis.z[2]);
         
         Vector2 planeVelocity = new Vector2(entity.Velocity.x, entity.Velocity.z);
         // GD.Print("-- Step: distance: " + targetDistance + " dir: " + planeTargetDirection + " speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
         planeVelocity -= planeVelocity * Damping;
         // GD.Print("  damp : speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
         
-        if (targetDistance < 0.01 || planeOrientation.Dot(planeTargetDirection) < 0.9)
+        if (DistanceToTarget < 0.01)
         {
             planeVelocity = Vector2.Zero;
-        } else {
+        } else if (TargetDeltaAngle == 0.0) {
             planeVelocity = planeVelocity.Length() * planeTargetDirection + planeTargetDirection * Accel * delta;
             // GD.Print("  accel: speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());            
 
             float projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
             // GD.Print("  Projected step: " + projectedStep + " Speed: " + planeVelocity.Length() + " delta: " + delta);
-            if (projectedStep > targetDistance)
+            if (projectedStep > DistanceToTarget)
             {
-                planeVelocity *= targetDistance / projectedStep;
+                planeVelocity *= DistanceToTarget / projectedStep;
                 projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
                 // GD.Print("  corr speed: " + planeVelocity.Length() + " step: " + projectedStep);                
             }
@@ -50,6 +73,10 @@ public class GroundMotionComponent : Component
                 planeVelocity *= MaxSpeed / planeSpeed;
             }
         }
+        else
+        {
+            planeVelocity = Vector2.Zero;
+        }
 
         entity.Velocity = new Vector3(planeVelocity.x, entity.Velocity.y, planeVelocity.y);
     }
@@ -88,9 +115,30 @@ public class GroundMotionComponent : Component
     
     public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation, TileWorld tileWorld)
     {
+        DirectionToTarget = (targetPosition - entity.GlobalTranslation);
+        DirectionToTarget.y = 0;
+        DistanceToTarget = DirectionToTarget.Length();
+
+        if (DistanceToTarget >= Globals.EpsPosition)
+        {
+            DirectionToTarget = DirectionToTarget.Normalized();
+            TargetAngle = Vector3.Right.SignedAngleTo(DirectionToTarget, Vector3.Up);
+            TargetDeltaAngle = entity.CalcShortestPlaneRotationToTarget(targetPosition);            
+        }
+        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;
+        }
+        
         CalcAndApplyOrientation(delta, entity, targetPosition, targetOrientation);
         CalcPlaneVelocity(delta, entity, targetPosition);
-        CalcVerticalVelocity(delta, entity, tileWorld);
+//        CalcVerticalVelocity(delta, entity, tileWorld);
 
         entity.Velocity = entity.MoveAndSlide(entity.Velocity);
         
@@ -98,38 +146,4 @@ public class GroundMotionComponent : Component
         //GD.Print("Post: speed: " + entity.Velocity.Length() + " Velocity: " + entity.Velocity);
     }
 
-    private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
-    {
-        Vector3 direction_to_target = targetPosition - entity.GlobalTranslation;
-
-        if (direction_to_target.LengthSquared() > Globals.EpsPositionSquared)
-        {
-            direction_to_target = direction_to_target.Normalized();
-            Vector3 localXAxis = Vector3.Up.Cross(direction_to_target);
-            targetOrientation = new Basis(localXAxis, Vector3.Up, direction_to_target).Quat().Normalized();
-        }
-        
-        float orientationErrorRadians = entity.GlobalTransform.basis.Quat().AngleTo(targetOrientation);
-        if (Mathf.Abs(orientationErrorRadians) > Mathf.Pi * 1.1)
-        {
-            GD.Print("moep");
-        }
-        if (orientationErrorRadians > 0)
-        {
-            Transform entityTransform = entity.Transform;
-            if (orientationErrorRadians < RotationSpeedRadPerSecond * delta)
-            {
-                entityTransform.basis = new Basis(targetOrientation);
-            }
-            else if (orientationErrorRadians > 0f)
-            {
-                Quat entityRotation = new Quat(entityTransform.basis);
-                float slerpWeight = RotationSpeedRadPerSecond / (orientationErrorRadians / delta);
-                entityRotation = entityRotation.Slerp(targetOrientation, slerpWeight).Normalized();
-                entityTransform.basis = new Basis(entityRotation);
-            }
-
-            entity.Transform = entityTransform;
-        }
-    }
 }

components/NavigationComponent.cs

@@ -79,6 +79,7 @@ public class NavigationComponent : Spatial
 
     private NavigationPoint _currentGoal;
     private Vector3 _currentGoalPositionWorld = Vector3.Zero;
+    private float _currentGoalAngleWorld = 0;
     private Quat _currentGoalOrientationWorld = Quat.Identity;
 
     private List<NavigationPoint> _planningPathWorldNavigationPoints = new List<NavigationPoint>();    
@@ -400,7 +401,7 @@ public class NavigationComponent : Spatial
 
         _currentGoal = _pathWorldNavigationPoints[0];
         _currentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
-        _currentGoalOrientationWorld = _pathWorldNavigationPoints[0].WorldOrientation;
+        //_currentGoalOrientationWorld = Vector3.Right.SignedAngleTo(_pathWorldNavigationPoints[0].WorldOrientation);
 
 //        GD.Print("Navigation Goal: pos " + _currentGoal.WorldPosition + " " + " rot: " + _currentGoal.WorldOrientation +
 //                 " flags: " + _currentGoal.Flags + " path length: " +

entities/Entity.cs

@@ -6,6 +6,23 @@ public class Entity : KinematicBody
     public Vector3 Velocity { get; set; } = Vector3.Zero;
     public float RotationalVelocity { get; set; } = 0;
 
+    /** 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);
+        set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation);
+    }
+   
+    public float CalcShortestPlaneRotationToTarget(Vector3 globalTargetPosition)
+    {
+        float angleToTarget = Vector3.Right.SignedAngleTo(globalTargetPosition - GlobalTranslation, 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;
+    }
+
     public override void _Ready()
     {
     }