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);
}
}
}
}