Fixed angle stuttering when reaching target.
parent
a0a3fea598
commit
0160e72eec
|
@ -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
|
||||||
{
|
{
|
||||||
|
|
|
@ -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));
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue