GodotComponentTest/components/GroundMotionComponent.cs

135 lines
5.6 KiB
C#
Raw Normal View History

using System;
2022-12-31 18:36:52 +01:00
using System.Numerics;
using Godot;
using Vector2 = Godot.Vector2;
using Vector3 = Godot.Vector3;
/// <summary>
/// </summary>
public class GroundMotionComponent : Component
{
2022-12-31 18:36:52 +01:00
public float Accel = 50;
public float Damping = 0.2f;
public float MaxSpeed = 8;
2022-12-31 18:36:52 +01:00
public float RotationSpeedRadPerSecond = 270 * Mathf.Pi / 180;
private void CalcPlaneVelocity(float delta, Entity entity, Vector3 targetPosition)
{
Vector2 planeTargetVector = new Vector2(targetPosition.x - entity.GlobalTranslation.x,
targetPosition.z - entity.GlobalTranslation.z);
float targetDistance = planeTargetVector.Length();
Vector2 planeTargetDirection = planeTargetVector / targetDistance;
Vector2 planeOrientation = new Vector2(entity.GlobalTransform.basis.z[0], entity.GlobalTransform.basis.z[2]);
Vector2 planeVelocity = new Vector2(entity.Velocity.x, entity.Velocity.z);
// GD.Print("-- Step: distance: " + targetDistance + " dir: " + planeTargetDirection + " speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
planeVelocity -= planeVelocity * Damping;
// GD.Print(" damp : speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
if (targetDistance < 0.01 || planeOrientation.Dot(planeTargetDirection) < 0.9)
{
planeVelocity = Vector2.Zero;
2022-12-31 18:36:52 +01:00
} else {
planeVelocity = planeVelocity.Length() * planeTargetDirection + planeTargetDirection * Accel * delta;
// GD.Print(" accel: speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
2022-12-31 18:36:52 +01:00
float projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
// GD.Print(" Projected step: " + projectedStep + " Speed: " + planeVelocity.Length() + " delta: " + delta);
if (projectedStep > targetDistance)
2022-12-31 18:36:52 +01:00
{
planeVelocity *= targetDistance / projectedStep;
projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
// GD.Print(" corr speed: " + planeVelocity.Length() + " step: " + projectedStep);
2022-12-31 18:36:52 +01:00
}
float planeSpeed = planeVelocity.Length();
if (planeSpeed > MaxSpeed)
2022-12-31 18:36:52 +01:00
{
planeVelocity *= MaxSpeed / planeSpeed;
2022-12-31 18:36:52 +01:00
}
}
entity.Velocity = new Vector3(planeVelocity.x, entity.Velocity.y, planeVelocity.y);
}
private void CalcVerticalVelocity(float delta, Entity entity, TileWorld tileWorld)
{
Vector3 entityVelocity = entity.Velocity;
2022-12-31 18:36:52 +01:00
Vector2 currentTile = tileWorld.WorldToOffsetCoords(entity.GlobalTransform.origin);
Vector2 nextTile = tileWorld.WorldToOffsetCoords(entity.GlobalTransform.origin + entityVelocity * delta);
if (nextTile != currentTile)
{
float currentHeight = tileWorld.GetHeightAtOffset(currentTile);
float nextHeight = tileWorld.GetHeightAtOffset(nextTile);
bool isOnFloor = entity.IsOnFloor();
if (nextHeight - entity.GlobalTransform.origin.y > 0.1)
{
GD.Print("Jump!");
entityVelocity.y = 10;
Transform entityTransform = entity.GlobalTransform;
entityTransform.origin.y = nextHeight;
entity.GlobalTransform = entityTransform;
}
}
2022-12-31 18:36:52 +01:00
entityVelocity.y -= 9.81f * delta;
entity.Velocity = entityVelocity;
}
public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation, TileWorld tileWorld)
{
CalcAndApplyOrientation(delta, entity, targetPosition, targetOrientation);
CalcPlaneVelocity(delta, entity, targetPosition);
CalcVerticalVelocity(delta, entity, tileWorld);
2022-12-31 18:36:52 +01:00
entity.Velocity = entity.MoveAndSlide(entity.Velocity);
2023-01-04 22:49:00 +01:00
//GD.Print("Pre : speed: " + prePhysicsVelocity.Length() + " Velocity: " + prePhysicsVelocity);
//GD.Print("Post: speed: " + entity.Velocity.Length() + " Velocity: " + entity.Velocity);
}
private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
{
Vector3 direction_to_target = targetPosition - entity.GlobalTranslation;
if (direction_to_target.LengthSquared() > Globals.EpsPositionSquared)
{
direction_to_target = direction_to_target.Normalized();
Vector3 localXAxis = Vector3.Up.Cross(direction_to_target);
targetOrientation = new Basis(localXAxis, Vector3.Up, direction_to_target).Quat().Normalized();
}
float orientationErrorRadians = entity.GlobalTransform.basis.Quat().AngleTo(targetOrientation);
2023-02-12 16:44:48 +01:00
if (Mathf.Abs(orientationErrorRadians) > Mathf.Pi * 1.1)
{
GD.Print("moep");
}
if (orientationErrorRadians > 0)
{
Transform entityTransform = entity.Transform;
if (orientationErrorRadians < RotationSpeedRadPerSecond * delta)
{
entityTransform.basis = new Basis(targetOrientation);
}
else if (orientationErrorRadians > 0f)
{
Quat entityRotation = new Quat(entityTransform.basis);
float slerpWeight = RotationSpeedRadPerSecond / (orientationErrorRadians / delta);
2023-05-01 18:37:35 +02:00
entityRotation = entityRotation.Slerp(targetOrientation, slerpWeight).Normalized();
entityTransform.basis = new Basis(entityRotation);
}
entity.Transform = entityTransform;
}
}
}