override public void InitState()
{
base.InitState();
{
State state = new PathfindingIdleState();
state.Init(this);
_stateMap[eStateType.IDLE] = state;
}
{
State state = new PathfindingState();
//State state = new PathfindingImmediateState();
state.Init(this);
_stateMap[eStateType.PATHFINDING] = state;
}
{
State state = new PathfindingMoveState();
state.Init(this);
_stateMap[eStateType.MOVE] = state;
}
_state = _stateMap[eStateType.IDLE];
}
public override void InitState()
{
base.InitState();
{
State state = new PlayerIdleState();
state.Init(this);
_stateMap[eStateType.IDLE] = state;
}
{
State state = new PathfindingMoveState();
state.Init(this);
_stateMap[eStateType.MOVE] = state;
}
{
State state = new PathfindingState();
state.Init(this);
_stateMap[eStateType.PATHFINDING] = state;
}
{
State state = new SelectTargetState();
state.Init(this);
_stateMap[eStateType.SELECT_TARGET] = state;
}
_state = _stateMap[eStateType.IDLE];
}
public override void OnFrame(Entity entity)
{
if (Vector3.Distance(entity.GetState <PositionState>().Position, targetPosition) < PositionTolerance)
{
pathfindingState = entity.GetState <PathfindingState>();
pathfindingState.ClearTarget();
ActionStatus = ActionStatus.Succeeded;
}
//TODO: Add timeout.
}
public override void OnFrame(Entity entity)
{
pathfindingState.SetNewTarget(targetWaypoint.GetState <PositionState>().Position);
if (Vector3.Distance(entity.GetState <PositionState>().Position, pathfindingState.GetTargetPosition().GetValueOrDefault()) < pathfindingState.GetStoppingDistance() + PositionTolerance)
{
pathfindingState = entity.GetState <PathfindingState>();
pathfindingState.ClearTarget();
ActionStatus = ActionStatus.Succeeded;
}
//TODO: Add timeout => Failure.
}
public override void OnStart(Entity entity)
{
pathfindingState = entity.GetState <PathfindingState>();
targetWaypoint = entity.GetState <ActionBlackboardState>().TargetEntity;
if (targetWaypoint != null)
{
pathfindingState.SetNewTarget(targetWaypoint.GetState <PositionState>().Position);
pathfindingState.SetStoppingDistance(StoppingDistance);
}
else
{
Debug.LogWarning("GotToMovingWaypointAction failed as there was no waypoint. This shouldn't happen");
ActionStatus = ActionStatus.Failed;
}
}
public Vector3[] FindPath(PathingTypes layerName, Vector3 from, IEnumerable <Vector3> to)
{
if (grids.ContainsKey(layerName))
{
PathfindingState state = new PathfindingState(grids[layerName]);
state.Start(WorldToLocal(from));
Vector2[] result = state.PathToNearest(to.Select(WorldToLocal));
if (result != null)
{
return(result.Select(input => LocalToWorld(input)).ToArray());
}
}
return(null);
}
public PathfindingState Step()
{
if (state == PathfindingState.Initialized)
{
state = PathfindingState.Finding;
var startingNodeWrapper = context.GetOrCreateNode(this.startingNode);
startingNodeWrapper.state = NodeState.Open;
context.openList.Enqueue(startingNodeWrapper);
return(state);
}
if (state != PathfindingState.Finding)
{
throw new Exception($"Unexpected state {state}");
}
NodeWrapper <T> first = context.openList.TryDequeue();
if (first != null)
{
// 把周围点 加入open
var neighbors = context.GetGraphData().CollectNeighbor(first.node);
foreach (T p in neighbors)
{
Collect(p, first);
}
first.state = NodeState.Closed;
if (first.node.Equals(this.endingNode))
{
state = PathfindingState.Found;
TraceBackForPath(first);
return(state);
}
}
else
{
state = PathfindingState.Failed;
TraceBackForPath(null);
return(state);
}
return(state);
}
public override void OnStart(Entity entity)
{
pathfindingState = entity.GetState <PathfindingState>();
var targetWaypoint = entity.GetState <ActionBlackboardState>().TargetEntity;
if (targetWaypoint != null)
{
targetPosition = targetWaypoint.GetState <PositionState>().Position;
targetRotation = targetWaypoint.GetState <RotationState>().Rotation.eulerAngles.y;
pathfindingState.SetNewTarget(targetPosition, targetRotation);
pathfindingState.SetStoppingDistance(0f);
}
else
{
Debug.LogError("GotToWaypointAction failed as there was no waypoint. This shouldn't happen!");
ActionStatus = ActionStatus.Failed;
}
}
public void NotifyNodeDataModified(T nodeData)
{
if (state == PathfindingState.Found || state == PathfindingState.Failed)
{
state = PathfindingState.Finding;
//NodeWrapperEx<T> endingNodeWrapper = context.GetOrCreateNode(this.endingNode);
//endingNodeWrapper.g = float.PositiveInfinity;
//endingNodeWrapper.rhs = float.PositiveInfinity;
//endingNodeWrapper.h = 0;
//endingNodeWrapper.key = CalculateKey(endingNodeWrapper);
//if (context.openList.Contains(endingNodeWrapper))
//{
// context.openList.Remove(endingNodeWrapper);
//}
}
NodeWrapperEx <T> nodeWrapper = this.context.GetOrCreateNode(nodeData);
nodeWrapper.rhs = CalculateRHS(nodeWrapper);
if (nodeWrapper.GetConsistency() == NodeConsistency.Overconsistent)
{
nodeWrapper.g = nodeWrapper.rhs;
}
else
{
nodeWrapper.g = float.PositiveInfinity;
UpdateNode(nodeWrapper);
}
var neighbors = context.GetGraphData().CollectNeighbor(nodeData).Select(n => context.GetOrCreateNode(n));
foreach (var nei in neighbors)
{
UpdateNode(nei);
}
}
public override void OnFrame(Entity entity)
{
if (!destinationReached && Vector3.Distance(entity.GetState <PositionState>().Position, pathfindingState.GetTargetPosition().GetValueOrDefault()) <= PositionTolerance)
{
pathfindingState = entity.GetState <PathfindingState>();
destinationReached = true;
}
if (destinationReached && !rotationStarted)
{
rotationStarted = true;
if (!pathfindingState.GetTargetRotation().HasValue)
{
RotationFinished();
}
else
{
pathfindingState.ClearPosition();
var rotationTime = GetRotationTime(entity.GameObject.transform.rotation.eulerAngles.y, pathfindingState.GetTargetRotation().Value);
entity.GameObject.transform.DORotate(new Vector3(0, pathfindingState.GetTargetRotation().Value, 0), rotationTime).OnComplete(RotationFinished);
}
}
//TODO: Add timeout => Failure.
}
public void Init()
{
this.state = PathfindingState.Initialized;
}
public void CleanUp()
{
context.Reset();
result.Clear();
state = PathfindingState.Uninitialized;
}
public AStar(IGraphData <T> graphData, Algorithms algorithm)
{
this.context = new PathfindingContext <T>(graphData, algorithm);
this.state = PathfindingState.Uninitialized;
}
public void OnEntityAdded(Entity entity)
{
playerPathfindingState = entity.GetState <PathfindingState>();
}
public override void OnStart(Entity entity)
{
pathfindingState = entity.GetState <PathfindingState>();
pathfindingState.SetNewTarget(targetPosition);
pathfindingState.SetStoppingDistance(stoppingDistance);
}
public record PathFindingTestCase(PathRequest PathRequest, PathfindingState ResultState, float?Distance);
public PathfindingState Step()
{
if (state == PathfindingState.Initialized)
{
state = PathfindingState.Finding;
NodeWrapperEx <T> startingNodeWrapper = context.GetOrCreateNode(this.startingNode);
startingNodeWrapper.g = float.PositiveInfinity;
startingNodeWrapper.rhs = float.PositiveInfinity;
startingNodeWrapper.h = 0;
startingNodeWrapper.key = CalculateKey(startingNodeWrapper);
NodeWrapperEx <T> endingNodeWrapper = context.GetOrCreateNode(this.endingNode);
endingNodeWrapper.g = float.PositiveInfinity;
endingNodeWrapper.rhs = 0;
endingNodeWrapper.h = 0;
endingNodeWrapper.key = CalculateKey(endingNodeWrapper);
context.openList.Enqueue(endingNodeWrapper);
return(state);
}
if (state != PathfindingState.Finding)
{
throw new Exception($"Unexpected state {state}");
}
NodeWrapperEx <T> first = context.openList.TryDequeue();
if (first == null)
{
// 寻路失败
state = PathfindingState.Failed;
TraceBackForPath(null);
return(state);
}
NodeWrapperEx <T> startingNodeWrapper0 = context.GetOrCreateNode(startingNode);
startingNodeWrapper0.key = CalculateKey(startingNodeWrapper0);
if (startingNodeWrapper0.GetConsistency() == NodeConsistency.Consistent &&
first.CompareTo(startingNodeWrapper0) >= 0)
{
// 终点局部一致
// 此时有一条有效路径
// state = PathfindingState.Found;
// TraceBackForPath(startingNodeWrapper0);
if (MoveAgent())
{
state = PathfindingState.Found;
}
return(state);
}
Vector2 keyOld = first.key;
Vector2 key = CalculateKey(first);
if (keyOld.x < key.x && keyOld.y < key.y)
{
first.key = key;
context.openList.Enqueue(first);
}
else
{
if (first.GetConsistency() == NodeConsistency.Overconsistent)
{
// 障碍被清除 或者cost变低
first.g = first.rhs;
}
else
{
first.g = float.PositiveInfinity;
UpdateNode(first);
}
var neighbors = context.GetGraphData().CollectNeighbor(first.node).Select(n => context.GetOrCreateNode(n));
foreach (NodeWrapperEx <T> nei in neighbors)
{
UpdateNode(nei);
}
}
return(state);
}
