hpm6750/controller_yy_app/matrix/matrix.h

#pragma once

#include "math.h"

/**
 * @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 (norm > 1e-6f)
    {
        for (int i = 0; i < n; ++i)
        {
            V[i] /= norm;
        }
    }
}

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_DotProductRealNum(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)
{
    V[0] = V1[1] * V2[2] - V1[2] * V2[1];
    V[1] = V1[2] * V2[0] - V1[0] * V2[2];
    V[2] = V1[0] * V2[1] - V1[1] * V2[0];
}

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++)
        {
            P[i * n + j] = datap[i * n + j];
        }
    }
}

/**
 * @brief 矩阵转置
 *
 * @param M 要转置的矩阵
 * @param M_t 转置后的矩阵
 * @param m 矩阵行数
 * @param n 矩阵列数
 */
static inline void Matrix_Transpose(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];
        }
    }
}