/// <summary>
/// Returns the position after the given distance is travelled relative to the current position.
/// </summary>
/// <param name="distance"></param>
/// <returns></returns>
public GeoCoordinate PositionIn(Meter distance)
{
return _route.PositionAfter(_distanceFromStart + distance);
}
/// <summary>
/// Updates the tracker with the given location.
/// </summary>
/// <param name="location">The measured location.</param>
public void Track(GeoCoordinate location)
{
// project onto the route.
KeyValuePair<int, GeoCoordinate> projectedResult = this.ProjectOn(_route, location);
// set the current/route position.
_currentPosition = location;
_currentRoutePosition = projectedResult.Value;
// find the next instruction.
for (int instructionIdx = 0; instructionIdx < _instructions.Count; instructionIdx++)
{
Instruction instruction = _instructions[instructionIdx];
if (instruction.EntryIdx >= projectedResult.Key)
{ // stop here!
_nextInstructionIdx = instructionIdx;
break;
}
}
// calculate the distance to the next instruction.
GeoCoordinate previous = (new GeoCoordinate(_route.Entries[projectedResult.Key].Latitude,
_route.Entries[projectedResult.Key].Longitude));
Meter distance = previous.DistanceReal(projectedResult.Value);
for (int idx = projectedResult.Key; idx < _instructions[_nextInstructionIdx].EntryIdx - 1; idx++)
{
GeoCoordinate next = (new GeoCoordinate(_route.Entries[idx + 1].Latitude, _route.Entries[idx + 1].Longitude));
distance = distance + previous.DistanceReal(next);
previous = next;
}
_distanceNextInstruction = distance;
// calculate the distance from start.
previous = (new GeoCoordinate(_route.Entries[0].Latitude, _route.Entries[0].Longitude));
distance = 0;
for (int idx = 0; idx < projectedResult.Key - 1; idx++)
{
GeoCoordinate next = (new GeoCoordinate(_route.Entries[idx + 1].Latitude, _route.Entries[idx + 1].Longitude));
distance = distance + previous.DistanceReal(next);
previous = next;
}
distance = distance + previous.DistanceReal(projectedResult.Value);
_distanceFromStart = distance;
}
/// <summary>
/// Offset this coordinate with the given distance in meter along the provided direction.
/// </summary>
/// <param name="distance"></param>
/// <param name="direction"></param>
/// <returns></returns>
public GeoCoordinate OffsetWithDirection(Meter distance, DirectionEnum direction)
{
double radius_earth = Constants.RadiusOfEarth;
Radian ratio = distance.Value / radius_earth;
Radian oldLat = (Degree)(this.Latitude);
Radian oldLon = (Degree)(this.Longitude);
Radian bearing = (Degree)(int)direction;
Radian newLatitude = System.Math.Asin(
System.Math.Sin(oldLat.Value) *
System.Math.Cos(ratio.Value) +
System.Math.Cos(oldLat.Value) *
System.Math.Sin(ratio.Value) *
System.Math.Cos(bearing.Value));
Radian newLongitude = oldLon.Value + System.Math.Atan2(
System.Math.Sin(bearing.Value) *
System.Math.Sin(ratio.Value) *
System.Math.Cos(oldLat.Value),
System.Math.Cos(ratio.Value) -
System.Math.Sin(oldLat.Value) *
System.Math.Sin(newLatitude.Value));
// TODO: make this work in other hemispheres
var newLat = ((Degree)newLatitude).Value;
if(newLat > 180)
{
newLat = newLat - 360;
}
var newLon = ((Degree)newLongitude).Value;
if (newLon > 180)
{
newLon = newLon - 360;
}
return new GeoCoordinate(newLat, newLon);
}
/// <summary>
/// Offset this coordinate with the given distance in meter in both lat-lon directions.
/// The difference in distance will be sqrt(2) * meter.
/// </summary>
/// <param name="meter"></param>
/// <returns></returns>
public GeoCoordinate OffsetWithDistances(Meter meter)
{
GeoCoordinate offsetLat = new GeoCoordinate(this.Latitude + 0.1,
this.Longitude);
GeoCoordinate offsetLon = new GeoCoordinate(this.Latitude,
this.Longitude + 0.1);
Meter latDistance = offsetLat.DistanceReal(this);
Meter lonDistance = offsetLon.DistanceReal(this);
return new GeoCoordinate(this.Latitude + (meter.Value / latDistance.Value) * 0.1,
this.Longitude + (meter.Value / lonDistance.Value) * 0.1);
}
/// <summary>
/// Offsets this coordinate in a random direction.
/// </summary>
/// <param name="meter"></param>
/// <returns></returns>
public GeoCoordinate OffsetRandom(Meter meter)
{
return this.OffsetRandom(OsmSharp.Math.Random.StaticRandomGenerator.Get(), meter);
}
/// <summary>
/// Offsets this coordinate in a random direction.
/// </summary>
/// <param name="randomGenerator"></param>
/// <param name="meter"></param>
/// <returns></returns>
public GeoCoordinate OffsetRandom(IRandomGenerator randomGenerator, Meter meter)
{
GeoCoordinate offsetCoordinate = this.OffsetWithDistances(meter.Value /
System.Math.Sqrt(2));
double offsetLat = offsetCoordinate.Latitude - this.Latitude;
double offsetLon = offsetCoordinate.Longitude - this.Longitude;
offsetLat = (1.0 - randomGenerator.Generate(2.0)) * offsetLat;
offsetLon = (1.0 - randomGenerator.Generate(2.0)) * offsetLon;
return new GeoCoordinate(this.Latitude + offsetLat, this.Longitude + offsetLon);
}
/// <summary>
/// Calculates the closest point on the route relative to the given coordinate.
/// </summary>
/// <returns></returns>
public bool ProjectOn(GeoCoordinate coordinates, out GeoCoordinate projectedCoordinates, out int entryIndex, out Meter distanceFromStart, out Second timeFromStart)
{
double distance = double.MaxValue;
distanceFromStart = 0;
timeFromStart = 0;
double currentDistanceFromStart = 0;
projectedCoordinates = null;
entryIndex = -1;
// loop over all points and try to project onto the line segments.
GeoCoordinate projected;
double currentDistance;
var points = this.GetPoints();
for (int idx = 0; idx < points.Count - 1; idx++)
{
var line = new GeoCoordinateLine(points[idx], points[idx + 1], true, true);
var projectedPoint = line.ProjectOn(coordinates);
if (projectedPoint != null)
{ // there was a projected point.
projected = new GeoCoordinate(projectedPoint[1], projectedPoint[0]);
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{ // this point is closer.
projectedCoordinates = projected;
entryIndex = idx;
distance = currentDistance;
// calculate distance/time.
double localDistance = projected.DistanceReal(points[idx]).Value;
distanceFromStart = currentDistanceFromStart + localDistance;
if(this.HasTimes && idx > 0)
{ // there should be proper timing information.
double timeToSegment = this.Segments[idx].Time;
double timeToNextSegment = this.Segments[idx + 1].Time;
timeFromStart = timeToSegment + ((timeToNextSegment - timeToSegment) * (localDistance / line.LengthReal.Value));
}
}
}
// check first point.
projected = points[idx];
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{ // this point is closer.
projectedCoordinates = projected;
entryIndex = idx;
distance = currentDistance;
distanceFromStart = currentDistanceFromStart;
if (this.HasTimes)
{ // there should be proper timing information.
timeFromStart = this.Segments[idx].Time;
}
}
// update distance from start.
currentDistanceFromStart = currentDistanceFromStart + points[idx].DistanceReal(points[idx + 1]).Value;
}
// check last point.
projected = points[points.Count - 1];
currentDistance = coordinates.Distance(projected);
if (currentDistance < distance)
{ // this point is closer.
projectedCoordinates = projected;
entryIndex = points.Count - 1;
distance = currentDistance;
distanceFromStart = currentDistanceFromStart;
if (this.HasTimes)
{ // there should be proper timing information.
timeFromStart = this.Segments[points.Count - 1].Time;
}
}
return true;
}
/// <summary>
/// Returns an enumerable of route positions with the given interval between them.
/// </summary>
/// <param name="interval"></param>
/// <returns></returns>
public IEnumerable<GeoCoordinate> GetRouteEnumerable(Meter interval)
{
return new RouteEnumerable(this, interval);
}
/// <summary>
/// Calculates the position on the route after the given distance from the starting point.
/// </summary>
/// <param name="m"></param>
/// <returns></returns>
public GeoCoordinate PositionAfter(Meter m)
{
var distanceMeter = 0.0;
var points = this.GetPoints();
for (int idx = 0; idx < points.Count - 1; idx++)
{
var currentDistance = points[idx].DistanceReal(points[idx + 1]).Value;
if (distanceMeter + currentDistance >= m.Value)
{ // the current distance should be in this segment.
var segmentDistance = m.Value - distanceMeter;
var direction = points[idx + 1] - points[idx];
direction = direction * (segmentDistance / currentDistance);
var position = points[idx] + direction;
return new GeoCoordinate(position[1], position[0]);
}
distanceMeter += currentDistance;
}
return null;
}
/// <summary>
/// Calculates the closest point on the route relative to the given coordinate.
/// </summary>
/// <param name="coordinates"></param>
/// <param name="distanceFromStart"></param>
/// <returns></returns>
public bool ProjectOn(GeoCoordinate coordinates, out Meter distanceFromStart)
{
int entryIdx;
GeoCoordinate projectedCoordinates;
Second timeFromStart;
return this.ProjectOn(coordinates, out projectedCoordinates, out entryIdx, out distanceFromStart, out timeFromStart);
}
/// <summary>
/// Updates the tracker with the given location.
/// </summary>
/// <param name="location">The measured location.</param>
public void Track(GeoCoordinate location)
{
// set the current location.
_currentPosition = location;
// calculate the total distance.
var previous = new GeoCoordinate(_route.Segments[0].Latitude, _route.Segments[0].Longitude); ;
var totalDistance = 0.0;
for (int idx = 1; idx < _route.Segments.Length; idx++)
{
GeoCoordinate next = new GeoCoordinate(_route.Segments[idx].Latitude, _route.Segments[idx].Longitude);
totalDistance = totalDistance + previous.DistanceReal(next).Value;
previous = next;
}
double totalTime = _route.TotalTime;
// project onto the route.
int entryIdx;
_route.ProjectOn(_currentPosition, out _currentRoutePosition, out entryIdx, out _distanceFromStart, out _timeFromStart);
_distanceToEnd = totalDistance - _distanceFromStart;
_timeToEnd = totalTime - _timeFromStart.Value;
// find the next instruction.
_nextInstructionIdx = -1;
for (int instructionIdx = 0; instructionIdx < _instructions.Count; instructionIdx++)
{
var instruction = _instructions[instructionIdx];
if (instruction.LastSegmentIdx > entryIdx)
{ // stop here!
_nextInstructionIdx = instructionIdx;
break;
}
}
if(_nextInstructionIdx < 0)
{ // no instruction was found after the entryIdx: assume last instruction.
_nextInstructionIdx = _instructions.Count - 1;
}
// calculate the distance to the next instruction.
previous = _currentRoutePosition;
var distance = 0.0;
for (int idx = entryIdx + 1; idx <= _instructions[_nextInstructionIdx].LastSegmentIdx && idx < _route.Segments.Length; idx++)
{
var next = (new GeoCoordinate(_route.Segments[idx].Latitude, _route.Segments[idx].Longitude));
distance = distance + previous.DistanceReal(next).Value;
previous = next;
}
_distanceNextInstruction = distance;
}
/// <summary>
/// Tries to parse a distance measure from a given tag-value.
/// </summary>
/// <param name="s"></param>
/// <param name="result"></param>
/// <returns></returns>
public static bool TryParseLength(string s, out Meter result)
{
result = double.MaxValue;
if (Utilities.IsNullOrWhiteSpace(s))
return false;
Regex metresRegex = new Regex("^" + REGEX_DECIMAL + REGEX_UNIT_METERS + "$", RegexOptions.IgnoreCase);
Match metresMatch = metresRegex.Match(s);
if (metresMatch.Success)
{
result = double.Parse(metresMatch.Groups[1].Value, CultureInfo.InvariantCulture);
return true;
}
Regex feetInchesRegex = new Regex("^(\\d+)\\'(\\d+)\\\"$", RegexOptions.IgnoreCase);
Match feetInchesMatch = feetInchesRegex.Match(s);
if (feetInchesMatch.Success)
{
int feet = int.Parse(feetInchesMatch.Groups[1].Value, CultureInfo.InvariantCulture);
int inches = int.Parse(feetInchesMatch.Groups[2].Value, CultureInfo.InvariantCulture);
result = feet * 0.3048 + inches * 0.0254;
return true;
}
return false;
}
/// <summary>
/// Searches for a max width tag and returns the associated value.
///
/// http://wiki.openstreetmap.org/wiki/Key:maxwidth
/// </summary>
/// <param name="tags">The tags to search.</param>
/// <param name="result"></param>
/// <returns></returns>
public static bool TryGetMaxWidth(this TagsCollectionBase tags, out Meter result)
{
result = double.MaxValue;
string tagValue;
if (tags == null || !tags.TryGetValue("maxwidth", out tagValue) || Utilities.IsNullOrWhiteSpace(tagValue))
return false;
return TagExtensions.TryParseLength(tagValue, out result);
}
/// <summary>
/// Searches for a max length tag and returns the associated value.
///
/// http://wiki.openstreetmap.org/wiki/Key:maxlength
/// </summary>
/// <param name="tags">The tags to search.</param>
/// <param name="result"></param>
/// <returns></returns>
public static bool TryGetMaxLength(this IDictionary<string, string> tags, out Meter result)
{
result = double.MaxValue;
string tagValue;
if (tags == null || !tags.TryGetValue("maxlength", out tagValue) || string.IsNullOrWhiteSpace(tagValue))
return false;
return TagExtensions.TryParseLength(tagValue, out result);
}
/// <summary>
/// Searches for a max height tag and returns the associated value.
///
/// http://wiki.openstreetmap.org/wiki/Maxheight
/// </summary>
/// <param name="tags">The tags to search.</param>
/// <param name="result"></param>
/// <returns></returns>
public static bool TryGetMaxHeight(this TagsCollection tags, out Meter result)
{
result = double.MaxValue;
string tagValue;
if (tags == null || !tags.TryGetValue("maxheight", out tagValue) || string.IsNullOrWhiteSpace(tagValue))
return false;
return TagExtensions.TryParseLength(tagValue, out result);
}
