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 }); }
static void Main(string[] args) { MatrixWorker mo = new MatrixWorker(); var result = mo.Process(); for (int i = 0; i < result.GetLength(0); i++) { for (int j = 0; j < result.GetLength(1); j++) { Console.Write($" |{result[i,j]}| "); } Console.WriteLine(); } Console.ReadLine(); }