69 lines
2.6 KiB
C#
69 lines
2.6 KiB
C#
using Godot;
|
|
|
|
public class MovableComponent : Component {
|
|
public Vector3 targetPosition = Vector3.Zero;
|
|
public Vector3 currentPosition = Vector3.Zero;
|
|
public Vector3 currentVelocity = Vector3.Zero;
|
|
|
|
public float targetAngle;
|
|
public float currentAngle;
|
|
public float currentAngularVelocity;
|
|
|
|
[Export] public float maxSpeed = 3;
|
|
|
|
private SpringDamper _positionSpringDamper;
|
|
private SpringDamper _angleSpringDamper;
|
|
|
|
[Signal]
|
|
private delegate void PositionUpdated(Vector3 newPosition);
|
|
|
|
[Signal]
|
|
private delegate void OrientationUpdated(float newAngle);
|
|
|
|
// Called when the node enters the scene tree for the first time.
|
|
public override void _Ready() {
|
|
_positionSpringDamper = new SpringDamper(4, 0.99f, 0.5f);
|
|
_angleSpringDamper = new SpringDamper(4, 0.99f, 0.5f);
|
|
}
|
|
|
|
// Called every frame. 'delta' is the elapsed time since the previous frame.
|
|
public override void _Process(float delta) {
|
|
Vector3 targetError = targetPosition - currentPosition;
|
|
|
|
if (targetError.LengthSquared() > 0.01) {
|
|
Vector3 targetDir = targetError.Normalized();
|
|
targetAngle = new Vector3(0, 0, 1).SignedAngleTo(targetDir, Vector3.Up);
|
|
|
|
if (targetAngle - currentAngle > Mathf.Pi) {
|
|
currentAngle += 2 * Mathf.Pi;
|
|
} else if (targetAngle - currentAngle < -Mathf.Pi) {
|
|
currentAngle -= 2 * Mathf.Pi;
|
|
}
|
|
}
|
|
|
|
if (Mathf.Abs(currentAngularVelocity) > 0.1 || Mathf.Abs(targetAngle - currentAngle) > 0.01) {
|
|
(float, float) springDamperResult =
|
|
_angleSpringDamper.Calc(currentAngle, currentAngularVelocity, targetAngle, delta);
|
|
|
|
currentAngle = springDamperResult.Item1;
|
|
currentAngularVelocity = springDamperResult.Item2;
|
|
|
|
EmitSignal("OrientationUpdated", currentAngle);
|
|
}
|
|
|
|
if (Mathf.Abs(currentAngularVelocity) < 5 && Mathf.Abs(targetAngle - currentAngle) < 0.3
|
|
&& (targetPosition - currentPosition).LengthSquared() > 0.01) {
|
|
(Vector3, Vector3) springDamperResult =
|
|
_positionSpringDamper.CalcClampedSpeedXZ(currentPosition, currentVelocity, targetPosition, delta,
|
|
maxSpeed);
|
|
|
|
currentPosition = springDamperResult.Item1;
|
|
currentVelocity = springDamperResult.Item2;
|
|
}
|
|
|
|
currentVelocity.y = currentVelocity.y - 9.81f * delta;
|
|
currentPosition.y = currentPosition.y + currentVelocity.y * delta;
|
|
|
|
EmitSignal("PositionUpdated", currentPosition);
|
|
}
|
|
} |