GodotComponentTest/components/MovableComponent.cs

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);
}
}
}