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