martin
/
GodotComponentTest
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Somewhat improved GroundMotionComponent. Still buggy, though.

WorldChunkRefactoring
Martin Felis 2023-10-27 23:00:54 +02:00
parent aaa286bf77
commit d918da6fd6
3 changed files with 79 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
components/GroundMotionComponent.cs
View File

@ -13,33 +13,56 @@ public class GroundMotionComponent : Component
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;
}
}
}

3
components/NavigationComponent.cs
View File

@ -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: " +

17
entities/Entity.cs
View File

@ -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()
{
}