public static List <MoveAction> ReconstructInversePath(GoalRegion goal, Dictionary <MoveState, MoveAction> cameFrom, MoveAction currentNode) { var toReturn = new List <MoveAction>() { currentNode }; while (true) { if (!cameFrom.ContainsKey(currentNode.DestinationState)) { break; } currentNode = cameFrom[currentNode.DestinationState]; toReturn.Add(currentNode); /* * for (int frames = 0; frames < 6; frames++) * { * var sourceColor = goal.IsInGoalRegion(currentNode.SourceVoxel) ? Color.Green : Color.Red; * Drawer3D.DrawLine(currentNode.SourceVoxel.WorldPosition + Vector3.One * 0.5f, * currentNode.DestinationVoxel.WorldPosition + Vector3.One * 0.5f, Color.Red, 0.1f); * Drawer3D.DrawBox(currentNode.SourceVoxel.GetBoundingBox(), sourceColor, 0.1f, true); * Drawer3D.DrawBox(currentNode.DestinationVoxel.GetBoundingBox(), Color.Yellow, 0.1f, true); * foreach (var pair in cameFrom) * { * var color = Color.White; * if (goal.IsInGoalRegion(pair.Value.SourceVoxel)) * color = Color.Green; * Drawer3D.DrawLine(pair.Value.SourceVoxel.WorldPosition + Vector3.One * 0.5f, * pair.Value.DestinationVoxel.WorldPosition + Vector3.One * 0.5f, color, 0.05f); * * } * System.Threading.Thread.Sleep(16); * } */ if (goal.IsInGoalRegion(currentNode.DestinationState.Voxel)) { break; } } return(toReturn); }
// Find a path from the start to the goal by computing an inverse path from goal to the start. Should only be used // if the forward path fails. private static PlanResult InversePath(CreatureMovement mover, VoxelHandle startVoxel, GoalRegion goal, ChunkManager chunks, int maxExpansions, ref List <MoveAction> toReturn, float weight, Func <bool> continueFunc) { // Create a local clone of the octree, using only the objects belonging to the player. var octree = new OctTreeNode(mover.Creature.World.ChunkManager.Bounds.Min, mover.Creature.World.ChunkManager.Bounds.Max); List <Body> playerObjects = new List <Body>(mover.Creature.World.PlayerFaction.OwnedObjects); foreach (var obj in playerObjects) { octree.Add(obj, obj.GetBoundingBox()); } MoveState start = new MoveState() { Voxel = startVoxel }; var startTime = DwarfTime.Tick(); if (mover.IsSessile) { return(new PlanResult() { Result = PlanResultCode.Invalid, Expansions = 0, TimeSeconds = 0 }); } if (!start.IsValid) { return(new PlanResult() { Result = PlanResultCode.Invalid, Expansions = 0, TimeSeconds = 0 }); } // Sometimes a goal may not even be achievable a.priori. If this is true, we know there can't be a path // which satisifies that goal. // It only makes sense to do inverse plans for goals that have an associated voxel. if (!goal.IsPossible() || !goal.GetVoxel().IsValid) { toReturn = null; return(new PlanResult() { Result = PlanResultCode.Invalid, Expansions = 0, TimeSeconds = 0 }); } if (goal.IsInGoalRegion(start.Voxel)) { toReturn = new List <MoveAction>(); return(new PlanResult() { Result = PlanResultCode.Success, Expansions = 0, TimeSeconds = 0 }); } // Voxels that have already been explored. var closedSet = new HashSet <MoveState>(); // Voxels which should be explored. var openSet = new HashSet <MoveState>(); // Dictionary of voxels to the optimal action that got the mover to that voxel. var cameFrom = new Dictionary <MoveState, MoveAction>(); // Optimal score of a voxel based on the path it took to get there. var gScore = new Dictionary <MoveState, float>(); // Expansion priority of voxels. var fScore = new PriorityQueue <MoveState>(); var goalVoxels = new List <VoxelHandle>(); goalVoxels.Add(goal.GetVoxel()); // Starting conditions of the search. foreach (var goalVoxel in VoxelHelpers.EnumerateCube(goal.GetVoxel().Coordinate) .Select(c => new VoxelHandle(start.Voxel.Chunk.Manager.ChunkData, c))) { if (!goalVoxel.IsValid) { continue; } var goalState = new MoveState() { Voxel = goalVoxel }; if (goal.IsInGoalRegion(goalVoxel)) { goalVoxels.Add(goalVoxel); openSet.Add(goalState); gScore[goalState] = 0.0f; fScore.Enqueue(goalState, gScore[goalState] + weight * (goalVoxel.WorldPosition - start.Voxel.WorldPosition).LengthSquared()); } } // Keep count of the number of expansions we've taken to get to the goal. int numExpansions = 0; // Loop until we've either checked every possible voxel, or we've exceeded the maximum number of // expansions. while (openSet.Count > 0 && numExpansions < maxExpansions) { if (numExpansions % 10 == 0 && !continueFunc()) { return new PlanResult { Result = PlanResultCode.Cancelled, Expansions = numExpansions, TimeSeconds = DwarfTime.Tock(startTime) } } ; // Check the next voxel to explore. var current = GetStateWithMinimumFScore(fScore); if (!current.IsValid) { continue; } //Drawer3D.DrawBox(current.GetBoundingBox(), Color.Blue, 0.1f); numExpansions++; // If we've reached the goal already, reconstruct the path from the start to the // goal. if (current.Equals(start)) { toReturn = ReconstructInversePath(goal, cameFrom, cameFrom[start]); return(new PlanResult() { Result = PlanResultCode.Success, Expansions = numExpansions, TimeSeconds = DwarfTime.Tock(startTime) }); } // We've already considered the voxel, so add it to the closed set. openSet.Remove(current); closedSet.Add(current); IEnumerable <MoveAction> neighbors = null; // Get the voxels that can be moved to from the current voxel. neighbors = mover.GetInverseMoveActions(current, octree); // Otherwise, consider all of the neighbors of the current voxel that can be moved to, // and determine how to add them to the list of expansions. foreach (MoveAction n in neighbors) { //Drawer3D.DrawBox(n.SourceVoxel.GetBoundingBox(), Color.Red, 0.1f); // If we've already explored that voxel, don't explore it again. if (closedSet.Contains(n.SourceState)) { continue; } // Otherwise, consider the case of moving to that neighbor. float tenativeGScore = gScore[current] + GetDistance(current.Voxel, n.SourceState.Voxel, n.MoveType, mover); // IF the neighbor can already be reached more efficiently, ignore it. if (openSet.Contains(n.SourceState) && gScore.ContainsKey(n.SourceState) && !(tenativeGScore < gScore[n.SourceState])) { continue; } // Otherwise, add it to the list of voxels for consideration. openSet.Add(n.SourceState); // Add an edge to the voxel from the current voxel. var cameAction = n; cameFrom[n.SourceState] = cameAction; // Update the expansion scores for the next voxel. gScore[n.SourceState] = tenativeGScore; fScore.Enqueue(n.SourceState, gScore[n.SourceState] + weight * ((n.SourceState.Voxel.WorldPosition - start.Voxel.WorldPosition).LengthSquared() + (!n.SourceState.VehicleState.IsRidingVehicle ? 100.0f : 0.0f))); } // If we've expanded too many voxels, just give up. if (numExpansions >= maxExpansions) { return(new PlanResult() { Result = PlanResultCode.MaxExpansionsReached, Expansions = numExpansions, TimeSeconds = DwarfTime.Tock(startTime) }); } } // Somehow we've reached this code without having found a path. Return false. toReturn = null; return(new PlanResult() { Result = PlanResultCode.NoSolution, Expansions = numExpansions, TimeSeconds = DwarfTime.Tock(startTime) }); }
/// <summary> /// Creates a path from the start voxel to some goal region, returning a list of movements that can /// take a mover from the start voxel to the goal region. /// </summary> /// <param name="mover">The agent that we want to find a path for.</param> /// <param name="startVoxel"></param> /// <param name="goal">Goal conditions that the agent is trying to satisfy.</param> /// <param name="chunks">The voxels that the agent is moving through</param> /// <param name="maxExpansions">Maximum number of voxels to consider before giving up.</param> /// <param name="toReturn">The path of movements that the mover should take to reach the goal.</param> /// <param name="weight"> /// A heuristic weight to apply to the planner. If 1.0, the path is garunteed to be optimal. Higher values /// usually result in faster plans that are suboptimal. /// </param> /// <param name="continueFunc"></param> /// <returns>True if a path could be found, or false otherwise.</returns> private static PlanResult Path(CreatureMovement mover, VoxelHandle startVoxel, GoalRegion goal, ChunkManager chunks, int maxExpansions, ref List <MoveAction> toReturn, float weight, Func <bool> continueFunc) { // Create a local clone of the octree, using only the objects belonging to the player. var octree = new OctTreeNode(mover.Creature.World.ChunkManager.Bounds.Min, mover.Creature.World.ChunkManager.Bounds.Max); List <Body> playerObjects = new List <Body>(mover.Creature.World.PlayerFaction.OwnedObjects); foreach (var obj in playerObjects) { octree.Add(obj, obj.GetBoundingBox()); } var start = new MoveState() { Voxel = startVoxel }; var startTime = DwarfTime.Tick(); if (mover.IsSessile) { return(new PlanResult() { Expansions = 0, Result = PlanResultCode.Invalid, TimeSeconds = DwarfTime.Tock(startTime) }); } // Sometimes a goal may not even be achievable a.priori. If this is true, we know there can't be a path // which satisifies that goal. if (!goal.IsPossible()) { toReturn = null; return(new PlanResult() { Expansions = 0, Result = PlanResultCode.Invalid, TimeSeconds = DwarfTime.Tock(startTime) }); } // Voxels that have already been explored. var closedSet = new HashSet <MoveState>(); // Voxels which should be explored. var openSet = new HashSet <MoveState> { start }; // Dictionary of voxels to the optimal action that got the mover to that voxel. var cameFrom = new Dictionary <MoveState, MoveAction>(); // Optimal score of a voxel based on the path it took to get there. var gScore = new Dictionary <MoveState, float>(); // Expansion priority of voxels. var fScore = new PriorityQueue <MoveState>(); // Starting conditions of the search. gScore[start] = 0.0f; fScore.Enqueue(start, gScore[start] + weight * goal.Heuristic(start.Voxel)); // Keep count of the number of expansions we've taken to get to the goal. int numExpansions = 0; // Check the voxels adjacent to the current voxel as a quick test of adjacency to the goal. //var manhattanNeighbors = new List<VoxelHandle>(6); //for (int i = 0; i < 6; i++) //{ // manhattanNeighbors.Add(new VoxelHandle()); //} // Loop until we've either checked every possible voxel, or we've exceeded the maximum number of // expansions. while (openSet.Count > 0 && numExpansions < maxExpansions) { if (numExpansions % 10 == 0 && !continueFunc()) { return new PlanResult { Result = PlanResultCode.Cancelled, Expansions = numExpansions, TimeSeconds = DwarfTime.Tock(startTime) } } ; // Check the next voxel to explore. var current = GetStateWithMinimumFScore(fScore); if (!current.IsValid) { // If there wasn't a voxel to explore, just try to expand from // the start again. numExpansions++; continue; } numExpansions++; // If we've reached the goal already, reconstruct the path from the start to the // goal. if (goal.IsInGoalRegion(current.Voxel) && cameFrom.ContainsKey(current)) { toReturn = ReconstructPath(cameFrom, cameFrom[current], start); return(new PlanResult() { Result = PlanResultCode.Success, Expansions = numExpansions, TimeSeconds = DwarfTime.Tock(startTime) }); } //Drawer3D.DrawLine(start.Voxel.GetBoundingBox().Center(), goal.GetVoxel().GetBoundingBox().Center(), Color.Red, 0.1f); //Drawer3D.DrawBox(current.Voxel.GetBoundingBox(), Color.Red, 0.1f, true); // We've already considered the voxel, so add it to the closed set. openSet.Remove(current); closedSet.Add(current); IEnumerable <MoveAction> neighbors = null; // Get the voxels that can be moved to from the current voxel. neighbors = mover.GetMoveActions(current, octree).ToList(); //currentChunk.GetNeighborsManhattan(current, manhattanNeighbors); var foundGoalAdjacent = neighbors.FirstOrDefault(n => !n.DestinationState.VehicleState.IsRidingVehicle && goal.IsInGoalRegion(n.DestinationState.Voxel)); // A quick test to see if we're already adjacent to the goal. If we are, assume // that we can just walk to it. if (foundGoalAdjacent.DestinationState.IsValid && foundGoalAdjacent.SourceState.IsValid) { if (cameFrom.ContainsKey(current)) { List <MoveAction> subPath = ReconstructPath(cameFrom, foundGoalAdjacent, start); toReturn = subPath; return(new PlanResult() { Result = PlanResultCode.Success, Expansions = numExpansions, TimeSeconds = DwarfTime.Tock(startTime) }); } } // Otherwise, consider all of the neighbors of the current voxel that can be moved to, // and determine how to add them to the list of expansions. foreach (MoveAction n in neighbors) { // If we've already explored that voxel, don't explore it again. if (closedSet.Contains(n.DestinationState)) { continue; } // Otherwise, consider the case of moving to that neighbor. float tenativeGScore = gScore[current] + GetDistance(current.Voxel, n.DestinationState.Voxel, n.MoveType, mover); // IF the neighbor can already be reached more efficiently, ignore it. if (openSet.Contains(n.DestinationState) && !(tenativeGScore < gScore[n.DestinationState])) { continue; } // Otherwise, add it to the list of voxels for consideration. openSet.Add(n.DestinationState); // Add an edge to the voxel from the current voxel. var cameAction = n; cameFrom[n.DestinationState] = cameAction; // Update the expansion scores for the next voxel. gScore[n.DestinationState] = tenativeGScore; fScore.Enqueue(n.DestinationState, gScore[n.DestinationState] + weight * (goal.Heuristic(n.DestinationState.Voxel) + (!n.DestinationState.VehicleState.IsRidingVehicle ? 100.0f : 0.0f))); } // If we've expanded too many voxels, just give up. if (numExpansions >= maxExpansions) { return(new PlanResult() { Expansions = numExpansions, Result = PlanResultCode.MaxExpansionsReached, TimeSeconds = DwarfTime.Tock(startTime) }); } } // Somehow we've reached this code without having found a path. Return false. toReturn = null; return(new PlanResult() { Expansions = numExpansions, Result = PlanResultCode.NoSolution, TimeSeconds = DwarfTime.Tock(startTime) }); }
/// <summary> /// Creates a path from the start voxel to some goal region, returning a list of movements that can /// take a mover from the start voxel to the goal region. /// </summary> /// <param name="mover">The agent that we want to find a path for.</param> /// <param name="start">The voxel that the agent starts in.</param> /// <param name="goal">Goal conditions that the agent is trying to satisfy.</param> /// <param name="chunks">The voxels that the agent is moving through</param> /// <param name="maxExpansions">Maximum number of voxels to consider before giving up.</param> /// <param name="toReturn">The path of movements that the mover should take to reach the goal.</param> /// <param name="weight"> /// A heuristic weight to apply to the planner. If 1.0, the path is garunteed to be optimal. Higher values /// usually result in faster plans that are suboptimal. /// </param> /// <returns>True if a path could be found, or false otherwise.</returns> private static bool Path(CreatureMovement mover, Voxel start, GoalRegion goal, ChunkManager chunks, int maxExpansions, ref List <Creature.MoveAction> toReturn, float weight) { // Sometimes a goal may not even be achievable a.priori. If this is true, we know there can't be a path // which satisifies that goal. if (!goal.IsPossible()) { toReturn = null; return(false); } // Voxels that have already been explored. var closedSet = new HashSet <Voxel>(); // Voxels which should be explored. var openSet = new HashSet <Voxel> { start }; // Dictionary of voxels to the optimal action that got the mover to that voxel. var cameFrom = new Dictionary <Voxel, Creature.MoveAction>(); // Optimal score of a voxel based on the path it took to get there. var gScore = new Dictionary <Voxel, float>(); // Expansion priority of voxels. var fScore = new PriorityQueue <Voxel>(); // Starting conditions of the search. gScore[start] = 0.0f; fScore.Enqueue(start, gScore[start] + weight * goal.Heuristic(start)); // Keep count of the number of expansions we've taken to get to the goal. int numExpansions = 0; // Check the voxels adjacent to the current voxel as a quick test of adjacency to the goal. var manhattanNeighbors = new List <Voxel>(6); for (int i = 0; i < 6; i++) { manhattanNeighbors.Add(new Voxel()); } // Loop until we've either checked every possible voxel, or we've exceeded the maximum number of // expansions. while (openSet.Count > 0 && numExpansions < maxExpansions) { // Check the next voxel to explore. Voxel current = GetVoxelWithMinimumFScore(fScore); if (current == null) { // If there wasn't a voxel to explore, just try to expand from // the start again. current = start; numExpansions++; } numExpansions++; // If we've reached the goal already, reconstruct the path from the start to the // goal. if (goal.IsInGoalRegion(current)) { // Assume that the last action in the path involves walking to the goal. var first = new Creature.MoveAction { Voxel = current, MoveType = Creature.MoveType.Walk }; toReturn = ReconstructPath(cameFrom, first); return(true); } // We've already considered the voxel, so add it to the closed set. openSet.Remove(current); closedSet.Add(current); VoxelChunk currentChunk = chunks.ChunkData.ChunkMap[current.ChunkID]; List <Creature.MoveAction> neighbors = null; // Get the voxels that can be moved to from the current voxel. neighbors = mover.GetMoveActions(current); currentChunk.GetNeighborsManhattan(current, manhattanNeighbors); // A quick test to see if we're already adjacent to the goal. If we are, assume // that we can just walk to it. if (manhattanNeighbors.Contains(goal.GetVoxel())) { var first = new Creature.MoveAction { Voxel = current, MoveType = Creature.MoveType.Walk }; var last = new Creature.MoveAction { Voxel = goal.GetVoxel(), MoveType = Creature.MoveType.Walk }; List <Creature.MoveAction> subPath = ReconstructPath(cameFrom, first); subPath.Add(last); toReturn = subPath; return(true); } // Otherwise, consider all of the neighbors of the current voxel that can be moved to, // and determine how to add them to the list of expansions. foreach (Creature.MoveAction n in neighbors) { // If we've already explored that voxel, don't explore it again. if (closedSet.Contains(n.Voxel)) { continue; } // Otherwise, consider the case of moving to that neighbor. float tenativeGScore = gScore[current] + GetDistance(current, n.Voxel, n.MoveType, mover); // IF the neighbor can already be reached more efficiently, ignore it. if (openSet.Contains(n.Voxel) && !(tenativeGScore < gScore[n.Voxel])) { continue; } // Otherwise, add it to the list of voxels for consideration. openSet.Add(n.Voxel); // Add an edge to the voxel from the current voxel. Creature.MoveAction cameAction = n; cameAction.Voxel = current; cameFrom[n.Voxel] = cameAction; // Update the expansion scores for the next voxel. gScore[n.Voxel] = tenativeGScore; fScore.Enqueue(n.Voxel, gScore[n.Voxel] + weight * goal.Heuristic(n.Voxel)); } // If we've expanded too many voxels, just give up. if (numExpansions >= maxExpansions) { return(false); } } // Somehow we've reached this code without having found a path. Return false. toReturn = null; return(false); }
public List <MoveAction> ComputeGreedyFallback(int maxsteps = 10, List <VoxelHandle> exploredVoxels = null) { List <MoveAction> toReturn = new List <MoveAction>(); GoalRegion goal = GetGoal(); var creatureVoxel = Agent.Physics.CurrentVoxel; if (goal.IsInGoalRegion(creatureVoxel)) { return(toReturn); } var currentVoxel = creatureVoxel; while (toReturn.Count < maxsteps) { var actions = Agent.Movement.GetMoveActions(new MoveState() { Voxel = currentVoxel }, Creature.World.OctTree); float minCost = float.MaxValue; var minAction = new MoveAction(); bool hasMinAction = false; foreach (var action in actions) { if (toReturn.Any(a => a.DestinationVoxel == action.DestinationVoxel && a.MoveType == action.MoveType)) { continue; } var vox = action.DestinationVoxel; float cost = goal.Heuristic(vox) * MathFunctions.Rand(1.0f, 1.1f) + Agent.Movement.Cost(action.MoveType); if (exploredVoxels != null && exploredVoxels.Contains(action.DestinationVoxel)) { cost *= 10; } if (cost < minCost) { minAction = action; minCost = cost; hasMinAction = true; } } if (hasMinAction) { MoveAction action = minAction; action.DestinationVoxel = currentVoxel; toReturn.Add(action); currentVoxel = minAction.DestinationVoxel; if (goal.IsInGoalRegion(minAction.DestinationVoxel)) { return(toReturn); } } else { return(toReturn); } } return(toReturn); }
private static bool Path(CreatureMovement mover, Voxel start, GoalRegion goal, ChunkManager chunks, int maxExpansions, ref List <Creature.MoveAction> toReturn, bool reverse) { VoxelChunk startChunk = chunks.ChunkData.ChunkMap[start.ChunkID]; VoxelChunk endChunk = chunks.ChunkData.ChunkMap[goal.GetVoxel().ChunkID]; if (startChunk.IsCompletelySurrounded(start) || endChunk.IsCompletelySurrounded(goal.GetVoxel())) { toReturn = null; return(false); } HashSet <Voxel> closedSet = new HashSet <Voxel>(); HashSet <Voxel> openSet = new HashSet <Voxel> { start }; Dictionary <Voxel, Creature.MoveAction> cameFrom = new Dictionary <Voxel, Creature.MoveAction>(); Dictionary <Voxel, float> gScore = new Dictionary <Voxel, float>(); PriorityQueue <Voxel> fScore = new PriorityQueue <Voxel>(); gScore[start] = 0.0f; fScore.Enqueue(start, gScore[start] + Heuristic(start, goal.GetVoxel())); int numExpansions = 0; List <Voxel> manhattanNeighbors = new List <Voxel>(6); for (int i = 0; i < 6; i++) { manhattanNeighbors.Add(new Voxel()); } while (openSet.Count > 0 && numExpansions < maxExpansions) { Voxel current = GetVoxelWithMinimumFScore(fScore, openSet); if (current == null) { current = start; numExpansions++; } numExpansions++; if (goal.IsInGoalRegion(current)) { Creature.MoveAction first = new Creature.MoveAction() { Voxel = current, MoveType = Creature.MoveType.Walk }; toReturn = ReconstructPath(cameFrom, first); return(true); } openSet.Remove(current); closedSet.Add(current); VoxelChunk currentChunk = chunks.ChunkData.ChunkMap[current.ChunkID]; List <Creature.MoveAction> neighbors = null; neighbors = mover.GetMoveActions(current); currentChunk.GetNeighborsManhattan(current, manhattanNeighbors); if (manhattanNeighbors.Contains(goal.GetVoxel())) { Creature.MoveAction first = new Creature.MoveAction() { Voxel = current, MoveType = Creature.MoveType.Walk }; Creature.MoveAction last = new Creature.MoveAction() { Voxel = goal.GetVoxel(), MoveType = Creature.MoveType.Walk }; List <Creature.MoveAction> subPath = ReconstructPath(cameFrom, first); subPath.Add(last); toReturn = subPath; return(true); } foreach (Creature.MoveAction n in neighbors) { if (closedSet.Contains(n.Voxel)) { continue; } float tenativeGScore = gScore[current] + GetDistance(current, n.Voxel, n.MoveType, chunks); if (openSet.Contains(n.Voxel) && !(tenativeGScore < gScore[n.Voxel])) { continue; } openSet.Add(n.Voxel); Creature.MoveAction cameAction = n; cameAction.Voxel = current; cameFrom[n.Voxel] = cameAction; gScore[n.Voxel] = tenativeGScore; fScore.Enqueue(n.Voxel, gScore[n.Voxel] + Heuristic(n.Voxel, goal.GetVoxel())); } if (numExpansions >= maxExpansions) { return(false); } } toReturn = null; return(false); }