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