private void OpenSetSortedInsert(List<VMRoomPortal> set, Dictionary<VMRoomPortal, double> fScore, VMRoomPortal portal)
{
var myScore = fScore[portal];
for (int i = 0; i < set.Count; i++)
{
if (myScore < fScore[set[i]])
{
set.Insert(i, portal);
return;
}
}
set.Add(portal);
}
private bool AttemptRoute(VMFindLocationResult route)
{
//returns false if there is no room portal route to the destination room.
CurRoute = route;
WalkTo = null; //reset routing state
Walking = false;
Turning = false;
AttemptedChair = false;
TurnTweak = 0;
var avatar = (VMAvatar)Caller;
//if we are routing to a chair, let it take over.
if (route.Chair != null)
{
AttemptedChair = false;
return true;
}
Rooms = new Stack<VMRoomPortal>();
var DestRoom = VM.Context.GetRoomAt(route.Position);
var MyRoom = VM.Context.GetRoomAt(avatar.Position);
if (DestRoom == MyRoom || (route.Flags & SLOTFlags.IgnoreRooms) > 0) return true; //we don't have to do any room finding for this
else {
//find shortest room traversal to destination. Simple A* pathfind.
//Portals are considered nodes to allow multiple portals between rooms to be considered.
var openSet = new List<VMRoomPortal>(); //we use this like a queue, but we need certain functions for sorted queue that are only provided by list.
var closedSet = new HashSet<VMRoomPortal>();
var gScore = new Dictionary<VMRoomPortal, double>();
var fScore = new Dictionary<VMRoomPortal, double>();
var parents = new Dictionary<VMRoomPortal, VMRoomPortal>();
var StartPortal = new VMRoomPortal(Caller.ObjectID, MyRoom); //consider the sim as a portal to this room (as a starting point)
openSet.Add(StartPortal);
gScore[StartPortal] = 0;
fScore[StartPortal] = GetDist(Caller.Position, route.Position);
while (openSet.Count != 0) {
var current = openSet[0];
openSet.RemoveAt(0);
if (current.TargetRoom == DestRoom) {
//this portal gets us to the room.
while (current != StartPortal) //push previous portals till we get to our first "portal", the sim in its current room (we have already "traversed" this portal)
{
Rooms.Push(current);
current = parents[current];
}
return true;
}
closedSet.Add(current);
var portals = VM.Context.RoomInfo[current.TargetRoom].Portals;
foreach (var portal in portals) { //evaluate all neighbor portals
if (closedSet.Contains(portal)) continue; //already evaluated!
var pos = VM.GetObjectById(portal.ObjectID).Position;
var gFromCurrent = gScore[current] + GetDist(VM.GetObjectById(current.ObjectID).Position, pos);
var newcomer = !openSet.Contains(portal);
if (newcomer || gFromCurrent < gScore[portal]) {
parents[portal] = current; //best parent for now
gScore[portal] = gFromCurrent;
fScore[portal] = gFromCurrent + GetDist(pos, route.Position);
if (newcomer) { //add and move to relevant position
OpenSetSortedInsert(openSet, fScore, portal);
} else { //remove and reinsert to refresh sort
openSet.Remove(portal);
OpenSetSortedInsert(openSet, fScore, portal);
}
}
}
}
return false;
}
}
