102 lines
2.4 KiB
C#
102 lines
2.4 KiB
C#
using System.Collections.Generic;
|
|
using Godot;
|
|
|
|
public class MoveNode : ProgramNode
|
|
{
|
|
public Vector3 startPosition;
|
|
public Vector3I targetPosition;
|
|
public List<Vector3> pathPoints;
|
|
|
|
public MoveNode()
|
|
{
|
|
DisplayText = "Move";
|
|
TooltipText = "Moves the robot to the selected map coordinate. The path must be reachable from the robot's current position.";
|
|
}
|
|
|
|
public override NodeResult Execute(Robot robot, double delta)
|
|
{
|
|
Vector3I closestPosition = Pathfinding.GetClosestStartPoint(robot.Position);
|
|
if (pathPoints == null)
|
|
{
|
|
pathPoints = new List<Vector3>(Pathfinding.GetPath(closestPosition, targetPosition));
|
|
}
|
|
|
|
if (pathPoints.Count <= 0)
|
|
{
|
|
if ((closestPosition - targetPosition).Length() == 0)
|
|
{
|
|
lastExecutionMessage = "";
|
|
return NodeResult.SUCCESS;
|
|
}
|
|
|
|
lastExecutionMessage = "No path available";
|
|
return NodeResult.FAILURE;
|
|
}
|
|
|
|
return MoveAlongPath(robot, delta);
|
|
}
|
|
|
|
private NodeResult MoveAlongPath(Robot robot, double delta)
|
|
{
|
|
startPosition = robot.Position;
|
|
Vector3 target = pathPoints[0] - startPosition;
|
|
float distance = target.Length();
|
|
float movementSpeed = robot.GetMovementSpeed();
|
|
|
|
if (distance < 0.1f * Mathf.Sqrt(movementSpeed))
|
|
{
|
|
return FinishCurrentStep(robot);
|
|
}
|
|
|
|
Vector3 direction = target / distance;
|
|
RotateRobot(robot, direction);
|
|
robot.GlobalPosition += direction * (float)delta * movementSpeed;
|
|
|
|
return NodeResult.RUNNING;
|
|
}
|
|
|
|
private NodeResult FinishCurrentStep(Robot robot)
|
|
{
|
|
robot.Position = pathPoints[0];
|
|
VisitCurrentTile(robot);
|
|
pathPoints.RemoveAt(0);
|
|
lastExecutionMessage = "";
|
|
|
|
if (pathPoints.Count > 0) return NodeResult.RUNNING;
|
|
|
|
pathPoints = null;
|
|
return NodeResult.SUCCESS;
|
|
}
|
|
|
|
private void VisitCurrentTile(Robot robot)
|
|
{
|
|
Vector3I mapIndex = Pathfinding.GetClosestStartPoint(robot.Position);
|
|
Tile tile = GameData.map[mapIndex.Y].tiles[mapIndex.X, mapIndex.Z];
|
|
if (!tile.wasVisited)
|
|
{
|
|
tile.VisitTile();
|
|
}
|
|
}
|
|
|
|
private void RotateRobot(Robot robot, Vector3 direction)
|
|
{
|
|
Vector3 lookDirection = new Vector3(direction.X, 0, direction.Z);
|
|
if (lookDirection.Length() <= 0.1f) return;
|
|
|
|
robot.LookAt(robot.GlobalPosition + lookDirection, Vector3.Up);
|
|
}
|
|
|
|
public override ProgramNode Duplicate()
|
|
{
|
|
return new MoveNode
|
|
{
|
|
targetPosition = targetPosition
|
|
};
|
|
}
|
|
|
|
public override string Save()
|
|
{
|
|
return $"Name: {DisplayText}, Position: ({targetPosition.X}|{targetPosition.Y}|{targetPosition.Z})";
|
|
}
|
|
}
|