Beispiel #1
0
        private double ConvertVtoY(double v, MapRoom currentRoom)
        {
            double y;

            y = (v - currentRoom.RootNodeUV.Y) * (currentRoom.LengthY / currentRoom.LengthV);
            return(Math.Abs(y));
        }
Beispiel #2
0
 //copy constructor
 public mPoint(mPoint nPoint)
 {
     PointType = nPoint.PointType;
     X         = nPoint.X;
     Y         = nPoint.Y;
     RoomIn    = nPoint.RoomIn;
 }
Beispiel #3
0
        public mPoint GetUVFromXY(MapRoom currentRoom)
        {
            double x = ConvertXtoU(X, currentRoom);
            double y = ConvertYtoV(Y, currentRoom);

            return(new mPoint(x, y));
        }
Beispiel #4
0
        private double ConvertUtoX(double u, MapRoom currentRoom)
        {
            double x;

            x = (u - currentRoom.RootNodeUV.X) * (currentRoom.WidthX / currentRoom.WidthU);
            return(Math.Abs(x));
        }
Beispiel #5
0
        public mPoint(double x, double y, MapRoom nRoom = null)
        {
            X = Math.Round(x, 4);
            Y = Math.Round(y, 4);

            RoomIn = nRoom;
        }
Beispiel #6
0
 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);
        }
Beispiel #8
0
 public mPoint GetPointUVInCurrentRoom(MapRoom currentRoom)
 {
     if (RoomA == currentRoom)
     {
         return(PointInA);
     }
     else
     {
         return(PointInB);
     }
 }
Beispiel #9
0
 public MapRoom GetOpositRoom(MapRoom currentRoom)
 {
     if (RoomA == currentRoom)
     {
         return(RoomB);
     }
     else
     {
         return(RoomA);
     }
 }
Beispiel #10
0
        public bool Load(string dirname)
        {
            BuildingMap  = new Map(XMLFileToFloorPlan(dirname));
            StartingRoom = BuildingMap.GetStartingRoom();

            if (BuildingMap != null)
            {
                return(true);
            }

            return(false);
        }
Beispiel #11
0
 // 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);
         }
     }
 }
Beispiel #12
0
        //****************** 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;
        }
Beispiel #13
0
        //************************************* 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);
        }
Beispiel #14
0
        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);
        }
Beispiel #15
0
        //****************** 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);
        }
Beispiel #16
0
        // 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;
                }
            }
        }
Beispiel #17
0
        //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));
        }
Beispiel #18
0
        //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);
        }
Beispiel #19
0
        //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);
        }
Beispiel #20
0
        //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");
            }
        }
Beispiel #21
0
 public void SetRoom(MapRoom nRoom)
 {
     RoomIn = nRoom;
 }
Beispiel #22
0
 //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));
 }
Beispiel #23
0
        //****************** Constructors ******************

        public mGridPoint(double x, double y, MapRoom room)
        {
            SetRoom(room);
            X = Math.Round(x, 4);
            Y = Math.Round(y, 4);
        }
Beispiel #24
0
 //convert the given location
 public Vector getDirectionTo(mPoint startPointInUV, mPoint destPointInUV, MapRoom CurrentRoom)
 {
     return(new Vector(destPointInUV.X - startPointInUV.X, destPointInUV.Y - startPointInUV.Y));
 }