Fixed angle stuttering when reaching target.

WorldChunkRefactoring
Martin Felis 2023-11-01 12:33:04 +01:00
parent a0a3fea598
commit 0160e72eec
3 changed files with 8 additions and 6 deletions

View File

@ -24,10 +24,9 @@ public class GroundMotionComponent : Component
float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle); float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle);
if (deltaAngleAbsolute > 0.001) if (deltaAngleAbsolute > 0.001)
{ {
Transform entityTransform = entity.Transform;
if (RotationSpeedRadPerSecond * delta + 0.001 >= deltaAngleAbsolute) 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; entity.PlaneAngle = TargetAngle;
TargetDeltaAngle = 0; TargetDeltaAngle = 0;
} }
@ -123,7 +122,7 @@ public class GroundMotionComponent : Component
{ {
DirectionToTarget = DirectionToTarget.Normalized(); DirectionToTarget = DirectionToTarget.Normalized();
TargetAngle = Vector3.Right.SignedAngleTo(DirectionToTarget, Vector3.Up); TargetAngle = Vector3.Right.SignedAngleTo(DirectionToTarget, Vector3.Up);
TargetDeltaAngle = entity.CalcShortestPlaneRotationToTarget(targetPosition); TargetDeltaAngle = entity.CalcShortestPlaneRotationToTargetDirection(DirectionToTarget);
} }
else else
{ {

View File

@ -46,7 +46,9 @@ public class NavigationComponent : Spatial
public bool IsReached(Transform worldTransform) public bool IsReached(Transform worldTransform)
{ {
bool goalReached = false; 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(); worldTransform.basis.Quat();
float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation)); float orientationError = Mathf.Abs(worldTransform.basis.Quat().AngleTo(WorldOrientation));

View File

@ -13,12 +13,13 @@ public class Entity : KinematicBody
set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation); 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 currentAngle = PlaneAngle;
float delta = angleToTarget - currentAngle; float delta = angleToTarget - currentAngle;
delta += (delta > Mathf.Pi) ? -Mathf.Pi * 2 : (delta < -Mathf.Pi) ? Mathf.Pi * 2 : 0; delta += (delta > Mathf.Pi) ? -Mathf.Pi * 2 : (delta < -Mathf.Pi) ? Mathf.Pi * 2 : 0;
return delta; return delta;
} }