GodotComponentTest/components/GroundMotionComponent.cs

140 lines
5.3 KiB
C#

using Godot;
/// <summary>
/// </summary>
public class GroundMotionComponent : Component
{
public float Accel = 50;
public float Damping = 0.2f;
public Vector3 DirectionToTarget = Vector3.Zero;
public float DistanceToTarget;
public float MaxSpeed = 8;
public float RotationSpeedRadPerSecond = 270 * Mathf.Pi / 180;
public float TargetAngle;
public float TargetDeltaAngle;
private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
{
float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle);
if (deltaAngleAbsolute > 0.001)
{
if (RotationSpeedRadPerSecond * delta >= deltaAngleAbsolute)
{
entity.PlaneAngle = TargetAngle;
TargetDeltaAngle = 0;
}
else
{
entity.PlaneAngle += Mathf.Sign(TargetDeltaAngle) * RotationSpeedRadPerSecond * delta;
}
}
else
{
TargetDeltaAngle = 0;
}
}
private void CalcPlaneVelocity(float delta, Entity entity, Vector3 targetPosition)
{
Vector2 planeTargetDirection = new Vector2(DirectionToTarget.x, DirectionToTarget.z);
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 (DistanceToTarget < 0.01)
{
planeVelocity = Vector2.Zero;
}
else if (TargetDeltaAngle == 0.0)
{
planeVelocity = planeVelocity.Length() * planeTargetDirection + planeTargetDirection * Accel * delta;
// GD.Print(" accel: speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
var projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
// GD.Print(" Projected step: " + projectedStep + " Speed: " + planeVelocity.Length() + " delta: " + delta);
if (projectedStep > DistanceToTarget)
{
planeVelocity *= DistanceToTarget / projectedStep;
projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
// GD.Print(" corr speed: " + planeVelocity.Length() + " step: " + projectedStep);
}
var planeSpeed = planeVelocity.Length();
if (planeSpeed > MaxSpeed) planeVelocity *= MaxSpeed / planeSpeed;
}
else
{
planeVelocity = Vector2.Zero;
}
entity.Velocity = new Vector3(planeVelocity.x, entity.Velocity.y, planeVelocity.y);
}
private void CalcVerticalVelocity(float delta, Entity entity, TileWorld tileWorld)
{
Vector3 entityVelocity = entity.Velocity;
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;
}
}
entityVelocity.y -= 9.81f * delta;
entity.Velocity = entityVelocity;
}
public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation,
World world)
{
DirectionToTarget = targetPosition - entity.GlobalTranslation;
DirectionToTarget.y = 0;
DistanceToTarget = DirectionToTarget.Length();
if (DistanceToTarget >= Globals.EpsPosition)
{
DirectionToTarget = DirectionToTarget.Normalized();
TargetAngle = Vector3.Right.SignedAngleTo(DirectionToTarget, Vector3.Up);
TargetDeltaAngle = entity.CalcShortestPlaneRotationToTargetDirection(DirectionToTarget);
}
else
{
GD.Print("Target Position reached!");
DirectionToTarget = Vector3.Right;
TargetAngle = entity.PlaneAngle;
TargetDeltaAngle = 0;
entity.GlobalTranslation = targetPosition;
entity.Velocity = new Vector3(0, entity.Velocity.y, 0);
return;
}
CalcAndApplyOrientation(delta, entity, targetPosition, targetOrientation);
CalcPlaneVelocity(delta, entity, targetPosition);
// CalcVerticalVelocity(delta, entity, tileWorld);
entity.Velocity = entity.MoveAndSlide(entity.Velocity);
//GD.Print("Pre : speed: " + prePhysicsVelocity.Length() + " Velocity: " + prePhysicsVelocity);
//GD.Print("Post: speed: " + entity.Velocity.Length() + " Velocity: " + entity.Velocity);
}
}