/// <summary> /// Constructs a new PathFinder initialized with the desired building ID /// </summary> /// <param name="buildingId"></param> public BlueprintPathfinder(BlueprintUnitOfWork uow, Blueprint bluePrint) { _bluePrint = bluePrint; _uow = uow; }
/// <summary> /// Fullfills pathfinding requests by returning a Route object. /// </summary> /// <param name="startX"></param> /// <param name="endX"></param> /// <param name="startY"></param> /// <param name="endY"></param> /// <param name="startFloor"></param> /// <param name="endFloor"></param> /// <returns></returns> public RouteDto getPath(string startBuildingId, string endBuildingId, double startPercentX, double startPercentY, double endPercentX, double endPercentY, int startFloor, int endFloor) { using (var uow = new BlueprintUnitOfWork()) { _startBluePrint = uow.BluePrints.FirstOrDefault(b => b.Id == ObjectId.Parse(startBuildingId)); // The start cannot be null if (_startBluePrint == null) { return new RouteDto().pathfindingError(); } var startFloorPlan = _startBluePrint.Floorplans.FirstOrDefault(fp => fp.Floor == startFloor); var endFloorPlan = startFloorPlan; if (startBuildingId == endBuildingId && startFloor != endFloor) { endFloorPlan = _startBluePrint.Floorplans.FirstOrDefault(fp => fp.Floor == endFloor); } int startGridWidth = startFloorPlan.ImageDataWidth; int startGridHeight = startFloorPlan.ImageDataHeight; int endGridWidth = endFloorPlan.ImageDataWidth; int endGridHeight = endFloorPlan.ImageDataHeight; int startX = (int)(startGridWidth * startPercentX); int startY = (int)(startGridHeight * startPercentY); int endX = (int)(endGridWidth * endPercentX); int endY = (int)(endGridHeight * endPercentY); if (startBuildingId != endBuildingId) { _endBluePrint = uow.BluePrints.FirstOrDefault(b => b.Id == ObjectId.Parse(endBuildingId)); endFloorPlan = _endBluePrint.Floorplans.FirstOrDefault(fp => fp.Floor == endFloor); endGridWidth = endFloorPlan.ImageDataWidth; endGridHeight = endFloorPlan.ImageDataHeight; endX = (int)(endGridWidth * endPercentX); endY = (int)(endGridHeight * endPercentY); } // If the ending BluePrint is null, that only means that there is no second // building to navigate to. So we will just pathfind within the one building. if (_endBluePrint == null) { return new BlueprintPathfinder(uow, _startBluePrint).getPath(startX, startY, endX, endY, startFloorPlan, endFloorPlan); } //** The fun part! Here we will need to do a GPS (outdoor) transition between two buildings. **// // Get exit POIs for start and end buildings var startBldgExits = _startBluePrint.Floorplans .SelectMany(fp => fp.NavPois) .Where(poi => poi.Type == NavType.exit) .ToList(); var endBldgExits = _endBluePrint.Floorplans .SelectMany(fp => fp.NavPois) .Where(poi => poi.Type == NavType.exit) .ToList(); // var startBldgExits = _startBluePrint.NavigationalPois // .Where(poi => poi.Type == NavType.Exit) // .Select(poi => poi.Locations.FirstOrDefault()) // .ToList(); // var endBldgExits = _endBluePrint.NavigationalPois // .Where(poi => poi.Type == NavType.Exit) // .Select(poi => poi.Locations.FirstOrDefault()) // .ToList(); //** Get the distances between the exits of the buildings to find the closest ones **// double smallestDistance = Double.PositiveInfinity; NavPoi startLoc = null; NavPoi endLoc = null; foreach (var startNav in startBldgExits) { foreach (var endNav in endBldgExits) { double distance = GeoUtils.getDistance(startNav.Geotag[0], startNav.Geotag[1], endNav.Geotag[0], endNav.Geotag[1]); if (distance < smallestDistance) { smallestDistance = distance; startLoc = startNav; endLoc = endNav; } } } //** Add exit locations to list in order of priorty to consider for pathfinding **// var starts = new List<WeightedLocation>(); var ends = new List<WeightedLocation>(); // Floorplan for the navPoi Floorplan startNavFloorplan = _startBluePrint.GetFloorplanForNavPoi(startLoc.Id); Floorplan endNavFloorplan = _endBluePrint.GetFloorplanForNavPoi(endLoc.Id); // Set each possible start-exit location with a weight in relation to the ideal end-exit location foreach (var navPoi in startBldgExits) { double distance = GeoUtils.getDistance(navPoi.Geotag[0], navPoi.Geotag[1], endLoc.Geotag[0], endLoc.Geotag[1]); starts.Add(new WeightedLocation(navPoi.Location, distance, startNavFloorplan)); } // Set each possible end-exit location with a weight in relation to the ideal start-exit location foreach (var navPoi in endBldgExits) { double distance = GeoUtils.getDistance(navPoi.Geotag[0], navPoi.Geotag[1], startLoc.Geotag[0], startLoc.Geotag[1]); ends.Add(new WeightedLocation(navPoi.Location, distance, endNavFloorplan)); } // sort the lists into priority order according to weight starts.Sort((geo1, geo2) => geo1.Weight.CompareTo(geo2.Weight)); ends.Sort((geo1, geo2) => geo1.Weight.CompareTo(geo2.Weight)); //** Find valid routes from within each building to an exit **// WeightedLocation outidePathStarts = starts.First(); WeightedLocation outidePathEnds = ends.First(); var startPathFinder = new BlueprintPathfinder(uow, _startBluePrint); var endPathFinder = new BlueprintPathfinder(uow, _endBluePrint); RouteDto startRoute = null; RouteDto endRoute = null; // Get and validate the startPath foreach (WeightedLocation loc in starts) { startRoute = startPathFinder.getPath(startX, startY, (int)(loc.Location.X * startGridWidth), (int)(loc.Location.Y * startGridHeight), startFloorPlan, loc.Floorplan); if (startRoute != null && startRoute.code == RouteDto.CODE_SUCCESS) { startLoc.Location = loc.Location; break; } } if (startRoute == null) { return new RouteDto().pathfindingError(); } if (startRoute.code != RouteDto.CODE_SUCCESS) { return startRoute; } // Get and validate the end path foreach (WeightedLocation loc in ends) { endRoute = endPathFinder.getPath((int)(loc.Location.X * endGridWidth), (int)(loc.Location.Y * endGridHeight), endX, endY, loc.Floorplan, endFloorPlan); if (endRoute != null && endRoute.code == RouteDto.CODE_SUCCESS) { endLoc.Location = loc.Location; break; } } if (endRoute == null) { return new RouteDto().pathfindingError(); } if (endRoute.code != RouteDto.CODE_SUCCESS) { return endRoute; } // Get the Google path GoogJson resp = GoogleMapsApi.getDirections(startLoc.Geotag[0], startLoc.Geotag[1], endLoc.Geotag[0], endLoc.Geotag[1], GoogleMapsApi.MODE_WALKING); if (resp.Routes.FirstOrDefault() == null) { return new RouteDto().pathfindingError(); } RouteDto outdoorRoute = new RouteDto() .addOutdoorTransisiton(NavType.walking, new CoordinateDto((int)(startLoc.Location.X * startGridWidth), (int)(startLoc.Location.Y * startGridHeight), startPathFinder.lastFloorGridWidth, startPathFinder.lastFloorGridHeight), startNavFloorplan.Floor); outdoorRoute.addEvent( new RouteEvent() .setOutdoor(resp.Routes.FirstOrDefault().Overview_Polyline.Points, _startBluePrint.Id.ToString(), _endBluePrint.Id.ToString(), startLoc.Geotag[0], startLoc.Geotag[1], endLoc.Geotag[0], endLoc.Geotag[1])); return startRoute.append(outdoorRoute).append(endRoute); } }
/// <summary> /// Queries the Blueprint db object to get the data-map of a given floor /// </summary> /// <param name="floor"></param> /// <param name="bp"></param> /// <returns></returns> private bool[,] getFloorMap(int floor, Blueprint bp) { try { var fp = bp.Floorplans.FirstOrDefault(f => f.Floor == floor); var width = fp.ImageDataWidth; var height = fp.ImageDataHeight; bool[,] result = new bool[height, width]; string path = string.Format("fp_{0}.txt", fp.Id); string url = string.Format(Defs.DataGridUrl, path); // TODO: Using the WebClient is for testing off of the server's file system. // SOme performance could be gained if we use a file stream and absolute path. using (var fs = new WebClient().OpenRead(url)) using (var bs = new BufferedStream(fs)) using (var sr = new StreamReader(bs)) { // Read the file line by line and populate the data grid string line; int y = 0; while ((line = sr.ReadLine()) != null) { int x = 0; foreach (char c in line) { // set a true value if it is a space result[y, x] = (c == ' '); x++; } y++; } } return result; } catch (Exception) { return null; } }