/// <summary>
/// Distance in metres between two points expressed as longitude/latitude
/// </summary>
/// <param name="longLat0">longitude and latitude for first point</param>
/// <param name="longLat1">longitude and latitude for second point</param>
/// <returns></returns>
public static double DistancePointToPointLongLat(LongLat longLat0, LongLat longLat1)
{
// use spherical coordinates: rho, phi, theta
const double rho = 6378200; // earth radius in metres
double sinPhi0 = Math.Sin(0.5 * Math.PI + longLat0.Latitude / 180.0 * Math.PI);
double cosPhi0 = Math.Cos(0.5 * Math.PI + longLat0.Latitude / 180.0 * Math.PI);
double sinTheta0 = Math.Sin(longLat0.Longitude / 180.0 * Math.PI);
double cosTheta0 = Math.Cos(longLat0.Longitude / 180.0 * Math.PI);
double sinPhi1 = Math.Sin(0.5 * Math.PI + longLat1.Latitude / 180.0 * Math.PI);
double cosPhi1 = Math.Cos(0.5 * Math.PI + longLat1.Latitude / 180.0 * Math.PI);
double sinTheta1 = Math.Sin(longLat1.Longitude / 180.0 * Math.PI);
double cosTheta1 = Math.Cos(longLat1.Longitude / 180.0 * Math.PI);
var p0 = new GeneralMatrix(3, 1);
p0.SetElement(0, 0, rho * sinPhi0 * cosTheta0);
p0.SetElement(1, 0, rho * sinPhi0 * sinTheta0);
p0.SetElement(2, 0, rho * cosPhi0);
var p1 = new GeneralMatrix(3, 1);
p1.SetElement(0, 0, rho * sinPhi1 * cosTheta1);
p1.SetElement(1, 0, rho * sinPhi1 * sinTheta1);
p1.SetElement(2, 0, rho * cosPhi1);
double distance = DistancePointToPoint(p0, p1);
return(distance);
}
private static LongLat ReadLongLat(BinaryReader br)
{
var longLat = new LongLat();
longLat.Longitude = (double)br.ReadInt32() / 3600000;
longLat.Latitude = (double)br.ReadInt32() / 3600000;
return(longLat);
}
public bool Equals(LongLat other)
{
if (other == null)
{
return(false);
}
return(longitude == other.Longitude && latitude == other.Latitude);
}
public static GeneralMatrix To3x1Matrix(LongLat longLat)
{
GeneralMatrix m = new GeneralMatrix(3, 1);
m.SetElement(0, 0, longLat.Longitude);
m.SetElement(1, 0, longLat.Latitude);
m.SetElement(2, 0, 1);
return(m);
}
/// <summary>
/// Speed in metres per second
/// </summary>
/// <param name="longLat0">longitude and latitude for first point</param>
/// <param name="longLat1">longitude and latitude for second point</param>
/// <param name="t0">time for first point</param>
/// <param name="t1">time for first point</param>
/// <returns></returns>
public static double CalculateSpeed(LongLat longLat0, LongLat longLat1, DateTime t0, DateTime t1)
{
double distance = DistancePointToPointLongLat(longLat0, longLat1);
TimeSpan timeSpan = t1.Subtract(t0);
double speed = distance / timeSpan.TotalSeconds;
return(speed);
}
public static byte[] GetExifGpsPosition(LongLat longLat)
{
var lng = BitConverter.GetBytes(Convert.ToInt32(longLat.Longitude * 3600000));
var lat = BitConverter.GetBytes(Convert.ToInt32(longLat.Latitude * 3600000));
var x = BitConverter.GetBytes((int)int.MinValue + 1);
var result = new byte[8];
lng.CopyTo(result, 0);
lat.CopyTo(result, 4);
return(result);
}
/// <summary>
/// Applies an ortographic projection to the coordinate.
/// http://en.wikipedia.org/wiki/Orthographic_projection_%28cartography%29
/// </summary>
/// <param name="projectionOrigin">The origin longitude/latitude coordinate of the projection</param>
/// <returns>A point that gives the distance in meters from the projection origin</returns>
public PointD Project(LongLat projectionOrigin)
{
const double rho = 6378200; // earth radius in metres
double lambda0 = projectionOrigin.Longitude * Math.PI / 180.0;
double phi0 = projectionOrigin.Latitude * Math.PI / 180.0;
double lambda = longitude * Math.PI / 180.0;
double phi = latitude * Math.PI / 180.0;
return(new PointD(rho * Math.Cos(phi) * Math.Sin(lambda - lambda0),
rho * (Math.Cos(phi0) * Math.Sin(phi) - Math.Sin(phi0) * Math.Cos(phi) * Math.Cos(lambda - lambda0))));
}
protected Document(SerializationInfo info, StreamingContext context)
{
Map = (Map)(info.GetValue("map", typeof(Map)));
settings = (DocumentSettings)(info.GetValue("settings", typeof(DocumentSettings)));
sessions = (SessionCollection)(info.GetValue("sessions", typeof(SessionCollection)));
projectionOrigin = (LongLat)(info.GetValue("projectionOrigin", typeof(LongLat)));
// todo: how handle non-existing properties field?
try
{
properties = (DocumentProperties)(info.GetValue("properties", typeof(DocumentProperties)));
}
catch (Exception)
{ }
}
public Transformation(LongLatBox longLatBox, Size imageSize)
{
// calculate projection origin
ProjectionOrigin = new LongLat((longLatBox.East + longLatBox.West) / 2,
(longLatBox.North + longLatBox.South) / 2);
// get image corners from kml file
var imageCornerLongLats = longLatBox.GetRotatedBoxCornerLongLats();
// project them on flat surface
var projectedImageCorners = new Dictionary <Corner, PointD>();
projectedImageCorners[Corner.NorthWest] = imageCornerLongLats[Corner.NorthWest].Project(ProjectionOrigin);
projectedImageCorners[Corner.SouthEast] = imageCornerLongLats[Corner.SouthEast].Project(ProjectionOrigin);
// calculate transformation matrix
TransformationMatrix = LinearAlgebraUtil.CalculateTransformationMatrix(
projectedImageCorners[Corner.NorthWest], new PointD(0, 0),
projectedImageCorners[Corner.SouthEast], new PointD(imageSize.Width - 1, imageSize.Height - 1), null, true);
}
public static LongLat Deproject(PointD coordinate, LongLat projectionOrigin)
{
if (LinearAlgebraUtil.DistancePointToPoint(coordinate, new PointD(0, 0)) < 0.0000001)
{
return(new LongLat(projectionOrigin.Longitude, projectionOrigin.Latitude));
}
const double r = 6378200; // earth radius in metres
var longLat = new LongLat();
var rho = Math.Sqrt(coordinate.X * coordinate.X + coordinate.Y * coordinate.Y);
var c = Math.Asin(rho / r);
var lambda0 = projectionOrigin.Longitude * Math.PI / 180.0;
var phi1 = projectionOrigin.Latitude * Math.PI / 180.0;
longLat.Latitude =
Math.Asin(Math.Cos(c) * Math.Sin(phi1) +
(coordinate.Y * Math.Sin(c) * Math.Cos(phi1) / rho)) / Math.PI * 180.0;
longLat.Longitude = (lambda0 +
Math.Atan(coordinate.X * Math.Sin(c) /
(rho * Math.Cos(phi1) * Math.Cos(c) -
coordinate.Y * Math.Sin(phi1) * Math.Sin(c)))) / Math.PI * 180.0;
return(longLat);
}
public Dictionary <Corner, LongLat> GetRotatedBoxCornerLongLats()
{
var rotation = -Rotation;
var corners = new Dictionary <Corner, LongLat>();
corners[Corner.NorthEast] = new LongLat(East, North);
corners[Corner.NorthWest] = new LongLat(West, North);
corners[Corner.SouthWest] = new LongLat(West, South);
corners[Corner.SouthEast] = new LongLat(East, South);
var projectionOrigin = new LongLat((East + West) / 2, (North + South) / 2);
var projectedMapCenter = projectionOrigin.Project(projectionOrigin);
var projectedCorners = new Dictionary <Corner, PointD>();
projectedCorners[Corner.NorthEast] = corners[Corner.NorthEast].Project(projectionOrigin);
projectedCorners[Corner.NorthWest] = corners[Corner.NorthWest].Project(projectionOrigin);
projectedCorners[Corner.SouthWest] = corners[Corner.SouthWest].Project(projectionOrigin);
projectedCorners[Corner.SouthEast] = corners[Corner.SouthEast].Project(projectionOrigin);
var projectedRotatedCorners = new Dictionary <Corner, PointD>();
projectedRotatedCorners[Corner.NorthEast] = LinearAlgebraUtil.Rotate(projectedCorners[Corner.NorthEast], projectedMapCenter, rotation);
projectedRotatedCorners[Corner.NorthWest] = LinearAlgebraUtil.Rotate(projectedCorners[Corner.NorthWest], projectedMapCenter, rotation);
projectedRotatedCorners[Corner.SouthWest] = LinearAlgebraUtil.Rotate(projectedCorners[Corner.SouthWest], projectedMapCenter, rotation);
projectedRotatedCorners[Corner.SouthEast] = LinearAlgebraUtil.Rotate(projectedCorners[Corner.SouthEast], projectedMapCenter, rotation);
var rotatedCorners = new Dictionary <Corner, LongLat>();
rotatedCorners[Corner.NorthWest] = LongLat.Deproject(projectedRotatedCorners[Corner.NorthWest], projectionOrigin);
rotatedCorners[Corner.NorthEast] = LongLat.Deproject(projectedRotatedCorners[Corner.NorthEast], projectionOrigin);
rotatedCorners[Corner.SouthWest] = LongLat.Deproject(projectedRotatedCorners[Corner.SouthWest], projectionOrigin);
rotatedCorners[Corner.SouthEast] = LongLat.Deproject(projectedRotatedCorners[Corner.SouthEast], projectionOrigin);
return(rotatedCorners);
}
public static GeneralMatrix CreateInitialTransformationMatrix(Route route, Size mapSize, LongLat projectionOrigin)
{
// create initial adjustment: route should fit in the 75% inner rectangle of the map
RectangleD routeRectangle = route.BoundingProjectedRectangle(projectionOrigin);
RectangleD mapRectangle = new RectangleD(1.0 / 8.0 * mapSize.Width, 1.0 / 8.0 * mapSize.Height, 3.0 / 4.0 * mapSize.Width, 3.0 / 4.0 * mapSize.Height);
// check width/height ratio for each of the rectangles, and adjust the map rectangle to have the same ratio as the route rectangle
double routeRatio = routeRectangle.Width / routeRectangle.Height;
double mapRatio = mapRectangle.Width / mapRectangle.Height;
if (mapRatio < routeRatio)
{
// too narrow
mapRectangle = new RectangleD(mapRectangle.Left, mapRectangle.Center.Y - mapRectangle.Width / routeRatio / 2.0, mapRectangle.Width, mapRectangle.Width / routeRatio);
}
else
{
// too wide
mapRectangle = new RectangleD(mapRectangle.Center.X - mapRectangle.Height * routeRatio / 2.0, mapRectangle.Top, mapRectangle.Height * routeRatio, mapRectangle.Height);
}
GeneralMatrix t = LinearAlgebraUtil.CalculateTransformationMatrix(routeRectangle.LowerLeft, mapRectangle.UpperLeft, routeRectangle.UpperRight, mapRectangle.LowerRight, null, false);
return(t);
}
/// <summary>
/// Distance in longitude/latitude units, not meters
/// </summary>
/// <param name="p0"></param>
/// <param name="p1"></param>
/// <returns></returns>
public static double DistancePointToPoint(LongLat p0, LongLat p1)
{
return(Math.Sqrt((p1.Longitude - p0.Longitude) * (p1.Longitude - p0.Longitude) + (p1.Latitude - p0.Latitude) * (p1.Latitude - p0.Latitude)));
}
public Transformation(GeneralMatrix transformationMatrix, LongLat projectionOrigin)
{
TransformationMatrix = transformationMatrix;
ProjectionOrigin = projectionOrigin;
}
private static Session ReadSession(BinaryReader reader, int length)
{
List <DateTime> mapReadingList = null;
Route route = null;
HandleCollection handles = null;
LongLat projectionOrigin = null;
LapCollection laps = null;
var startPos = reader.BaseStream.Position;
SessionInfo sessionInfo = null;
DateTime lastTime;
while (reader.BaseStream.Position < startPos + length)
{
var tag = (Tags)reader.ReadByte();
var tagLength = Convert.ToInt32(reader.ReadUInt32());
switch (tag)
{
case Tags.Route:
var attributes = reader.ReadUInt16();
var extraWaypointAttributesLength = reader.ReadUInt16();
var routeSegments = new List <RouteSegment>();
var segmentCount = reader.ReadUInt32();
lastTime = DateTime.MinValue;
for (var i = 0; i < segmentCount; i++)
{
var rs = new RouteSegment();
var waypointCount = reader.ReadUInt32();
for (var j = 0; j < waypointCount; j++)
{
var w = new Waypoint();
w.LongLat = ReadLongLat(reader);
w.Time = ReadTime(lastTime, reader);
lastTime = w.Time;
if ((attributes & (UInt16)WaypointAttribute.HeartRate) == (UInt16)WaypointAttribute.HeartRate)
{
w.HeartRate = reader.ReadByte();
}
if ((attributes & (UInt16)WaypointAttribute.Altitude) == (UInt16)WaypointAttribute.Altitude)
{
w.Altitude = reader.ReadInt16();
}
reader.BaseStream.Position += extraWaypointAttributesLength; // for forward compatibility
rs.Waypoints.Add(w);
}
routeSegments.Add(rs);
}
route = new Route(routeSegments);
break;
case Tags.Handles:
handles = new HandleCollection();
var handleCount = reader.ReadUInt32();
var handleMarkerDrawer = SessionSettings.CreateDefaultMarkerDrawers()[MarkerType.Handle];
for (var i = 0; i < handleCount; i++)
{
var handle = new Handle();
// transformation matrix
handle.TransformationMatrix = new GeneralMatrix(3, 3);
for (var j = 0; j < 9; j++)
{
handle.TransformationMatrix.SetElement(j / 3, j % 3, reader.ReadDouble());
}
// parameterized location
var segmentIndex = Convert.ToInt32(reader.ReadUInt32());
var value = reader.ReadDouble();
handle.ParameterizedLocation = new ParameterizedLocation(segmentIndex, value);
// pixel location
handle.Location = new PointD(reader.ReadDouble(), reader.ReadDouble());
// type
handle.Type = (Handle.HandleType)reader.ReadInt16();
// use default marker drawer
handle.MarkerDrawer = handleMarkerDrawer;
handles.Add(handle);
}
break;
case Tags.ProjectionOrigin:
projectionOrigin = ReadLongLat(reader);
break;
case Tags.Laps:
laps = new LapCollection();
var lapCount = reader.ReadUInt32();
for (var i = 0; i < lapCount; i++)
{
var lap = new Lap();
lap.Time = DateTime.FromBinary(reader.ReadInt64());
lap.LapType = (LapType)reader.ReadByte();
laps.Add(lap);
}
break;
case Tags.SessionInfo:
sessionInfo = new SessionInfo();
sessionInfo.Person = new SessionPerson();
sessionInfo.Person.Name = ReadString(reader);
sessionInfo.Person.Club = ReadString(reader);
sessionInfo.Person.Id = reader.ReadUInt32();
sessionInfo.Description = ReadString(reader);
// when more fields are added, check so that tagLength is not passed
break;
case Tags.MapReadingInfo:
mapReadingList = new List <DateTime>();
lastTime = DateTime.MinValue;
var startPosition = reader.BaseStream.Position;
while (reader.BaseStream.Position - startPosition < tagLength)
{
var time = ReadTime(lastTime, reader);
mapReadingList.Add(time);
lastTime = time;
}
break;
default:
reader.BaseStream.Position += tagLength;
break;
}
}
if (mapReadingList != null && route != null)
{
route = new Route(Route.AddMapReadingWaypoints(route.Segments, mapReadingList));
}
var session = new Session(
route,
laps,
new Size(0, 0),
handles != null && handles.Count > 0 ? handles[0].TransformationMatrix : null,
projectionOrigin,
new SessionSettings());
if (handles != null)
{
foreach (var h in handles)
{
session.AddHandle(h);
}
}
if (sessionInfo != null)
{
session.SessionInfo = sessionInfo;
}
return(session);
}
/// <summary>
/// Creating a new document using the specified map, route, laps, initial transformation matrix, projection origin and document settings, and adding one new session with the specified route and laps.
/// </summary>
/// <param name="map"></param>
/// <param name="route"></param>
/// <param name="laps"></param>
/// <param name="initialTransformationMatrix"></param>
/// <param name="projectionOrigin"></param>
/// <param name="settings"></param>
public Document(Map map, Route route, LapCollection laps, GeneralMatrix initialTransformationMatrix, LongLat projectionOrigin, DocumentSettings settings)
{
Map = map;
sessions.Add(new Session(route, laps, map.Image.Size, initialTransformationMatrix, projectionOrigin, settings.DefaultSessionSettings));
this.settings = settings;
UpdateDocumentToCurrentVersion(this);
}
/// <summary>
/// Converts a map image pixel coordinate to a longitude and latitude coordinate
/// </summary>
/// <param name="mapImagePosition">Map pixel coordinate, referring to unzoomed map without any borders and image header</param>
/// <returns></returns>
public LongLat GetLongLatForMapImagePosition(PointD mapImagePosition, GeneralMatrix averageTransformationMatrixInverse)
{
var projectedPosition = LinearAlgebraUtil.Transform(mapImagePosition, averageTransformationMatrixInverse);
return(LongLat.Deproject(projectedPosition, ProjectionOrigin));
}
/// <summary>
/// Using linear least squares algorithm described at http://en.wikipedia.org/wiki/Linear_least_squares
/// </summary>
/// <returns></returns>
public Transformation CalculateAverageTransformation()
{
var averageProjectionOrigin = new LongLat();
foreach (var session in this)
{
averageProjectionOrigin += session.ProjectionOrigin / Count;
}
if (Count == 0)
{
return(null);
}
var n = 4;
var XtX = new GeneralMatrix(n, n);
var Xty = new GeneralMatrix(n, 1);
var numberOfUnknowns = 0;
foreach (var session in this)
{
var m = session.Handles.Length;
if (m < 2)
{
continue;
}
numberOfUnknowns += m;
var startDistance = session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Route.FirstPL).Value;
for (var i = 0; i < m; i++)
{
var longLat = session.Route.GetLocationFromParameterizedLocation(session.Handles[i].ParameterizedLocation);
var p = longLat.Project(averageProjectionOrigin); // projected point on earth (metres)
var q = session.Handles[i].Location; // point on map image (pixels)
var endDistance = (i != m - 1)
? (session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Handles[i].ParameterizedLocation).Value +
session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Handles[i + 1].ParameterizedLocation).Value) / 2
: session.Route.GetAttributeFromParameterizedLocation(WaypointAttribute.Distance, session.Route.LastPL).Value;
var w = endDistance - startDistance; // weight
startDistance = endDistance;
XtX.SetElement(0, 0, XtX.GetElement(0, 0) + w * (p.X * p.X + p.Y * p.Y));
XtX.SetElement(0, 2, XtX.GetElement(0, 2) + w * p.X);
XtX.SetElement(0, 3, XtX.GetElement(0, 3) - w * p.Y);
XtX.SetElement(1, 1, XtX.GetElement(1, 1) + w * (p.X * p.X + p.Y * p.Y));
XtX.SetElement(1, 2, XtX.GetElement(1, 2) + w * p.Y);
XtX.SetElement(1, 3, XtX.GetElement(1, 3) + w * p.X);
XtX.SetElement(2, 0, XtX.GetElement(2, 0) + w * p.X);
XtX.SetElement(2, 1, XtX.GetElement(2, 1) + w * p.Y);
XtX.SetElement(2, 2, XtX.GetElement(2, 2) + w);
XtX.SetElement(3, 0, XtX.GetElement(3, 0) - w * p.Y);
XtX.SetElement(3, 1, XtX.GetElement(3, 1) + w * p.X);
XtX.SetElement(3, 3, XtX.GetElement(3, 3) + w);
Xty.SetElement(0, 0, Xty.GetElement(0, 0) + w * (q.X * p.X - q.Y * p.Y));
Xty.SetElement(1, 0, Xty.GetElement(1, 0) + w * (q.X * p.Y + q.Y * p.X));
Xty.SetElement(2, 0, Xty.GetElement(2, 0) + w * q.X);
Xty.SetElement(3, 0, Xty.GetElement(3, 0) + w * q.Y);
}
}
var T = new GeneralMatrix(3, 3);
if (numberOfUnknowns == 0)
{
T = this[0].InitialTransformationMatrix;
}
else if (numberOfUnknowns == 1)
{
T = this[0].Handles[0].TransformationMatrix;
}
else
{
var B = XtX.QRD().Solve(Xty);
T.SetElement(0, 0, B.GetElement(0, 0));
T.SetElement(0, 1, B.GetElement(1, 0));
T.SetElement(0, 2, B.GetElement(2, 0));
T.SetElement(1, 0, B.GetElement(1, 0));
T.SetElement(1, 1, -B.GetElement(0, 0));
T.SetElement(1, 2, B.GetElement(3, 0));
T.SetElement(2, 0, 0);
T.SetElement(2, 1, 0);
T.SetElement(2, 2, 1);
}
return(new Transformation(T, averageProjectionOrigin));
}
private static void WriteLongLat(LongLat longLat, BinaryWriter bw)
{
bw.Write(Convert.ToInt32(longLat.Longitude * 3600000));
bw.Write(Convert.ToInt32(longLat.Latitude * 3600000));
}
