You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

matrix.c 4.9KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122
  1. /* matrix.c - matrix-algebra code
  2. *
  3. * This file is Copyright (c)2014-2018 by the GPSD project
  4. * SPDX-License-Identifier: BSD-2-clause
  5. */
  6. #include "gpsd_config.h" /* must be before all includes */
  7. #include <math.h>
  8. #include <stdbool.h>
  9. #include "matrix.h"
  10. bool matrix_invert(double mat[4][4], double inverse[4][4])
  11. /* selected elements from 4x4 matrox inversion */
  12. {
  13. // Find all NECESSARY 2x2 subdeterminants
  14. double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
  15. double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
  16. //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0];
  17. double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
  18. //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1];
  19. //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2];
  20. double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
  21. //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0];
  22. double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
  23. //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1];
  24. double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
  25. //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2];
  26. double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
  27. double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
  28. double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
  29. double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
  30. double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
  31. double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
  32. // Find all NECESSARY 3x3 subdeterminants
  33. double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02
  34. + mat[0][2] * Det2_12_01;
  35. //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03
  36. // + mat[0][3]*Det2_12_01;
  37. //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03
  38. // + mat[0][3]*Det2_12_02;
  39. //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13
  40. // + mat[0][3]*Det2_12_12;
  41. //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02
  42. // + mat[0][2]*Det2_13_01;
  43. double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03
  44. + mat[0][3] * Det2_13_01;
  45. //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03
  46. // + mat[0][3]*Det2_13_02;
  47. //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13
  48. // + mat[0][3]*Det2_13_12;
  49. //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02
  50. // + mat[0][2]*Det2_23_01;
  51. //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03
  52. // + mat[0][3]*Det2_23_01;
  53. double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03
  54. + mat[0][3] * Det2_23_02;
  55. //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13
  56. // + mat[0][3]*Det2_23_12;
  57. double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02
  58. + mat[1][2] * Det2_23_01;
  59. double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03
  60. + mat[1][3] * Det2_23_01;
  61. double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03
  62. + mat[1][3] * Det2_23_02;
  63. double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13
  64. + mat[1][3] * Det2_23_12;
  65. // Find the 4x4 determinant
  66. static double det;
  67. det = mat[0][0] * Det3_123_123
  68. - mat[0][1] * Det3_123_023
  69. + mat[0][2] * Det3_123_013 - mat[0][3] * Det3_123_012;
  70. // Very small determinants probably reflect floating-point fuzz near zero
  71. if (fabs(det) < 0.0001)
  72. return false;
  73. inverse[0][0] = Det3_123_123 / det;
  74. //inverse[0][1] = -Det3_023_123 / det;
  75. //inverse[0][2] = Det3_013_123 / det;
  76. //inverse[0][3] = -Det3_012_123 / det;
  77. //inverse[1][0] = -Det3_123_023 / det;
  78. inverse[1][1] = Det3_023_023 / det;
  79. //inverse[1][2] = -Det3_013_023 / det;
  80. //inverse[1][3] = Det3_012_023 / det;
  81. //inverse[2][0] = Det3_123_013 / det;
  82. //inverse[2][1] = -Det3_023_013 / det;
  83. inverse[2][2] = Det3_013_013 / det;
  84. //inverse[2][3] = -Det3_012_013 / det;
  85. //inverse[3][0] = -Det3_123_012 / det;
  86. //inverse[3][1] = Det3_023_012 / det;
  87. //inverse[3][2] = -Det3_013_012 / det;
  88. inverse[3][3] = Det3_012_012 / det;
  89. return true;
  90. }
  91. #ifdef __UNUSED_
  92. // cppcheck-suppress unusedFunction
  93. void matrix_symmetrize(double mat[4][4], double prod[4][4])
  94. /* symmetrize a matrix, multiply it by its transpose */
  95. {
  96. int i, j, k;
  97. for (i = 0; i < 4; ++i) { //< rows
  98. for (j = 0; j < 4; ++j) { //< cols
  99. prod[i][j] = 0.0;
  100. for (k = 0; k < 4; ++k) {
  101. prod[i][j] += mat[k][i] * mat[k][j];
  102. }
  103. }
  104. }
  105. }
  106. #endif /* UNUSED */
  107. /* end */