/// <summary>
///
/// </summary>
/// <param name="p0">First point on route</param>
/// <param name="q0">First point on map</param>
/// <param name="p1">Second point on route</param>
/// <param name="q1">Second point on map</param>
/// <param name="p2">Third point on route</param>
/// <param name="q2">Third point on map</param>
/// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param>
/// <returns></returns>
public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, PointD p2, PointD q2, GeneralMatrix fallbackMatrix)
{
try
{
var m = new GeneralMatrix(3, 3);
m.SetElement(0, 0, p0.X);
m.SetElement(0, 1, p0.Y);
m.SetElement(0, 2, 1.0);
m.SetElement(1, 0, p1.X);
m.SetElement(1, 1, p1.Y);
m.SetElement(1, 2, 1.0);
m.SetElement(2, 0, p2.X);
m.SetElement(2, 1, p2.Y);
m.SetElement(2, 2, 1.0);
var v1 = new GeneralMatrix(3, 1);
v1.SetElement(0, 0, q0.X);
v1.SetElement(1, 0, q1.X);
v1.SetElement(2, 0, q2.X);
var t1 = m.Inverse() * v1;
var v2 = new GeneralMatrix(3, 1);
v2.SetElement(0, 0, q0.Y);
v2.SetElement(1, 0, q1.Y);
v2.SetElement(2, 0, q2.Y);
var t2 = m.Inverse() * v2;
var v3 = new GeneralMatrix(3, 1);
v3.SetElement(0, 0, 1.0);
v3.SetElement(1, 0, 1.0);
v3.SetElement(2, 0, 1.0);
var t3 = m.Inverse() * v3;
var t = new GeneralMatrix(3, 3);
t.SetElement(0, 0, t1.GetElement(0, 0));
t.SetElement(0, 1, t1.GetElement(1, 0));
t.SetElement(0, 2, t1.GetElement(2, 0));
t.SetElement(1, 0, t2.GetElement(0, 0));
t.SetElement(1, 1, t2.GetElement(1, 0));
t.SetElement(1, 2, t2.GetElement(2, 0));
t.SetElement(2, 0, t3.GetElement(0, 0));
t.SetElement(2, 1, t3.GetElement(1, 0));
t.SetElement(2, 2, t3.GetElement(2, 0));
return(t);
}
catch (Exception)
{
return((GeneralMatrix)fallbackMatrix.Clone());
}
}
/// <summary>
///
/// </summary>
/// <param name="p0">First point on route, in projected (metric) coordinates relative to projection origin</param>
/// <param name="q0">First point on map, in pixels</param>
/// <param name="p1">Second point on route, in projected (metric) coordinates relative to projection origin</param>
/// <param name="q1">Second point on map, in pixels</param>
/// <param name="fallbackMatrix">Matrix to use if calculation fails due to singular matrix</param>
/// <param name="useRotation">If true, assumes orthogonal map and calculates scale and rotation. If false, calculates different scale in x and y directions and no rotation.</param>
/// <returns></returns>
public static GeneralMatrix CalculateTransformationMatrix(PointD p0, PointD q0, PointD p1, PointD q1, GeneralMatrix fallbackMatrix, bool useRotation)
{
try
{
if (useRotation)
{
// note that we need to mirror y pixel value in x axis
double angleDifferece = GetAngleR(p1 - p0, new PointD(q1.X, -q1.Y) - new PointD(q0.X, -q0.Y));
double lengthQ = DistancePointToPoint(q0, q1);
double lengthP = DistancePointToPoint(p0, p1);
double scaleFactor = lengthP == 0 ? 0 : lengthQ / lengthP;
double cos = Math.Cos(angleDifferece);
double sin = Math.Sin(angleDifferece);
// translation to origo in metric space
var a = new GeneralMatrix(3, 3);
a.SetElement(0, 0, 1);
a.SetElement(0, 1, 0);
a.SetElement(0, 2, -p0.X);
a.SetElement(1, 0, 0);
a.SetElement(1, 1, 1);
a.SetElement(1, 2, -p0.Y);
a.SetElement(2, 0, 0);
a.SetElement(2, 1, 0);
a.SetElement(2, 2, 1);
// rotation
var b = new GeneralMatrix(3, 3);
b.SetElement(0, 0, cos);
b.SetElement(0, 1, -sin);
b.SetElement(0, 2, 0);
b.SetElement(1, 0, sin);
b.SetElement(1, 1, cos);
b.SetElement(1, 2, 0);
b.SetElement(2, 0, 0);
b.SetElement(2, 1, 0);
b.SetElement(2, 2, 1);
// scaling, note that we need to mirror y scale around x axis
var c = new GeneralMatrix(3, 3);
c.SetElement(0, 0, scaleFactor);
c.SetElement(0, 1, 0);
c.SetElement(0, 2, 0);
c.SetElement(1, 0, 0);
c.SetElement(1, 1, -scaleFactor);
c.SetElement(1, 2, 0);
c.SetElement(2, 0, 0);
c.SetElement(2, 1, 0);
c.SetElement(2, 2, 1);
// translation from origo to pixel space
var d = new GeneralMatrix(3, 3);
d.SetElement(0, 0, 1);
d.SetElement(0, 1, 0);
d.SetElement(0, 2, q0.X);
d.SetElement(1, 0, 0);
d.SetElement(1, 1, 1);
d.SetElement(1, 2, q0.Y);
d.SetElement(2, 0, 0);
d.SetElement(2, 1, 0);
d.SetElement(2, 2, 1);
return(d * c * b * a);
}
else // useRotation == false
{
var m1 = new GeneralMatrix(2, 2);
m1.SetElement(0, 0, p0.X);
m1.SetElement(0, 1, 1);
m1.SetElement(1, 0, p1.X);
m1.SetElement(1, 1, 1);
var v1 = new GeneralMatrix(2, 1);
v1.SetElement(0, 0, q0.X);
v1.SetElement(1, 0, q1.X);
var t1 = m1.Inverse() * v1;
var m2 = new GeneralMatrix(2, 2);
m2.SetElement(0, 0, p0.Y);
m2.SetElement(0, 1, 1);
m2.SetElement(1, 0, p1.Y);
m2.SetElement(1, 1, 1);
var v2 = new GeneralMatrix(2, 1);
v2.SetElement(0, 0, q0.Y);
v2.SetElement(1, 0, q1.Y);
var t2 = m2.Inverse() * v2;
var t = new GeneralMatrix(3, 3);
t.SetElement(0, 0, t1.GetElement(0, 0));
t.SetElement(0, 1, 0);
t.SetElement(0, 2, t1.GetElement(1, 0));
t.SetElement(1, 0, 0);
t.SetElement(1, 1, t2.GetElement(0, 0));
t.SetElement(1, 2, t2.GetElement(1, 0));
t.SetElement(2, 0, 0);
t.SetElement(2, 1, 0);
t.SetElement(2, 2, 1);
return(t);
}
}
catch (Exception)
{
return((GeneralMatrix)fallbackMatrix.Clone());
}
}
