public void MatrixInverseTest()
{
double[,] matrix = new double[, ] {
{ 1, 2, 3 },
{ -3, 2, 1 },
{ 4, -1, 1 }
};
double[,] realInverted = new double[, ] {
{ 1.5, -2.5, -2 },
{ 3.5, -5.5, -5 },
{ -2.5, 4.5, 4 }
};
double[,] inverted = MatrixWorker.Invert(matrix);
int n = realInverted.GetLength(0);
Assert.AreEqual(n, inverted.GetLength(0));
Assert.AreEqual(n, inverted.GetLength(1));
for (int i = 0; i < n; i++)
{
for (int j = 0; j < n; j++)
{
Assert.AreEqual(realInverted[i, j], inverted[i, j]);
}
}
}
/// <summary>
/// Convert LLA to ENU
/// </summary>
/// <param name="lat">Latitude</param>
/// <param name="lon">Longitude</param>
/// <param name="alt">Altitude</param>
/// <param name="reflat">Latitude of ENU coordinate system center</param>
/// <param name="reflon">Longitude of ENU coordinate system center</param>
/// <param name="refalt">Altitude of ENU coordinate system center</param>
/// <returns>ENU coordinates of given LLA point</returns>
public double[] ENUfromLLA(double lat, double lon, double alt, double reflat, double reflon, double refalt)
{
double[,] T = MatrixWorker.Invert(ECEFfromENUTransform(reflat, reflon, refalt));
double[] p = ECEFfromLLA(lat, lon, alt);
double tx = T[0, 0] * p[0] + T[0, 1] * p[1] + T[0, 2] * p[2] + T[0, 3];
double ty = T[1, 0] * p[0] + T[1, 1] * p[1] + T[1, 2] * p[2] + T[1, 3];
double tz = T[2, 0] * p[0] + T[2, 1] * p[1] + T[2, 2] * p[2] + T[2, 3];
return(new double[3] {
tx, ty, tz
});
}