4d_ObstacleRadarDroneCAN/math/matrix.h

#pragma once

#include "float.h"
#include "helpler_funtions.h"
#include "math.h"

#ifdef __cplusplus
extern "C" {
#endif

/**
 * @brief 向量求模
 *
 * @param V 向量
 * @param n V的维度
 * @return float V的模值
 */
static inline float Vector_GetNorm(const float *V, int n) {
  float norm = 0.0f;
  for (int i = 0; i < n; i++)
    norm += V[i] * V[i];
  norm = sqrtf(norm);
  return norm;
}

/**
 * @brief 向量 V 归一化
 * @param V 向量
 * @param n V的维度
 */
static inline void Vector_Normalize(float *V, int n) {
  float norm = 0.0f;
  for (int i = 0; i < n; i++)
    norm += V[i] * V[i];
  norm = sqrtf(norm);

  if (!is_zerof(norm)) {
    for (int i = 0; i < n; ++i) {
      V[i] /= norm;
    }
  }
}

/**
 * @brief 向量约束模值
 * @param V 向量
 * @param n 向量维度
 * #param norm_max 最大模值
 */
static inline void Vector_ConstrainNorm(float *V, int n, float norm_max) {
  if (norm_max > 0) {
    float norm = Vector_GetNorm(V, n);
    if (norm > norm_max) {
      float ratio = norm_max / norm;
      for (int i = 0; i < n; ++i) {
        V[i] *= ratio;
      }
    }
  }
}

/**
 * @brief 向量点乘
 *
 * @param V1 向量
 * @param V2 向量
 * @param n V1 V1 的维数
 * @return float V1 * V2 的结果
 */
static inline float Vector_DotProduct(const float *V1, const float *V2, int n) {
  float dotProduct = 0.0f;
  for (int i = 0; i < n; ++i)
    dotProduct += V1[i] * V2[i];
  return dotProduct;
}

/**
 * @brief 向量和实数乘
 *
 * @param V 向量
 * @param k 实数
 * @param n 向量维数
 */
static inline void Vector_Scale(float *V, float k, int n) {
  for (int i = 0; i < n; ++i) {
    V[i] *= k;
  }
}

/**
 * @brief 3 纬向量叉乘
 *
 * @param V1
 * @param V2
 * @param V V1 x V2
 */
static inline void Vector_CrossProduct_3D(const float V1[3], const float V2[3],
                                          float *V) {
  float_t temp_v[3];
  temp_v[0] = V1[1] * V2[2] - V1[2] * V2[1];
  temp_v[1] = V1[2] * V2[0] - V1[0] * V2[2];
  temp_v[2] = V1[0] * V2[1] - V1[1] * V2[0];
  for (int i = 0; i < 3; i++) {
    V[i] = temp_v[i];
  }
}

/**
 * @brief 矩阵初始化
 *
 * @param P 矩阵
 * @param datap
 * @param m
 * @param n
 */
static inline void Matrix_Init(float *P, const float *datap, int m, int n) {
  for (int i = 0; i < m; i++) {
    for (int j = 0; j < n; j++) {
      if (datap == 0) {
        P[i * n + j] = 0.0f;
      } else {
        P[i * n + j] = datap[i * n + j];
      }
    }
  }
}

/**
 * @brief 矩阵转置
 *
 * @param M 要转置的矩阵
 * @param M_t 转置后的矩阵
 * @param m 矩阵行数
 * @param n 矩阵列数
 */
static inline void Matrix_Transpose(const float *M, float *M_t, int m, int n) {
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < m; j++) {
      M_t[i * m + j] = M[j * n + i];
    }
  }
}

/**
 * @brief 矩阵乘法
 *
 * @param P 矩阵
 * @param Q 矩阵
 * @param R P * Q
 * @param l P的行数
 * @param m P的列数 M的行数
 * @param n M的列数
 */
static inline void Matrix_Multiplication(const float *P, const float *Q,
                                         float *R, int l, int m, int n) {
  // 将 R 矩阵零化
  for (int i = 0; i < l; i++) {
    for (int j = 0; j < n; j++) {
      R[i * n + j] = 0.0f;
    }
  }
  // 计算 P * Q = R
  for (int i = 0; i < l; i++) {
    for (int j = 0; j < n; j++) {
      for (int k = 0; k < m; k++) {
        R[i * n + j] += P[i * m + k] * Q[k * n + j];
      }
    }
  }
}

/**
 * @brief 矩阵和实数乘
 *
 * @param P 矩阵
 * @param k 实数
 * @param m P的行数
 * @param n P的列数
 */
static inline void Matrix_Multiplication_RealNum(float *P, float k, int m,
                                                 int n) {
  for (int i = 0; i < m; i++) {
    for (int j = 0; j < n; j++) {
      P[i * n + j] *= k;
    }
  }
}

/**
 * @brief 方阵的迹
 *
 * @param P 方阵
 * @param m P的行列维度
 * @return float
 */
static inline float Matrix_GetTrace(const float *P, int m) {
  float trace = 0.0f;
  for (int i = 0; i < m; ++i)
    trace += P[i * (m + 1)];

  return trace;
}

/**
 * @brief 矩阵按元素加
 *
 */
static inline void Matrix_AdditionByElement(const float *P, const float *Q,
                                            float *R, int m, int n) {
  for (int i = 0; i < m; ++i) {
    for (int j = 0; j < n; ++j) {
      R[i * n + j] = P[i * n + j] + Q[i * n + j];
    }
  }
}

/**
 * @brief 矩阵按元数减
 *
 */
static inline void Matrix_SubtractionByElement(const float *P, const float *Q,
                                               float *R, int m, int n) {
  for (int i = 0; i < m; ++i) {
    for (int j = 0; j < n; ++j) {
      R[i * n + j] = P[i * n + j] - Q[i * n + j];
    }
  }
}

int Matrix_Inv_3D(const float P[3][3], float inv[3][3]);

int Matrix_Brinv(float *a, int n);

#ifdef __cplusplus
}
#endif