80 lines
2.4 KiB
C#
80 lines
2.4 KiB
C#
using Godot;
|
|
using System;
|
|
|
|
public class MovableComponent : Node
|
|
{
|
|
public Vector3 targetPosition = Vector3.Zero;
|
|
public Vector3 currentPosition = Vector3.Zero;
|
|
public Vector3 currentVelocity = Vector3.Zero;
|
|
|
|
public float targetAngle = 0;
|
|
public float currentAngle = 0;
|
|
public float currentAngularVelocity = 0;
|
|
|
|
[Export] public float maxSpeed = 3;
|
|
|
|
private SpringDamper _positionSpringDamper;
|
|
private SpringDamper _angleSpringDamper;
|
|
|
|
[Signal]
|
|
delegate void PositionUpdated(Vector3 newPosition);
|
|
|
|
[Signal]
|
|
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 targetDir = Vector3.Zero;
|
|
Vector3 targetError = targetPosition - currentPosition;
|
|
|
|
if (targetError.LengthSquared() > 0.01)
|
|
{
|
|
targetDir = targetError.Normalized();
|
|
targetAngle = new Vector3(0, 0, 1).SignedAngleTo(targetDir, Vector3.Up);
|
|
|
|
if (currentAngle < Mathf.Pi && targetAngle > Mathf.Pi)
|
|
{
|
|
|
|
}
|
|
|
|
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)
|
|
{
|
|
var springDamperResult = _angleSpringDamper.Calc(currentAngle, currentAngularVelocity, targetAngle, delta);
|
|
|
|
currentAngle = springDamperResult.Item1;
|
|
currentAngularVelocity = springDamperResult.Item2;
|
|
|
|
EmitSignal("OrientationUpdated", this.currentAngle);
|
|
}
|
|
|
|
if (( Mathf.Abs(currentAngularVelocity) < 5 && Mathf.Abs(targetAngle - currentAngle) < 0.3)
|
|
&& (targetPosition - currentPosition).LengthSquared() > 0.01)
|
|
{
|
|
var springDamperResult =
|
|
_positionSpringDamper.CalcClampedSpeed(currentPosition, currentVelocity, targetPosition, delta, maxSpeed);
|
|
|
|
currentPosition = springDamperResult.Item1;
|
|
currentVelocity = springDamperResult.Item2;
|
|
|
|
EmitSignal("PositionUpdated", currentPosition);
|
|
}
|
|
}
|
|
} |