From d918da6fd6a9bbecda0a7b8d462c3d11159695e3 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Fri, 27 Oct 2023 23:00:54 +0200 Subject: [PATCH] Somewhat improved GroundMotionComponent. Still buggy, though. --- components/GroundMotionComponent.cs | 106 ++++++++++++++++------------ components/NavigationComponent.cs | 3 +- entities/Entity.cs | 17 +++++ 3 files changed, 79 insertions(+), 47 deletions(-) diff --git a/components/GroundMotionComponent.cs b/components/GroundMotionComponent.cs index 81dc0e7..d81901a 100644 --- a/components/GroundMotionComponent.cs +++ b/components/GroundMotionComponent.cs @@ -11,35 +11,58 @@ public class GroundMotionComponent : Component public float Accel = 50; public float Damping = 0.2f; 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, - 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 planeTargetDirection = new Vector2(DirectionToTarget.x, DirectionToTarget.z); 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; - } - } } \ No newline at end of file diff --git a/components/NavigationComponent.cs b/components/NavigationComponent.cs index add6a67..b29c205 100644 --- a/components/NavigationComponent.cs +++ b/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 _planningPathWorldNavigationPoints = new List(); @@ -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: " + diff --git a/entities/Entity.cs b/entities/Entity.cs index 69ea556..b10dd7b 100644 --- a/entities/Entity.cs +++ b/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() { }