public RouteDto append(RouteDto route) { this.paths.AddRange(route.paths); this.navStack.AddRange(route.navStack); return this; }
/// <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(int startBuildingId, int endBuildingId, double startPercentX, double startPercentY, double endPercentX, double endPercentY, int startFloor, int endFloor) { using (BlueprintUnitOfWork uow = new BlueprintUnitOfWork()) { _startBluePrint = uow.BluePrints.FirstOrDefault(b => b.Id == startBuildingId); // The start cannot be null if (_startBluePrint == null) { return new RouteDto().pathfindingError(); } FloorPlan startFloorPlan = _startBluePrint.FloorPlans.FirstOrDefault(fp => fp.Floor == startFloor); FloorPlan 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 == 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.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; FloorPlanLocation startLoc = null; FloorPlanLocation endLoc = null; foreach (FloorPlanLocation s in startBldgExits) { foreach (FloorPlanLocation e in endBldgExits) { double distance = GeoUtils.getDistance(s.Latitude, s.Longitude, e.Latitude, e.Longitude); if (distance < smallestDistance) { smallestDistance = distance; startLoc = s; endLoc = e; } } } //** Add exit locations to list in order of priorty to consider for pathfinding **// var starts = new List<WeightedLocation>(); var ends = new List<WeightedLocation>(); // Set each possible start-exit location with a weight in relation to the ideal end-exit location foreach (FloorPlanLocation loc in startBldgExits) { double distance = GeoUtils.getDistance(loc.Latitude, loc.Longitude, endLoc.Latitude, endLoc.Longitude); starts.Add(new WeightedLocation(loc, distance)); } // Set each possible end-exit location with a weight in relation to the ideal start-exit location foreach (FloorPlanLocation loc in endBldgExits) { double distance = GeoUtils.getDistance(loc.Latitude, loc.Longitude, startLoc.Latitude, startLoc.Longitude); ends.Add(new WeightedLocation(loc, distance)); } // 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(); BluePrintPathFinder startPathFinder = new BluePrintPathFinder(uow, _startBluePrint); BluePrintPathFinder 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.XPos * startGridWidth), (int) (loc.location.YPos * startGridHeight), startFloorPlan, loc.location.FloorPlan); if (startRoute != null && startRoute.code == RouteDto.CODE_SUCCESS) { startLoc = 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.XPos * endGridWidth), (int) (loc.location.YPos * endGridHeight), endX, endY, loc.location.FloorPlan, endFloorPlan); if (endRoute != null && endRoute.code == RouteDto.CODE_SUCCESS) { endLoc = 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.Latitude, startLoc.Longitude, endLoc.Latitude, endLoc.Longitude, GoogleMapsApi.MODE_WALKING); if (resp.Routes.FirstOrDefault() == null) { return new RouteDto().pathfindingError(); } RouteDto outdoorRoute = new RouteDto() .addOutdoorTransisiton(NavType.Walking, new CoordinateDto((int) (startLoc.XPos * startGridWidth), (int) (startLoc.YPos * startGridHeight), startPathFinder.lastFloorGridWidth, startPathFinder.lastFloorGridHeight), startLoc.FloorPlan.Floor); outdoorRoute.addEvent( new RouteEvent() .setOutdoor(resp.Routes.FirstOrDefault().Overview_Polyline.Points, _startBluePrint.Id, _endBluePrint.Id, startLoc.Latitude, startLoc.Longitude, endLoc.Latitude, endLoc.Longitude)); return startRoute.append(outdoorRoute).append(endRoute); } }
/// <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(int startX, int startY, int endX, int endY, FloorPlan startFloorPlan, FloorPlan endFloorPlan) { int startFloor = startFloorPlan.Floor; int endFloor = endFloorPlan.Floor; // The route object will contain the final route, if one exists RouteDto route = new RouteDto(); if(_bluePrint == null) { return route.pathfindingError(); } // Get map of start floor through api. // If something goes wrong, return a fail-code route bool[,] startFloorMap = getFloorMap(startFloor, _bluePrint); if (startFloorMap == null) { return route.pathfindingError(); } // API call for endFloor map // (needed in places of different scope below) bool[,] endFloorMap = null; if (startFloor != endFloor) { endFloorMap = getFloorMap(endFloor, _bluePrint); if (endFloorMap == null) { return route.pathfindingError(); } } // BEGIN PATHFINDING ///////////////// if (startFloor == endFloor) { AStarSearch searchPath = new AStarSearch(startFloorMap).run(startX, startY, endX, endY); if (searchPath.hasSolution()) { // Create the event for the path and add it to our route lastFloorGridWidth = searchPath.gridLengthX; lastFloorGridHeight = searchPath.gridLengthY; return route.addPath(startFloor, searchPath.Solution.ToList()); } // falls out to third-tier search } else { if (endFloorMap == null) { return route.pathfindingError(); } // API call searching for all "vertical paths" (VPs) from startFloor directly to endFloor var directPois = _bluePrint.NavigationalPois .Where(v => v.Locations.Any(l => l.FloorPlan.Floor == startFloor) && v.Locations.Any(asd => asd.FloorPlan.Floor == endFloor)) .ToList(); if (directPois != null && directPois.Count > 0) { // Create the search objs here to be reused. Prevents duplicatoin of weighted-grid work. AStarSearch search1 = new AStarSearch(startFloorMap); AStarSearch search2 = new AStarSearch(endFloorMap); // Here we are looking for a valid path using only one stairs, elevator, etc. foreach (NavigationalPoi v in directPois) { // start floor FloorPlanLocation fp1 = v.Locations.Where(x => x.FloorPlan.Floor == startFloor).FirstOrDefault(); int vertX1 = (int) (fp1.XPos * startFloorPlan.ImageDataWidth); int vertY1 = (int) (fp1.YPos * startFloorPlan.ImageDataHeight); // find path from user position to the "stairs" search1.run(startX, startY, vertX1, vertY1); // if we can't get to the "stairs" from here, lets try the next way up if (!search1.hasSolution()) { continue; } // end floor FloorPlanLocation fp2 = v.Locations.Where(x => x.FloorPlan.Floor == endFloor).FirstOrDefault(); int vertX2 = (int) (fp2.XPos * endFloorPlan.ImageDataWidth); int vertY2 = (int) (fp2.YPos * endFloorPlan.ImageDataHeight); // find path from "stairs" to the ending position search2.run(vertX2, vertY2, endX, endY); // if we can't get to the "stairs" from here, we do not want to get to the return statement if (!search2.hasSolution()) { continue; } lastFloorGridWidth = search2.gridLengthX; lastFloorGridHeight = search2.gridLengthY; // Create solution return route .addPath(startFloor, search1.Solution.ToList()) .addVerticalTransisiton(v.Type, new CoordinateDto(vertX1, vertY1, search1.gridLengthX, search1.gridLengthY), startFloor,endFloor) .addPath(endFloor, search2.Solution.ToList()); } // falls out to third-tier search } } // the infamous third-tier search return route; }