Somewhat improved GroundMotionComponent. Still buggy, though.
parent
aaa286bf77
commit
d918da6fd6
|
@ -11,35 +11,58 @@ public class GroundMotionComponent : Component
|
|||
public float Accel = 50;
|
||||
public float Damping = 0.2f;
|
||||
public float MaxSpeed = 8;
|
||||
|
||||
|
||||
public float RotationSpeedRadPerSecond = 270 * Mathf.Pi / 180;
|
||||
|
||||
public Vector3 DirectionToTarget = Vector3.Zero;
|
||||
public float DistanceToTarget = 0;
|
||||
public float TargetAngle = 0;
|
||||
public float TargetDeltaAngle = 0;
|
||||
|
||||
|
||||
private void CalcAndApplyOrientation(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation)
|
||||
{
|
||||
float deltaAngleAbsolute = Mathf.Abs(TargetDeltaAngle);
|
||||
if (deltaAngleAbsolute > 0.001)
|
||||
{
|
||||
Transform entityTransform = entity.Transform;
|
||||
if (RotationSpeedRadPerSecond * delta + 0.001 >= deltaAngleAbsolute)
|
||||
{
|
||||
GD.Print("Target Angle " + TargetAngle + " reached! Current Angle: " + entity.PlaneAngle);
|
||||
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 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 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 (targetDistance < 0.01 || planeOrientation.Dot(planeTargetDirection) < 0.9)
|
||||
if (DistanceToTarget < 0.01)
|
||||
{
|
||||
planeVelocity = Vector2.Zero;
|
||||
} else {
|
||||
} else if (TargetDeltaAngle == 0.0) {
|
||||
planeVelocity = planeVelocity.Length() * planeTargetDirection + planeTargetDirection * Accel * delta;
|
||||
// GD.Print(" accel: speed: " + planeVelocity.Length() + " vel dir: " + planeVelocity.Normalized());
|
||||
|
||||
float projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
|
||||
// GD.Print(" Projected step: " + projectedStep + " Speed: " + planeVelocity.Length() + " delta: " + delta);
|
||||
if (projectedStep > targetDistance)
|
||||
if (projectedStep > DistanceToTarget)
|
||||
{
|
||||
planeVelocity *= targetDistance / projectedStep;
|
||||
planeVelocity *= DistanceToTarget / projectedStep;
|
||||
projectedStep = planeTargetDirection.Dot(planeVelocity * delta);
|
||||
// GD.Print(" corr speed: " + planeVelocity.Length() + " step: " + projectedStep);
|
||||
}
|
||||
|
@ -50,6 +73,10 @@ public class GroundMotionComponent : Component
|
|||
planeVelocity *= MaxSpeed / planeSpeed;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
planeVelocity = Vector2.Zero;
|
||||
}
|
||||
|
||||
entity.Velocity = new Vector3(planeVelocity.x, entity.Velocity.y, planeVelocity.y);
|
||||
}
|
||||
|
@ -88,9 +115,30 @@ public class GroundMotionComponent : Component
|
|||
|
||||
public void PhysicsProcess(float delta, Entity entity, Vector3 targetPosition, Quat targetOrientation, TileWorld tileWorld)
|
||||
{
|
||||
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.CalcShortestPlaneRotationToTarget(targetPosition);
|
||||
}
|
||||
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);
|
||||
// CalcVerticalVelocity(delta, entity, tileWorld);
|
||||
|
||||
entity.Velocity = entity.MoveAndSlide(entity.Velocity);
|
||||
|
||||
|
@ -98,38 +146,4 @@ public class GroundMotionComponent : Component
|
|||
//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);
|
||||
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);
|
||||
entityRotation = entityRotation.Slerp(targetOrientation, slerpWeight).Normalized();
|
||||
entityTransform.basis = new Basis(entityRotation);
|
||||
}
|
||||
|
||||
entity.Transform = entityTransform;
|
||||
}
|
||||
}
|
||||
}
|
|
@ -79,6 +79,7 @@ public class NavigationComponent : Spatial
|
|||
|
||||
private NavigationPoint _currentGoal;
|
||||
private Vector3 _currentGoalPositionWorld = Vector3.Zero;
|
||||
private float _currentGoalAngleWorld = 0;
|
||||
private Quat _currentGoalOrientationWorld = Quat.Identity;
|
||||
|
||||
private List<NavigationPoint> _planningPathWorldNavigationPoints = new List<NavigationPoint>();
|
||||
|
@ -400,7 +401,7 @@ public class NavigationComponent : Spatial
|
|||
|
||||
_currentGoal = _pathWorldNavigationPoints[0];
|
||||
_currentGoalPositionWorld = _pathWorldNavigationPoints[0].WorldPosition;
|
||||
_currentGoalOrientationWorld = _pathWorldNavigationPoints[0].WorldOrientation;
|
||||
//_currentGoalOrientationWorld = Vector3.Right.SignedAngleTo(_pathWorldNavigationPoints[0].WorldOrientation);
|
||||
|
||||
// GD.Print("Navigation Goal: pos " + _currentGoal.WorldPosition + " " + " rot: " + _currentGoal.WorldOrientation +
|
||||
// " flags: " + _currentGoal.Flags + " path length: " +
|
||||
|
|
|
@ -6,6 +6,23 @@ public class Entity : KinematicBody
|
|||
public Vector3 Velocity { get; set; } = Vector3.Zero;
|
||||
public float RotationalVelocity { get; set; } = 0;
|
||||
|
||||
/** Defines the angle in plane coordinates, 0 => pointing to the right/east, pi/2 pointing up/north, range [-pi,pi]. */
|
||||
public float PlaneAngle
|
||||
{
|
||||
get => GlobalTransform.basis.x.SignedAngleTo(Vector3.Right.Rotated(Vector3.Up, Mathf.Pi * 0.5f), -Vector3.Up);
|
||||
set => GlobalTransform = new Transform(new Basis(Vector3.Up, value + Mathf.Pi * 0.5f), GlobalTranslation);
|
||||
}
|
||||
|
||||
public float CalcShortestPlaneRotationToTarget(Vector3 globalTargetPosition)
|
||||
{
|
||||
float angleToTarget = Vector3.Right.SignedAngleTo(globalTargetPosition - GlobalTranslation, Vector3.Up);
|
||||
float currentAngle = PlaneAngle;
|
||||
|
||||
float delta = angleToTarget - currentAngle;
|
||||
delta += (delta > Mathf.Pi) ? -Mathf.Pi * 2 : (delta < -Mathf.Pi) ? Mathf.Pi * 2 : 0;
|
||||
return delta;
|
||||
}
|
||||
|
||||
public override void _Ready()
|
||||
{
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue