4d_ObstacleRadarDroneCAN/math/matrix.c

#include "matrix.h"

int Matrix_Inv_3D(const float P[3][3], float inv[3][3]) {
  /* 计算行列式 */
  float det = P[0][0] * (P[1][1] * P[2][2] - P[2][1] * P[1][2]) -
              P[0][1] * (P[1][0] * P[2][2] - P[1][2] * P[2][0]) +
              P[0][2] * (P[1][0] * P[2][1] - P[1][1] * P[2][0]);

  if (fabs((float)det) < FLT_EPSILON || !isfinite(det)) {
    return -1;
  }

  inv[0][0] = (P[1][1] * P[2][2] - P[2][1] * P[1][2]) / det;
  inv[0][1] = (P[0][2] * P[2][1] - P[0][1] * P[2][2]) / det;
  inv[0][2] = (P[0][1] * P[1][2] - P[0][2] * P[1][1]) / det;
  inv[1][0] = (P[1][2] * P[2][0] - P[1][0] * P[2][2]) / det;
  inv[1][1] = (P[0][0] * P[2][2] - P[0][2] * P[2][0]) / det;
  inv[1][2] = (P[1][0] * P[0][2] - P[0][0] * P[1][2]) / det;
  inv[2][0] = (P[1][0] * P[2][1] - P[2][0] * P[1][1]) / det;
  inv[2][1] = (P[2][0] * P[0][1] - P[0][0] * P[2][1]) / det;
  inv[2][2] = (P[0][0] * P[1][1] - P[1][0] * P[0][1]) / det;

  return 0;
}

int Matrix_Brinv(float *a, int n) {

  int *is, *js, i, j, k, l, u, v;
  int temp1[n];
  int temp2[n];

  float d, p;
  is = temp1;
  js = temp2;
  for (k = 0; k <= n - 1; k++) {
    d = 0.0f;
    for (i = k; i <= n - 1; i++) {
      for (j = k; j <= n - 1; j++) {
        l = i * n + j;
        p = fabsf(a[l]);
        if (p > d) {
          d = p;
          is[k] = i;
          js[k] = j;
        }
      }
    }
    if (d + 1.0f == 1.0f) {
      return (0);
    }
    if (is[k] != k) {
      for (j = 0; j <= n - 1; j++) {
        u = k * n + j;
        v = is[k] * n + j;
        p = a[u];
        a[u] = a[v];
        a[v] = p;
      }
    }
    if (js[k] != k) {
      for (i = 0; i <= n - 1; i++) {
        u = i * n + k;
        v = i * n + js[k];
        p = a[u];
        a[u] = a[v];
        a[v] = p;
      }
    }
    l = k * n + k;
    a[l] = 1.0f / a[l];
    for (j = 0; j <= n - 1; j++) {
      if (j != k) {
        u = k * n + j;
        a[u] = a[u] * a[l];
      }
    }
    for (i = 0; i <= n - 1; i++) {
      if (i != k) {
        for (j = 0; j <= n - 1; j++)
          if (j != k) {
            u = i * n + j;
            a[u] = a[u] - a[i * n + k] * a[k * n + j];
          }
      }
    }
    for (i = 0; i <= n - 1; i++) {
      if (i != k) {
        u = i * n + k;
        a[u] = -a[u] * a[l];
      }
    }
  }
  for (k = n - 1; k >= 0; k--) {
    if (js[k] != k) {
      for (j = 0; j <= n - 1; j++) {
        u = k * n + j;
        v = js[k] * n + j;
        p = a[u];
        a[u] = a[v];
        a[v] = p;
      }
    }
    if (is[k] != k) {
      for (i = 0; i <= n - 1; i++) {
        u = i * n + k;
        v = i * n + is[k];
        p = a[u];
        a[u] = a[v];
        a[v] = p;
      }
    }
  }
  return (1);
}