public bool CollidesWithChunks(ChunkManager chunks, Vector3 pos, bool applyForce, bool allowInvisible, float size = 0.5f, float height = 2.0f) { var box = new BoundingBox(pos - new Vector3(size, height * 0.5f, size), pos + new Vector3(size, height * 0.5f, size)); bool gotCollision = false; foreach (var v in VoxelHelpers.EnumerateCube(GlobalVoxelCoordinate.FromVector3(pos)) .Select(n => new VoxelHandle(chunks, n))) { if (!v.IsValid) { continue; } if (v.IsEmpty) { continue; } if (!allowInvisible && !v.IsVisible) { continue; } var voxAABB = v.GetBoundingBox(); if (box.Intersects(voxAABB)) { gotCollision = true; if (applyForce) { Collide(v, box, voxAABB); } else { return(true); } } } return(gotCollision); }
// Inverts GetMoveActions. So, returns the list of move actions whose target is the current voxel. // Very, very slow. public IEnumerable <MoveAction> GetInverseMoveActions(MoveState currentstate, OctTreeNode OctTree) { if (Parent == null) { yield break; } var creature = Creature; if (creature == null) { yield break; } var current = currentstate.Voxel; if (Can(MoveType.Teleport)) { var teleportObjects = Parent.Faction.OwnedObjects.Where(obj => obj.Active && obj.Tags.Contains("Teleporter")); foreach (var obj in teleportObjects) { if ((obj.Position - current.WorldPosition).LengthSquared() > 2) { continue; } for (int dx = -TeleportDistance; dx <= TeleportDistance; dx++) { for (int dz = -TeleportDistance; dz <= TeleportDistance; dz++) { for (int dy = -TeleportDistance; dy <= TeleportDistance; dy++) { if (dx * dx + dy * dy + dz * dz > TeleportDistanceSquared) { continue; } VoxelHandle teleportNeighbor = new VoxelHandle(Parent.World.ChunkManager.ChunkData, current.Coordinate + new GlobalVoxelOffset(dx, dy, dz)); if (teleportNeighbor.IsValid && teleportNeighbor.IsEmpty && !VoxelHelpers.GetNeighbor(teleportNeighbor, new GlobalVoxelOffset(0, -1, 0)).IsEmpty) { yield return(new MoveAction() { InteractObject = obj, Diff = new Vector3(dx, dx, dz), SourceVoxel = teleportNeighbor, DestinationState = currentstate, MoveType = MoveType.Teleport }); } } } } } } foreach (var v in VoxelHelpers.EnumerateCube(current.Coordinate) .Select(n => new VoxelHandle(current.Chunk.Manager.ChunkData, n)) .Where(h => h.IsValid && h.IsEmpty)) { foreach (var a in GetMoveActions(new MoveState() { Voxel = v }, OctTree).Where(a => a.DestinationState == currentstate)) { yield return(a); } if (!Can(MoveType.RideVehicle)) { continue; } // Now that dwarfs can ride vehicles, the inverse of the move actions becomes extremely complicated. We must now // iterate through all rails intersecting every neighbor and see if we can find a connection from that rail to this one. // Further, we must iterate through the entire rail network and enumerate all possible directions in and out of that rail. // Yay! var bodies = new HashSet <Body>(); OctTree.EnumerateItems(v.GetBoundingBox(), bodies); var rails = bodies.OfType <Rail.RailEntity>().Where(r => r.Active); foreach (var rail in rails) { if (rail.GetContainingVoxel() != v) { continue; } /* * if (!DwarfGame.IsMainThread) * { * for (int i = 0; i < 1; i++) * { * Drawer3D.DrawBox(rail.GetBoundingBox(), Color.Purple, 0.1f, false); * System.Threading.Thread.Sleep(1); * } * } */ foreach (var neighborRail in rail.NeighborRails.Select(neighbor => creature.Manager.FindComponent(neighbor.NeighborID) as Rail.RailEntity)) { var actions = GetMoveActions(new MoveState() { Voxel = v, VehicleState = new VehicleState() { Rail = rail, PrevRail = neighborRail } }, OctTree); foreach (var a in actions.Where(a => a.DestinationState == currentstate)) { yield return(a); /* * if (!DwarfGame.IsMainThread && a.MoveType == MoveType.RideVehicle) * { * for (int i = 0; i < 10; i++) * { * Drawer3D.DrawBox(rail.GetBoundingBox(), Color.Red, 0.1f, false); * System.Threading.Thread.Sleep(1); * } * } */ } } foreach (var a in GetMoveActions(new MoveState() { Voxel = v, VehicleState = new VehicleState() { Rail = rail, PrevRail = null } }, OctTree).Where(a => a.DestinationState == currentstate)) { yield return(a); } } } }
// 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) }); }
// Inverts GetMoveActions. So, returns the list of move actions whose target is the current voxel. // Very, very slow. public IEnumerable <MoveAction> GetInverseMoveActions(MoveState currentstate, OctTreeNode OctTree, List <Body> teleportObjects) { if (Parent == null) { yield break; } if (Creature == null) { yield break; } var current = currentstate.Voxel; if (Can(MoveType.Teleport)) { foreach (var obj in teleportObjects) { if ((obj.Position - current.WorldPosition).LengthSquared() > 2) { continue; } for (int dx = -TeleportDistance; dx <= TeleportDistance; dx++) { for (int dz = -TeleportDistance; dz <= TeleportDistance; dz++) { for (int dy = -TeleportDistance; dy <= TeleportDistance; dy++) { if (dx * dx + dy * dy + dz * dz > TeleportDistanceSquared) { continue; } VoxelHandle teleportNeighbor = new VoxelHandle(Parent.World.ChunkManager.ChunkData, current.Coordinate + new GlobalVoxelOffset(dx, dy, dz)); if (teleportNeighbor.IsValid && teleportNeighbor.IsEmpty && !VoxelHelpers.GetNeighbor(teleportNeighbor, new GlobalVoxelOffset(0, -1, 0)).IsEmpty) { yield return(new MoveAction() { InteractObject = obj, Diff = new Vector3(dx, dx, dz), SourceVoxel = teleportNeighbor, DestinationState = currentstate, MoveType = MoveType.Teleport }); } } } } } } var storage = new MoveActionTempStorage(); foreach (var v in VoxelHelpers.EnumerateCube(current.Coordinate) .Select(n => new VoxelHandle(current.Chunk.Manager.ChunkData, n)) .Where(h => h.IsValid)) { foreach (var a in GetMoveActions(new MoveState() { Voxel = v }, OctTree, teleportObjects, storage).Where(a => a.DestinationState == currentstate)) { yield return(a); } if (!Can(MoveType.RideVehicle)) { continue; } // Now that dwarfs can ride vehicles, the inverse of the move actions becomes extremely complicated. We must now // iterate through all rails intersecting every neighbor and see if we can find a connection from that rail to this one. // Further, we must iterate through the entire rail network and enumerate all possible directions in and out of that rail. // Yay! // Actually - why not just not bother with rails when inverse pathing, since it should only be invoked when forward pathing fails anyway? var bodies = new HashSet <Body>(); OctTree.EnumerateItems(v.GetBoundingBox(), bodies); var rails = bodies.OfType <Rail.RailEntity>().Where(r => r.Active); foreach (var rail in rails) { if (rail.GetContainingVoxel() != v) { continue; } foreach (var neighborRail in rail.NeighborRails.Select(neighbor => Creature.Manager.FindComponent(neighbor.NeighborID) as Rail.RailEntity)) { var actions = GetMoveActions(new MoveState() { Voxel = v, VehicleState = new VehicleState() { Rail = rail, PrevRail = neighborRail } }, OctTree, teleportObjects, storage); foreach (var a in actions.Where(a => a.DestinationState == currentstate)) { yield return(a); } } foreach (var a in GetMoveActions(new MoveState() { Voxel = v, VehicleState = new VehicleState() { Rail = rail, PrevRail = null } }, OctTree, teleportObjects, storage).Where(a => a.DestinationState == currentstate)) { yield return(a); } } } }