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