private void PrepareDistanceMatrix()
{
distanceMatrix = new double[allPoints.Count, allPoints.Count];
for (int i = 0; i < allPoints.Count; i++)
{
for (int j = 0; j < allPoints.Count; j++)
{
distanceMatrix[i, j] = -1000;
}
}
foreach (GeoPath path in mapData["Paths"])
{
GeoPoint point1 = path.Path.ElementAt(0);
GeoPoint point2;
for (int i = 1; i < path.Path.Count; i++)
{
point2 = path.Path.ElementAt(i);
double dist = LatLongMath.GetDistance(pointsByID[point1.ID].Position, pointsByID[point2.ID].Position);
//Important since the path is both ways - bidirectional and the probe may occur from any side.
distanceMatrix[pointsIndex[point1.ID], pointsIndex[point2.ID]] = dist;
distanceMatrix[pointsIndex[point2.ID], pointsIndex[point1.ID]] = dist;
//The right sequence is to make the point1 = point2 otherwise the base always remains the same
point1 = point2;
}
}
unvisitedNodes = new List <GeoGraphPoint>(allPoints.Count);
}
public void SwitchToNavigator(GeoPlacemark destination)
{
int nodeID = int.MaxValue;
double tempDistance = double.PositiveInfinity, dummy;
foreach (GeoPoint point in allPoints)
{
if ((dummy = LatLongMath.GetDistance(currentPosition, point.Position)) < tempDistance)
{
tempDistance = dummy;
nodeID = point.ID;
}
}
if (nodeID != int.MaxValue)
{
pathWay = FindShortestPath(pointsByID[nodeID], destination);
}
GeoPlacemark place = pathWay.ElementAt(0);
place.Distance = LatLongMath.GetDistance(currentPosition, place.Position);
place.Orientation = LatLongMath.GetCompassBearing(currentPosition, place.Position);
Mode = AugmentedMode.Navigator;
}
public void PositionUpdate(GeoCoordinate position)
{
currentPosition = position;
if (Mode == AugmentedMode.Explorer)
{
UpdatePlacemarks();
}
else if (Mode == AugmentedMode.Navigator)
{
if (pathWay != null)
{
if (pathWay.Count > 0)
{
if (LatLongMath.GetDistance(position, pathWay.ElementAt(0).Position) <= distanceThreshold)
{
pathWay.RemoveAt(0);
}
}
if (pathWay.Count > 0)
{
GeoPlacemark place = pathWay.ElementAt(0);
place.Orientation = LatLongMath.GetCompassBearing(position, place.Position);
place.Distance = LatLongMath.GetDistance(position, place.Position);
}
}
}
}
private void UpdatePlacemarks()
{
foreach (GeoPlacemark placemark in allPlacemarks)
{
placemark.Orientation = LatLongMath.GetCompassBearing(currentPosition, placemark.Position);
placemark.Distance = LatLongMath.GetDistance(currentPosition, placemark.Position);
}
}
private void PrepareDisplayPositions()
{
foreach (GeoPlacemark place in visiblePlacemarks)
{
double mathAngle = 90 - Math.Abs(place.Angle);
double x = (place.Distance * Math.Cos(LatLongMath.ToRad(mathAngle)));
double y = (place.Distance * Math.Sin(LatLongMath.ToRad(mathAngle)));
x = (x * (displayWidth / maxDistance));
y = (place.Distance * (displayHeight / maxDistance));
if (place.Angle < 0)
{
x = -x;
}
place.DisplayX = (displayWidth / 2) + x;
place.DisplayY = (displayHeight - y);
}
}
public GeoPlacemark GetNextPoint(double compassHeading)
{
if (pathWay.Count > 0)
{
GeoPlacemark place = pathWay.ElementAt(0);
double newSpan = 80;
left = ((compassHeading - newSpan) + 360) % 360;
right = ((compassHeading + newSpan) + 360) % 360;
place.Angle = FindAngularDisplacement(place, compassHeading);
if (place.Angle != -180)
{
double mathAngle = 90 - Math.Abs(place.Angle);
double x = (place.Distance * Math.Cos(LatLongMath.ToRad(mathAngle)));
double y = (place.Distance * Math.Sin(LatLongMath.ToRad(mathAngle)));
x = (x * ((displayWidth / 3) / place.Distance));
y = (place.Distance * (displayHeight / (maxDistance / 3)));
if (place.Angle < 0)
{
x = -x;
}
place.DisplayX = (displayWidth / 2) + x;
//place.DisplayY = (displayHeight - y);
place.DisplayY = (displayHeight) - y;
}
return(place);
}
return(null);
}