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