// Here define A,B, and H Matrice
private Joint kalmanFilterFull(int pos)
{
// Prepare
//GeneralMatrix r = GeneralMatrix.Identity(3, 3);
//R = r.MultiplyEquals(0.01);
// Set r from Deviation Standard
GeneralMatrix r = GeneralMatrix.Identity(3, 3);
r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X
r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z
r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y
R = r;
// Set estimation from coordinate average in frame buffer
GeneralMatrix Z = GeneralMatrix.Random(3, 1);
Z.SetElement(0, 0, jointAveragePart[pos, 0]);
Z.SetElement(1, 0, jointAveragePart[pos, 1]);
Z.SetElement(2, 0, jointAveragePart[pos, 2]);
// Predict
GeneralMatrix Xp = F * Xk[pos];
GeneralMatrix Pp = F * P[pos] * F.Transpose() + Q;
// Measurement update ( Correction )
K = Pp * H.Transpose() * (H * Pp * H.Transpose() + R).Inverse();
Xk[pos] = Xp + (K * (Z - (H * Xp)));
GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension);
P[pos] = (I - (K * H)) * Pp;
return(convertMatrixTojoint(Xk[pos]));
}
/// <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);
}
/******************* Method Kalman Filter ******************/
// Variable A,B, and H we will asumtion this is a constant value
// Most probably is 1.
private Joint kalmanFilter(GeneralMatrix z, int pos)
{
// Prepare
Joint join = new Joint();
GeneralMatrix r = GeneralMatrix.Identity(3, 3);
r.SetElement(0, 0, Rv[pos, 0]); // Variance Base on Joint Type , X
r.SetElement(1, 1, Rv[pos, 1]); // Variance Base on Joint Type , Z
r.SetElement(2, 2, Rv[pos, 2]); // Variance Base on Joint Type , Y
R = r;
// Predict
GeneralMatrix Xp = Xk[pos];
GeneralMatrix Pp = P[pos];
// Measurement Update (correction
GeneralMatrix s = Pp + R;
K = Pp * s.Inverse(); // Calculate Kalman Gain
Xk[pos] = Xp + (K * (z - Xp));
GeneralMatrix I = GeneralMatrix.Identity(Pp.RowDimension, Pp.ColumnDimension);
P[pos] = (I - K) * Pp;
estimationX = Xk[pos].GetElement(0, 0);
estimationY = Xk[pos].GetElement(1, 0);
estimationZ = Xk[pos].GetElement(2, 0);
join.Position.X = (float)estimationX;
join.Position.Y = (float)estimationY;
join.Position.Z = (float)estimationZ;
return(join);
}
/// <summary>
/// (
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
public static GeneralMatrix ExpandUtility(GeneralMatrix matrix)
{
double val = 0.0;
int n = matrix.RowDimension;
int m = matrix.ColumnDimension;
if (n != m)
{
throw new ArgumentException("Criteria matrix must be symmetrical");
}
GeneralMatrix newMatrix = matrix.Transpose();
//for all transposed elements calculate their inverse values
//set diagonal elements to 0
for (int i = 0; i < n; i++)
{
for (int j = 0; j <= i; j++)
{
val = newMatrix.GetElement(i, j);
if (val == 0.0)
{
throw new ArgumentException("Criteria comparison values van't be 0");
}
newMatrix.SetElement(i, j, 1 / val);
if (i == j)
{
newMatrix.SetElement(i, j, 0);
}
}
}
//add transposed, inverse matrix to the original one
//create fully expanded matrix
return(newMatrix.Add(matrix));
}
public void Visit(Compiled.Gp elem)
{
GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length);
for (int i = 0; i < elem.Terms.Length; ++i)
{
cur.SetElement(0, i, ValueOf(elem.Terms[i]));
}
elem.Value = elem.Gpr.Evaluate(cur).GetElement(0, 0);
for (int i = 0; i < elem.Inputs.Length; ++i)
{
if (i < elem.dc)
{
//elem.Inputs[i].Weight = elem.Gpr.PartialDerivative(cur, i);
cur.SetElement(0, i, cur.GetElement(0, i) + 0.1);
elem.Inputs[i].Weight = (elem.Gpr.Evaluate(cur).GetElement(0, 0) - elem.Value) / 0.1;
cur.SetElement(0, i, cur.GetElement(0, i) - 0.1);
}
else
{
elem.Inputs[i].Weight = 0;
}
}
//Here Starts a dirty Hack
/*double sum=0;
* for(int n=0; n<elem.Inputs.Length; n++) {
* sum += elem.Inputs[n].Weight;
* }
* if(sum < 0.00001) {
* double val = elem.Value;
*
* double minDist = Double.MaxValue;
* int iMin=-1;
* double dist = 0, tmp;
* for(int n=0; n<elem.X.RowDimension; n++) {
* if(elem.Gpr.Y.GetElement(0, n) > val) {
* dist = 0;
* for(int m=0; m<elem.X.ColumnDimension; m++) {
* tmp = elem.X.GetElement(n, m) - cur.GetElement(0, m);
* dist += tmp*tmp;
* }
* if(dist < minDist) {
* minDist = dist;
* iMin = n;
* }
* }
* }
* if(iMin>=0) {
* for(int m=0; m<elem.X.ColumnDimension; m++) {
* elem.Inputs[m].Weight = elem.X.GetElement(m, iMin) - cur.GetElement(0, m);
* }
* }
* }*/
}
/******************* End Function for calculate Angle ******************/
#endregion
#region Support Function
private GeneralMatrix convertJoinToMatrix(Joint j)
{
GeneralMatrix matrix = GeneralMatrix.Identity(3, 1);
matrix.SetElement(0, 0, j.Position.X);
matrix.SetElement(1, 0, j.Position.Y);
matrix.SetElement(2, 0, j.Position.Z);
return(matrix);
}
public static GeneralMatrix To3x1Matrix(PointD pointD)
{
GeneralMatrix m = new GeneralMatrix(3, 1);
m.SetElement(0, 0, pointD.X);
m.SetElement(1, 0, pointD.Y);
m.SetElement(2, 0, 1);
return(m);
}
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);
}
private QFactorResult calculateLeastSquareLorenz()
{
int i, j = 0;
int f0_index;
PointPairInt range;
int n;
// Preprocessing (removing data points which are far away from transmittance peak)
f0_index = findCenterFrequencyIndex();
if (f0_index < 0 || f0_index > pointList.Count)
{
return(new QFactorResult()); // return "no result" if center frequency not found.
}
range = findBandwidthIndexes(f0_index, 3.0F);
if (range.I0 == int.MaxValue)
{
return(new QFactorResult()); // return "no result" if center frequency not found.
}
n = range.I1 - range.I0;
if (n < 5)
{ // When we have very small amount of points, turn off Q-calculations completely - we don't want to have useless results!
// We need to return the center frequency which is already found.
return(new QFactorResult(pointList[f0_index].frequency, n));
}
GeneralMatrix X = new GeneralMatrix(n, 3); // vandermonde matrix with measured frequency points
GeneralMatrix Y = new GeneralMatrix(n, 3); // vector with measured power levels
for (i = 0; i < pointList.Count; i++)
{
if (i > range.I0 && i <= range.I1)
{
X.SetElement(j, 0, Math.Pow(pointList[i].frequency, 2));
X.SetElement(j, 1, pointList[i].frequency);
X.SetElement(j, 2, 1);
Y.SetElement(j, 0, Math.Pow(10, -pointList[i].gain / 10)); // measured power is converted to linear form and inverted (1/x)
j++;
}
}
QRDecomposition QR = new QRDecomposition(X);
GeneralMatrix beta = QR.Solve(Y); // Linear regression used to find beta coefficients
double A = beta.GetElement(0, 0);
double B = beta.GetElement(1, 0);
double C = beta.GetElement(2, 0);
double F0 = -B / (2 * A);
double Q = -B / (2 * Math.Sqrt(4 * A * C - B * B));
double P0 = -1 / ((B * B) / (4 * A) - C);
P0 = 10 * Math.Log10(P0);
//return new QFactorResult(Q, F0, F0 - 0.5 * F0 / Q, F0 + 0.5 * F0 / Q, P0-3, P0-3);
return(new QFactorResult(Q, F0, P0, F0 / Q, n));
}
public Point GetActualPosition(Point p)
{
GeneralMatrix mat = new GeneralMatrix(3, 1);
mat.SetElement(0, 0, (double)p.X);
mat.SetElement(1, 0, (double)p.Y);
mat.SetElement(2, 0, 1.0);
GeneralMatrix ret = m_transform.Inverse().Multiply(mat);
return(new Point((int)ret.GetElement(0, 0), (int)ret.GetElement(1, 0)));
}
public PointF GetActualDisplayPosition(PointF p)
{
GeneralMatrix mat = new GeneralMatrix(3, 1);
mat.SetElement(0, 0, p.X);
mat.SetElement(1, 0, p.Y);
mat.SetElement(2, 0, 1.0);
GeneralMatrix ret = m_transform.Multiply(mat);
return(new PointF((float)ret.GetElement(0, 0), (float)ret.GetElement(1, 0)));
}
public void TestNorm1()
{
GeneralMatrix _gm = new GeneralMatrix(2, 2);
_gm.SetElement(0, 0, 1);
_gm.SetElement(0, 1, 2);
_gm.SetElement(1, 0, 3);
_gm.SetElement(1, 1, 4);
Assert.AreEqual(6, _gm.Norm1());
}
public void TestTranspose()
{
GeneralMatrix _gm = new GeneralMatrix(2, 2);
_gm.SetElement(0, 0, 1);
_gm.SetElement(0, 1, 2);
_gm.SetElement(1, 0, 3);
_gm.SetElement(1, 1, 4);
GeneralMatrix _ngm = _gm.Transpose();
Assert.AreEqual(1, 0, 2);
}
/// <summary>
///
/// </summary>
private void CalculatePriorities()
{
PrioritiesSelector selector = new PrioritiesSelector();
selector.ComputePriorities(_criteria);
//first (zero-th) element of the lambda matrix is the
//consistency ratio factor for the selection matrix
_lambdas.SetElement(0, 0, selector.ConsistencyRatio);
_orderedCriteria = selector.CalculatedMatrix;
}
/// <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());
}
}
public GeneralMatrix QuantizePolar(int grid_x, int grid_y)
{
// create a general matrix full of 0s
GeneralMatrix mesh = new GeneralMatrix(grid_y, grid_x, 0.0);
if (_points.Count == 0)
{
return(mesh);
}
double stepX = 2.0 * Math.PI / grid_x; // X is the theta coordinate
double stepY = 1.0 / grid_y; // Y is the R coordinate
foreach (GenericPoint pt in _points)
{
int x_index = (int)Math.Floor((pt.Theta(_polarCenter) + Math.PI) / stepX);
double r = pt.R(_polarCenter, _averageDist);
double t = pt.Theta(_polarCenter);
int y_index = (int)Math.Floor(pt.R(_polarCenter, _averageDist) / stepY);
if (y_index < grid_y && x_index < grid_x)
{
mesh.SetElement(y_index, x_index, 1.0);
}
}
#if FALSE
Console.WriteLine("Quantized polar");
printGeneralMatrix(ref mesh);
#endif
return(mesh);
}
/// <summary>
/// Calculates and saves the distance transform map for the screen coordinates.
/// Uses _sMesh and saves to _sDTM.
/// A distance transform map is a matrix in which every entry represents the
/// distance from that point to the nearest point with ink.
/// </summary>
private void ScreenDistanceTransform()
{
List <Coord> indices = IndexList(_sMesh);
_sDTM = new GeneralMatrix(GRID_Y_SIZE, GRID_X_SIZE, 0.0);
for (int i = 0; i < GRID_Y_SIZE; i++)
{
for (int j = 0; j < GRID_X_SIZE; j++)
{
Coord cp = new Coord(j, i);
double mindist = double.PositiveInfinity;
foreach (Coord pt in indices)
{
double distan = cp.distanceTo(pt);
if (distan < mindist)
{
mindist = distan;
}
}
_sDTM.SetElement(i, j, mindist);
}
}
}
/// <summary>
/// Gets the 3D coordinates relative to the center of the earth.
/// </summary>
/// <returns></returns>
public GeneralMatrix To3DPoint()
{
// use spherical coordinates: rho, phi, theta
const double rho = 6378200; // earth radius in metres
double sinPhi = Math.Sin(0.5 * Math.PI + latitude / 180.0 * Math.PI);
double cosPhi = Math.Cos(0.5 * Math.PI + latitude / 180.0 * Math.PI);
double sinTheta = Math.Sin(longitude / 180.0 * Math.PI);
double cosTheta = Math.Cos(longitude / 180.0 * Math.PI);
GeneralMatrix p = new GeneralMatrix(3, 1);
p.SetElement(0, 0, rho * sinPhi * cosTheta);
p.SetElement(1, 0, rho * sinPhi * sinTheta);
p.SetElement(2, 0, rho * cosPhi);
return(p);
}
public void TestSolve()
{
GeneralMatrix _ls = new GeneralMatrix(2, 2);
_ls.SetElement(0, 0, 1);
_ls.SetElement(0, 1, 2);
_ls.SetElement(1, 0, 3);
_ls.SetElement(1, 1, 4);
GeneralMatrix _rs = new GeneralMatrix(2, 1);
_rs.SetElement(0, 0, -3);
_rs.SetElement(1, 0, -5);
GeneralMatrix _solution = _ls.Solve(_rs);
Assert.AreEqual(_solution.GetElement(0, 0), 1);
Assert.AreEqual(_solution.GetElement(1, 0), -2);
}
public void Visit(Compiled.Gp elem)
{
GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length);
for (int i = 0; i < elem.Terms.Length; ++i)
{
cur.SetElement(0, i, ValueOf(elem.Terms[i]));
}
elem.Value = elem.Gpr.Evaluate(cur).GetElement(0, 0);
}
//
/// <summary>
/// Calculates all elements for <code>this.alpha</code>.
/// </summary>
protected void UpdateAlpha()
{
for (int i = 0; i < parameters.Length; i++)
{
for (int j = 0; j < parameters.Length; j++)
{
alpha.SetElement(i, j, CalculateAlphaElement(i, j));
}
}
}
public void Visit(Compiled.Gp elem)
{
GeneralMatrix cur = new GeneralMatrix(1, elem.Terms.Length);
for (int i = 0; i < elem.Terms.Length; ++i)
{
cur.SetElement(0, i, ValueOf(elem.Terms[i]));
}
LocalDerivative = elem.Gpr.PartialDerivative(cur, ArgumentIndex);
}
public void LUDecomposition()
{
GeneralMatrix A = new GeneralMatrix(columnwise, 4);
int n = A.ColumnDimension;
A = A.GetMatrix(0, n - 1, 0, n - 1);
A.SetElement(0, 0, 0.0);
LUDecomposition LU = A.LUD();
Assert.IsTrue(GeneralTests.Check(A.GetMatrix(LU.Pivot, 0, n - 1), LU.L.Multiply(LU.U)));
}
public GeneralMatrix CalculateHessian(double[] x)
{
GeneralMatrix hessian = new GeneralMatrix(Dimension, Dimension);
for (int i = 0; i < Dimension; i++)
{
for (int j = 0; j < Dimension; j++)
{
hessian.SetElement(i, j, GetPartialDerivativeVal(i, j, x));
}
}
return(hessian);
}
/// <summary>
/// Calculates final model results as a sum of product of choices rated
/// for each criteria times weighing (preference) of each criteria
/// </summary>
private void CalculateFinalResult()
{
double sum;
for (int i = 0; i < _mChoices; i++)
{
sum = 0;
for (int j = 0; j < _nCriteria; j++)
{
sum += _modelResult.GetElement(i, j) * this._orderedCriteria.GetElement(j, 0);
}
_calculatedChoices.SetElement(i, 0, sum);
}
}
/// <summary>
/// Average values of the priority matrix over sum of columns.
/// set values of sum of averaged rows into a new matrix
/// </summary>
/// <param name="argMatrix"></param>
/// <param name="selection"></param>
public void PCalc(GeneralMatrix argMatrix, GeneralMatrix selection)
{
int n = argMatrix.ColumnDimension;
GeneralMatrix sMatrix = new GeneralMatrix(argMatrix.ArrayCopy);
double c = 0.0;
int i, j;
for (i = 0; i < sMatrix.ColumnDimension; i++)
{
c = 0.0;
for (j = 0; j < sMatrix.RowDimension; j++)
{
c += sMatrix.GetElement(j, i);
}
selection.SetElement(i, 0, c);
}
for (i = 0; i < sMatrix.ColumnDimension; i++)
{
for (j = 0; j < sMatrix.RowDimension; j++)
{
sMatrix.SetElement(j, i, sMatrix.GetElement(j, i) / selection.GetElement(i, 0));
}
}
for (i = 0; i < sMatrix.RowDimension; i++)
{
c = 0.0;
for (j = 0; j < sMatrix.ColumnDimension; j++)
{
c += sMatrix.GetElement(i, j);
}
selection.SetElement(i, 0, c / n);
}
}
public GeneralMatrix QuantizeScreen(int grid_x, int grid_y)
{
// create a general matrix full of 0s
GeneralMatrix mesh = new GeneralMatrix(grid_x, grid_y, 0.0);
// Find the bounding box of the points.
System.Drawing.Rectangle bBox = BoundingBox();
if (_points.Count == 0)
{
return(mesh);
}
// find the step we want
// Note: this is an intentional deviation from the original paper.
// We do not maintain the original aspect ratio of the shape.
double stepX = bBox.Width / (double)(grid_x - 1);
double stepY = bBox.Height / (double)(grid_y - 1);
// find the center of the image
double cx = (double)(bBox.Left + bBox.Right) / 2.0;
double cy = (double)(bBox.Top + bBox.Bottom) / 2.0;
foreach (GenericPoint pt in _points)
{
// normalize the point's coordinates
int x_index = (int)Math.Floor(((pt.X - cx) / stepX) + grid_x / 2);
int y_index = (int)Math.Floor(((pt.Y - cy) / stepY) + grid_y / 2);
if (x_index < grid_x && y_index < grid_y)
{
mesh.SetElement(y_index, x_index, 1.0);
}
}
#if JESSI
Console.WriteLine("Quantized screen");
Console.Write(mesh.ToString());
#endif
return(mesh);
}
/// <summary>
/// Calculates and saves the distance transform map for the polar coordinates.
/// Uses _pMesh and saves to _pDTM.
/// A distance transform map is a matrix in which every entry represents the
/// distance from that point to the nearest point with ink.
/// </summary>
private void PolarDistanceTransform()
{
List <Coord> indices = IndexList(_pMesh);
for (int i = 0; i < GRID_Y_SIZE; i++)
{
for (int j = 0; j < GRID_X_SIZE; j++)
{
Coord cp = new Coord(j, i);
double mindist = double.PositiveInfinity;
foreach (Coord pt in indices)
{
// the straight distance between current point and current index
double dx = Math.Abs(cp.X - pt.X);
double dy = Math.Abs(cp.Y - pt.Y);
double straightDist = Math.Sqrt(dx * dx + dy * dy);
dx = (double)GRID_X_SIZE - dx;
double wrapDist = Math.Sqrt(dx * dx + dy * dy);
double distan = Math.Min(straightDist, wrapDist);
if (distan < mindist)
{
mindist = distan;
}
}
_pDTM.SetElement(i, j, mindist);
}
}
#if FALSE
Console.WriteLine("Your polar distance transform map:");
Console.Write(_pDTM.ToString())
#endif
}
/// <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());
}
}
public void BadSetValue1()
{
B.SetElement(B.RowDimension, B.ColumnDimension - 1, 0.0);
}