//goes through the grid points and finds the point that is the closest returns null if failed //TODO: Make sure the point is accessable. Can be made faster by again claculating the location public mGridPoint GetClosestGridPoint(mPoint pointUV) { mGridPoint closestPoint = null; double currentShortestDistance = 0; foreach (List <mGridPoint> cRow in ListOfGridPointRows) { foreach (mGridPoint cGridPoint in cRow) { //TODO: add a check for accesability if (closestPoint == null) { closestPoint = cGridPoint; currentShortestDistance = pointUV.GetDistanceToPoint(cGridPoint); } else { double tmpDistance = pointUV.GetDistanceToPoint(cGridPoint); //TODO: add a check for accesability if (tmpDistance < currentShortestDistance) { closestPoint = cGridPoint; currentShortestDistance = tmpDistance; } } } } return(closestPoint); }
//****************** Constructors ****************** public MapObject(string name, bool passibility, mPoint location, int width, int length) { Name = name; Passible = passibility; ObjShape = new RectangleObj(location, width, length); }
//copy constructor public mPoint(mPoint nPoint) { PointType = nPoint.PointType; X = nPoint.X; Y = nPoint.Y; RoomIn = nPoint.RoomIn; }
public MapRoom(Room nRoom) { Name = nRoom.Name; RootNodeUV = new mPoint(nRoom.Root_IPS_U, nRoom.Root_IPS_V); TopLeftCornerUV = new mPoint(nRoom.TopLeftU, nRoom.TopLeftV); WidthU = nRoom.Width_U; LengthV = nRoom.Length_V; WidthX = nRoom.Width_CM; LengthY = nRoom.Length_CM; //the -1 excludes the nodes GridPointsInU = nRoom.Number_Width_GridPoints - 1; GridPointsInV = nRoom.Number_Length_GridPoints - 1; //populate the rows for (int i = 0; i < GridPointsInV; i++) { ListOfGridPointRows.Add(new List <mGridPoint>()); } List <GridPoint> xmlGridPoints = new List <GridPoint>(); //ignore the first 4 nodes wich are the root nodes //all point are given in y for (int i = 4; i < nRoom.GridPoints.Count; i++) { xmlGridPoints.Add(nRoom.GridPoints[i]); } //x for (int i = 0; i < GridPointsInU; i++) { //x for (int j = 0; j < GridPointsInV; j++) { ListOfGridPointRows[j].Add(new mGridPoint(xmlGridPoints[(i * GridPointsInV) + j])); } } Console.Out.WriteLine(ToString()); foreach (Room_Object nObject in nRoom.Objects) { ListOfObjects.Add(new MapObject(nObject)); } foreach (Region nRegion in nRoom.Regions) { ListOfAreas.Add(new MapArea(nRegion)); } XMLRoom = nRoom; }
public SearchNode(MapRoom currentRoom, mPoint currentPoint, MapEdge currentMapEdge, List <mPoint> listOfPoints, double distance) { Room = currentRoom; locationPoint = currentPoint; Edge = currentMapEdge; AddPointsToListOfPointInUV(listOfPoints); Distance = distance; }
/// <summary> /// Public constructor /// </summary> public Cartographer(string dirname, mPoint startingPositionInXY) { //changed Load(dirname); //wait for an iteration of the IPS system that way //we can get the starting location StartingPositionInXY = startingPositionInXY; }
//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); }
//************************************* Test Methods ************************************* //returns true if the point is within the area public bool ContainsPointInUV(mPoint pointUV) { //checks to make sure it is withing the area assumes it is a rectangle. if (pointUV.X >= TopLeftCornerUV.X && pointUV.X <= TopLeftCornerUV.X + WidthU && pointUV.Y <= TopLeftCornerUV.Y + LengthV && pointUV.Y >= TopLeftCornerUV.Y) { return(true); } return(false); }
//retunrs a MapRoom structure from a given point in UV if there is no associated room we return null public MapRoom GetRoomFromPointInUV(mPoint pointInUV) { foreach (MapRoom currentRoom in RoomsInMap) { if (currentRoom.ContainsPointInUV(pointInUV)) { return(currentRoom); } } return(null); }
//gets Room topLeftPoint and dimentions of an area from a point in UV. returns null if //failed to find. public Tuple <mPoint, Tuple <double, double> > GetAreaCornerAndDimentions(mPoint pointUV) { foreach (MapArea cArea in ListOfAreas) { if (cArea.ContainsPoint(pointUV)) { return(cArea.GetCornerAndDimentions()); } } return(null); }
// 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; }
//************************************* Overriden From ObjectShape Methods ************************************* //retunrs if the point given is inside of the rectangle public override bool DoesContain(mPoint point, int buffer = 0) { //checks to see if the point is in the rectangle by making sure it is bounded by //the points //check in x //return (point.X - buffer >= Location.X && point.X + Width + buffer <= Location.X) && ////check in y // (point.Y - buffer >= Location.Y && point.Y + Length + buffer <= Location.Y); //check in x return((point.X >= Location.X - buffer && point.X <= Location.X + Width + buffer) && (point.Y >= Location.Y - buffer && point.Y <= Location.Y + Length + buffer)); //return (point.X >= Location.X && point.X <= Location.X + Width) && // (point.Y >= Location.Y && point.Y <= Location.Y + Length); }
//****************** 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); }
public override bool Equals(object obj) { //if we have been passed a null object if (obj == null) { return(false); } //if we cant cast the object to an mPoint mPoint nPoint = obj as mPoint; if ((System.Object)nPoint == null) { return(false); } return(X == nPoint.X && Y == nPoint.Y); }
//************************************* Test Methods ************************************* //returns if a line segment given by two points crosses the rectangle //http://stackoverflow.com/questions/5514366/how-to-know-if-a-line-intersects-a-rectangle public override bool DoesLineCross(mPoint pt1, mPoint pt2, int buffer = 0) { //check top (top left, top right) return(LineIntersectsLine(pt1, pt2, new mPoint(Location.X - buffer, Location.Y - buffer), new mPoint(Location.X + Width + buffer, Location.Y + buffer)) || //check left (top right, bottom right) LineIntersectsLine(pt1, pt2, new mPoint(Location.X + Width + buffer, Location.Y - buffer), new mPoint(Location.X + Width + buffer, Location.Y + Length + buffer)) || //check bottom (bottom right, bottom left) LineIntersectsLine(pt1, pt2, new mPoint(Location.X + Width + buffer, Location.Y + Length + buffer), new mPoint(Location.X - buffer, Location.Y + Length + buffer)) || //check right (bottom left, top left) LineIntersectsLine(pt1, pt2, new mPoint(Location.X - buffer, Location.Y + Length + buffer), new mPoint(Location.X - buffer, Location.Y - buffer)) || //check contains //from && (DoesContain(pt1, buffer) || DoesContain(pt2, buffer))); //return LineIntersectsLine(pt1, pt2, new mPoint(Location.X, Location.Y), new mPoint(Location.X + Width, Location.Y)) || // LineIntersectsLine(pt1, pt2, new mPoint(Location.X + Width, Location.Y), new mPoint(Location.X + Width, Location.Y + Length)) || // LineIntersectsLine(pt1, pt2, new mPoint(Location.X + Width, Location.Y + Length), new mPoint(Location.X, Location.Y + Length)) || // LineIntersectsLine(pt1, pt2, new mPoint(Location.X, Location.Y + Length), new mPoint(Location.X, Location.Y)) || // (DoesContain(pt1) && DoesContain(pt2)); }
//returns the location of the gridpoint in the list of lists (0, 0) is top left //TODO: You can make this constant time by calculating the location private Tuple <int, int> GetGridPointLocation(mPoint mPointUV) { int y = 0; int x = 0; foreach (List <mGridPoint> rowList in ListOfGridPointRows) { foreach (mGridPoint gPoint in rowList) { if (gPoint == mPointUV) { return(new Tuple <int, int>(x, y)); } x++; } x = 0; y++; } return(null); }
//************************************* Test Methods ************************************* //given two lines defined as two points it will return if the lines cross //http://stackoverflow.com/questions/5514366/how-to-know-if-a-line-intersects-a-rectangle protected static bool LineIntersectsLine(mPoint l1p1, mPoint l1p2, mPoint l2p1, mPoint l2p2) { double q = (l1p1.Y - l2p1.Y) * (l2p2.X - l2p1.X) - (l1p1.X - l2p1.X) * (l2p2.Y - l2p1.Y); double d = (l1p2.X - l1p1.X) * (l2p2.Y - l2p1.Y) - (l1p2.Y - l1p1.Y) * (l2p2.X - l2p1.X); if (d == 0) { return(false); } double r = q / d; q = (l1p1.Y - l2p1.Y) * (l1p2.X - l1p1.X) - (l1p1.X - l2p1.X) * (l1p2.Y - l1p1.Y); double s = q / d; if (r < 0 || r > 1 || s < 0 || s > 1) { return(false); } return(true); }
//************************************* Test Methods ************************************* //runs a check to see if the given Vector is intersecting //this object public bool DoesLineCross(mPoint startPt, mPoint endPt, int buffer = 0) { //check to see if the vector intersects the shape with the //addition of the bufferZone return(ObjShape.DoesLineCross(startPt, endPt, buffer)); }
//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); }
//convert the given location in UV and a destination point in UV public Vector GetDirectionTo(mPoint currentLocationInUV, mPoint endPointInUV) { //vector is = (pointInUV.x - pointInUV.x, pointInUV.y - pointInUV.y) return(new Vector(currentLocationInUV.X - endPointInUV.X, currentLocationInUV.Y - endPointInUV.Y)); }
//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); }
//************************************* Get and Set Methods ************************************* //gets the strait line distance between two mPoints public double GetDistanceToPoint(mPoint otherPoint) { return(Math.Sqrt(Math.Pow(X - otherPoint.X, 2) + Math.Pow(Y - otherPoint.Y, 2))); }
private double Length = 0; //in y direction //****************** Constructors ****************** //givent the upperleft hand corrner and the width in x and length in y public RectangleObj(mPoint location, double width, double length) { Location = location; Width = width; Length = length; }
public bool DoesContain(mPoint point, int buffer = 0) { return(ObjShape.DoesContain(point, buffer)); }
public void addPoint(mPoint newPoint) { ListOfPointsInUV.Add(newPoint); }
//****************** Constructors ****************** public MapArea(string name, mPoint location, double width, double length) { Name = name; ObjShape = new RectangleObj(location, width, length); }
abstract public bool DoesContain(mPoint point, int buffer);
//************************************* Abstract Methods ************************************* abstract public bool DoesLineCross(mPoint pt1, mPoint pt2, int buffer);
//****************** Constructors ****************** public MapPOI(string name, mPoint location) { Name = name; Location = location; }