private double ConvertVtoY(double v, MapRoom currentRoom)
{
double y;
y = (v - currentRoom.RootNodeUV.Y) * (currentRoom.LengthY / currentRoom.LengthV);
return(Math.Abs(y));
}
//copy constructor
public mPoint(mPoint nPoint)
{
PointType = nPoint.PointType;
X = nPoint.X;
Y = nPoint.Y;
RoomIn = nPoint.RoomIn;
}
public mPoint GetUVFromXY(MapRoom currentRoom)
{
double x = ConvertXtoU(X, currentRoom);
double y = ConvertYtoV(Y, currentRoom);
return(new mPoint(x, y));
}
private double ConvertUtoX(double u, MapRoom currentRoom)
{
double x;
x = (u - currentRoom.RootNodeUV.X) * (currentRoom.WidthX / currentRoom.WidthU);
return(Math.Abs(x));
}
public mPoint(double x, double y, MapRoom nRoom = null)
{
X = Math.Round(x, 4);
Y = Math.Round(y, 4);
RoomIn = nRoom;
}
public SearchNode(MapRoom currentRoom, mPoint currentPoint, MapEdge currentMapEdge, List <mPoint> listOfPoints, double distance)
{
Room = currentRoom;
locationPoint = currentPoint;
Edge = currentMapEdge;
AddPointsToListOfPointInUV(listOfPoints);
Distance = distance;
}
//gets Room topLeftPoint and dimentions
public Tuple <mPoint, Tuple <double, double> > GetRoomCornerAndDimentions(mPoint PointInRoomInUV)
{
MapRoom tmpRoom = BuildingMap.GetRoomFromPointInUV(PointInRoomInUV);
if (tmpRoom != null)
{
return(tmpRoom.GetRoomLocationAndDimentions());
}
return(null);
}
public mPoint GetPointUVInCurrentRoom(MapRoom currentRoom)
{
if (RoomA == currentRoom)
{
return(PointInA);
}
else
{
return(PointInB);
}
}
public MapRoom GetOpositRoom(MapRoom currentRoom)
{
if (RoomA == currentRoom)
{
return(RoomB);
}
else
{
return(RoomA);
}
}
public bool Load(string dirname)
{
BuildingMap = new Map(XMLFileToFloorPlan(dirname));
StartingRoom = BuildingMap.GetStartingRoom();
if (BuildingMap != null)
{
return(true);
}
return(false);
}
// Returns whether or not the given point is occupied by an obstacle
// The point should be in XY
public bool IsPointObstructed(mPoint point, MapRoom room)
{
lock (PointMarkedCount)
{
if (PointMarkedCount.ContainsKey(point))
{
return(PointMarkedCount[point] > 0);
}
else
{
return(false);
}
}
}
//****************** Constructors ******************
public MapEdge(MapRoom firstRoom, mPoint pointInFirstRoom, MapRoom secondRoom,
mPoint pointInsecondRoom, Connection nConnection)
{
RoomA = firstRoom;
RoomB = secondRoom;
PointInA = pointInFirstRoom;
PointInA.PointType = mPoint.mPointType.Door;
PointInB = pointInsecondRoom;
PointInB.PointType = mPoint.mPointType.Door;
DistanceUV = nConnection.UVDistance;
EdgeConnection = nConnection;
}
//************************************* Internal Methods *************************************
private double ConvertXtoU(double x, MapRoom currentRoom)
{
double u;
// handles conversion when root node is in topRight or bottomRight
if (currentRoom.RootNodeUV.X == currentRoom.TopLeftCornerUV.X + currentRoom.WidthU)
{
u = currentRoom.RootNodeUV.X - (x * (currentRoom.WidthU / currentRoom.WidthX));
return(u);
}
// handles conversion when root node is in topLeft or bottomLeft
u = x * (currentRoom.WidthU / currentRoom.WidthX) + currentRoom.RootNodeUV.X;
return(u);
}
private double ConvertYtoV(double y, MapRoom currentRoom)
{
double v;
// handles conversion when root node is in bottomLeft or bottomRight
if (currentRoom.RootNodeUV.Y == currentRoom.TopLeftCornerUV.Y + currentRoom.LengthV)
{
v = currentRoom.RootNodeUV.Y - (y * (currentRoom.LengthV / currentRoom.LengthY));
return(v);
}
// handles conversion when root node is in topLeft or topRight
v = y * (currentRoom.LengthV / currentRoom.LengthY) + currentRoom.RootNodeUV.Y;
return(v);
}
//****************** Methods for Loading ******************
//adds edges to the map
private void AddEdge(Connection nConnection)
{
mPoint uvPointA = new mPoint(nConnection.Room_U, nConnection.Room_V);
mPoint uvPointB = new mPoint(nConnection.Other_Room_U, nConnection.Other_Room_V);
//get the two rooms associated with the given points in uv
MapRoom RoomA = GetRoomFromPointInUV(uvPointA);
MapRoom RoomB = GetRoomFromPointInUV(uvPointB);
//create a new edge add it to the Associated Rooms then add it to the map.
MapEdge tmpEdge = new MapEdge(RoomA, uvPointA, RoomB, uvPointB, nConnection);
RoomA.AddEdge(tmpEdge);
RoomB.AddEdge(tmpEdge);
EdgesInMap.Add(tmpEdge);
}
// Adds the given obstacle to the given room
// Obstacles remain in the room for OBSTACLE_TTL seconds
// Expects the points to be in XY
public void AddObstaclePoints(List <mPoint> points, MapRoom room)
{
foreach (mPoint point in points)
{
int numberMarked = 0;
//Console.WriteLine("Adding point to obstacle map: XY({0}, {1}), UV({2}, {3})", point.X, point.Y, point.GetUVFromXY(room).X, point.GetUVFromXY(room).Y);
lock (PointMarkedCount)
{
if (PointMarkedCount.ContainsKey(point))
{
numberMarked = PointMarkedCount[point];
}
PointMarkedCount[point] = numberMarked + 1;
}
}
}
//gets the room containing the point and then gets the area from that room. It will return null
//if the point is not contained in an area.
public Tuple <mPoint, Tuple <double, double> > GetAreaCornerAndDimentions(mPoint PointInRoomUV)
{
MapRoom room = BuildingMap.GetRoomFromPointInUV(PointInRoomUV);
return(room.GetAreaCornerAndDimentions(PointInRoomUV));
}
//this will return a list of PointsInUV. from the given point to the end point.
//note: does include the startPoint
//if the start and end point is in the same room it will return a list containing the two points
//fail cases: return null
// we cant get the the given point from the start point
// the start or end points are not in a room
public List <mPoint> SearchRoute(mPoint startPointInUV, mPoint endPointInUV)
{
//get the start and end rooms and make sure they exist
MapRoom startRoom = GetRoomFromPointInUV(startPointInUV);
if (startRoom == null)
{
return(null);
}
MapRoom endRoom = GetRoomFromPointInUV(endPointInUV);
if (endRoom == null)
{
return(null);
}
if ((startPointInUV.X == endPointInUV.X) && (startPointInUV.Y == endPointInUV.Y))
{
List <mPoint> listOfPoints = new List <mPoint>();
listOfPoints.Add(endPointInUV);
return(listOfPoints);
}
//if we are searching in room
if (startRoom == endRoom)
{
//get the list of points and return them
List <mPoint> listOfPoints = new List <mPoint>();
listOfPoints.Add(startPointInUV);
listOfPoints.Add(endPointInUV);
return(listOfPoints);
}
PriorityQueue queue = new PriorityQueue();
//hashset to keep track of the visited points
HashSet <mPoint> TableOfVisitedPoints = new HashSet <mPoint>();
//add the starting point to the que
List <mPoint> startingListOfPoints = new List <mPoint>();
startingListOfPoints.Add(startPointInUV);
queue.Add(new SearchNode(startRoom, startPointInUV, null, startingListOfPoints, 0));
//queue.printPriorityQueue ();
int index = 0;
while (!queue.isEmpty())
{
index++;
//get the node with the shortest path
SearchNode currentNode = queue.Pop();
//mark the point we have visited off.
TableOfVisitedPoints.Add(currentNode.locationPoint);
//if we have found the shortest path to the endpoint
if (currentNode.locationPoint == endPointInUV)
{
return(currentNode.ListOfPointsInUV);
}
//if we have found a path to the last room. here we want to make sure we get
//the distance from the currentpoint to the end point
if (currentNode.Room == endRoom)
{
//claculate the new distance from the total path distance + the distance between the last point and the endpoint
double newDistance = currentNode.Distance + currentNode.locationPoint.GetDistanceToPoint(endPointInUV);
//the searchNode has the same room as the currentNode, the newpoint will be the endpoint, there is no edge
//the list of points will be updated with the endPoint and the distance will be updated
SearchNode newNode = new SearchNode(currentNode.Room,
endPointInUV,
null,
currentNode.ListOfPointsInUV,
newDistance);
newNode.addPoint(endPointInUV);
//does not check table because if we have hit the endpoint already we are done
queue.Add(newNode);
}
//if there is an edge to traverse
if (currentNode.Edge != null)
{
mPoint nextPoint = currentNode.Edge.GetPointUVInOpositRoom(currentNode.Room);
MapRoom newRoom = currentNode.Edge.GetOpositRoom(currentNode.Room);
double newDistance = currentNode.Distance + currentNode.Edge.GetDistanceUV();
//moves the room the the next room, moves the point to the next room's point
//keeps the edge the same and adds the distance of the edge to the node distance
SearchNode newNode = new SearchNode(newRoom,
nextPoint,
currentNode.Edge,
currentNode.ListOfPointsInUV,
newDistance);
newNode.addPoint(nextPoint);
//make sure we have not been to that point before
if (!TableOfVisitedPoints.Contains(newNode.locationPoint))
{
queue.Add(newNode);
}
}
//search through the room and get all of the edges
foreach (MapEdge currentEdge in currentNode.Room.ListOfEdges)
{
mPoint pointInRoom = currentEdge.GetPointUVInCurrentRoom(currentNode.Room);
//if the point has not been visited
if (!TableOfVisitedPoints.Contains(pointInRoom))
{
double newDistance = currentNode.locationPoint.GetDistanceToPoint(pointInRoom);
SearchNode newNode = new SearchNode(currentNode.Room,
pointInRoom,
currentEdge,
currentNode.ListOfPointsInUV,
newDistance);
newNode.addPoint(pointInRoom);
if (!TableOfVisitedPoints.Contains(newNode.locationPoint))
{
queue.Add(newNode);
}
}
}
}
return(null);
}
//returns a list of mPoints in UV that we can use to navigate a room.
//if they are the same point it will just return that point
//note: this does include the start point.
//if this fails it will return null
public List <mPoint> SearchPath(mPoint startPointUV, mPoint endPointUV, IObstacleMap obstacleMap)
{
List <mPoint> returnList = new List <mPoint>();
MapRoom room1 = GetRoomFromPointInUV(startPointUV);
if (room1 == null)
{
return(null);
}
MapRoom room2 = GetRoomFromPointInUV(endPointUV);
if (room2 == null)
{
return(null);
}
//The names of each room are unique this is enforced when the map is created
if (room1.Name != room2.Name)
{
return(null);
}
if (startPointUV == endPointUV)
{
returnList.Add(startPointUV);
return(returnList);
}
mGridPoint startingGridPoint = room1.GetClosestGridPoint(startPointUV);
if (startingGridPoint == null)
{
return(null);
}
mGridPoint endingGridPoint = room1.GetClosestGridPoint(endPointUV);
if (startingGridPoint == null)
{
return(null);
}
PathPQueue queue = new PathPQueue();
//hashset to keep track of the visited points
HashSet <mPoint> tableOfVisitedPoints = new HashSet <mPoint>();
Console.Out.WriteLine(room1.ToString());
//set up the queue and cast the starting point in UV to a grid point so we can store it in the
//PathSearchNode.
List <mGridPoint> startingListOfGPoints = new List <mGridPoint>();
startingListOfGPoints.Add(new mGridPoint(startPointUV.X, startPointUV.Y, room1));
startingListOfGPoints.Add(startingGridPoint);
queue.Add(new PathSearchNode(startingGridPoint, startingListOfGPoints, 0));
while (!queue.IsEmpty())
{
PathSearchNode currentNode = queue.Pop();
tableOfVisitedPoints.Add(currentNode.CurrentGridPoint);
if (currentNode.CurrentGridPoint == endingGridPoint)
{
//add the last point to the list
returnList = currentNode.GetListOfMPoints();
returnList.Add(endPointUV);
printPathSearch(currentNode, queue, tableOfVisitedPoints, room1, startingGridPoint, endingGridPoint, currentNode.CurrentGridPoint, new List <mGridPoint>());
return(returnList);
}
List <mGridPoint> adjacentGridPoints = room1.GetAdjacentGridPoints(currentNode.CurrentGridPoint);
foreach (mGridPoint adjGPoint in adjacentGridPoints)
{
//if we have not visited this point and it is passible
bool isPointObstructed = false; // obstacleMap.IsPointObstructed(adjGPoint, room1);
if (!tableOfVisitedPoints.Contains(adjGPoint) && adjGPoint.IsPassible() && !isPointObstructed)
{
double newDistance = room1.GetWeightedDistance(currentNode.CurrentGridPoint, adjGPoint, BufferDistance);
//get weighted distance returns -1 if it crosses an object.
if (newDistance > 0)
{
//add the point to the queue make a deep copy
List <mGridPoint> tmpGridPointList = new List <mGridPoint>(currentNode.ListOfGridPoints);
tmpGridPointList.Add(adjGPoint);
queue.Add(new PathSearchNode(adjGPoint, tmpGridPointList, currentNode.Distance + newDistance));
}
}
}
}
return(null);
}
//Testing Method
//very helpful for debugging what.
public void printPathSearch(PathSearchNode currentPath, PathPQueue queue, HashSet <mPoint> tableOfVisitedPoints, MapRoom cRoom, mGridPoint start, mGridPoint end, mGridPoint current,
List <mGridPoint> adjacentGridPoints)
{
string Row = "";
Console.Out.WriteLine("******************************");
//Console.Out.WriteLine("CurrentPath: " + currentPath);
//Console.Out.WriteLine("---------------------------------");
//Console.Out.WriteLine("Queue: " + queue);
//print top row
foreach (mGridPoint gpoint in cRoom.ListOfGridPointRows[0])
{
Row += Math.Round(gpoint.X, 1) + "\t";
}
Console.Out.WriteLine(" \t" + Row);
Row = "";
//print each row ad mark if we have visited a place
foreach (List <mGridPoint> gridRows in cRoom.ListOfGridPointRows)
{
Row = Math.Round(gridRows[0].Y, 1) + "\t";
foreach (mGridPoint gpoint in gridRows)
{
if (adjacentGridPoints.Contains(gpoint))
{
Row += "a";
}
if (currentPath.ListOfGridPoints.Contains(gpoint))
{
Row += "p";
}
if (!gpoint.IsPassible())
{
Row += "[";
}
if (gpoint == current)
{
Row += " C ";
}
else if (gpoint == start)
{
Row += " S ";
}
else if (gpoint == end)
{
Row += " E ";
}
else if (tableOfVisitedPoints.Contains(gpoint))
{
Row += " X ";
}
else
{
Row += " . ";
}
if (!gpoint.IsPassible())
{
Row += "]";
}
Row += "\t";
}
Console.Out.WriteLine(Row + "\t");
}
}
public void SetRoom(MapRoom nRoom)
{
RoomIn = nRoom;
}
//takes the current room and point in XY and the current room then returns the point in UV
public mPoint GetLocationInUV(mPoint pointXY, MapRoom currentRoom)
{
return(pointXY.GetUVFromXY(currentRoom));
}
//****************** Constructors ******************
public mGridPoint(double x, double y, MapRoom room)
{
SetRoom(room);
X = Math.Round(x, 4);
Y = Math.Round(y, 4);
}
//convert the given location
public Vector getDirectionTo(mPoint startPointInUV, mPoint destPointInUV, MapRoom CurrentRoom)
{
return(new Vector(destPointInUV.X - startPointInUV.X, destPointInUV.Y - startPointInUV.Y));
}