S3315_RK3588/NeoTracker/src/Common/Arith_Utils.cpp

#include <cmath>
#include <iostream>
#include <fstream>
#include <string>
#include <cstring>
#include <cstdlib>
#include "Arith_Utils.h"
#include "Arith_SysStruct.h"

#ifdef __linux__
#include <unistd.h>
#define ACCESS access 
#elif _WIN32
#include <windows.h>
#include <io.h>
#define ACCESS _access 
#endif


void AddMat(double* A, double* B, int mn, double* AaddB)
{
    for (size_t i = 0; i < mn; i++)
    {
        AaddB[i] = A[i] + B[i];
    }

}

void MultiMatScalar(double scalar, double* A, double* sA, int mn)
{
    for (size_t i = 0; i < mn; i++)
    {
        sA[i] = scalar * A[i];
    }
}

void TransposeMat(double* A, int w, int h, double* AT)
{
    for (int i = 0; i < h; i++)
    {
        for (int j = 0; j < w; j++)
        {
            AT[j * w + i] = A[i * h + j];
        }
    }
}


/* 1    0      0
0    cos    sin
0   -sin    cos
*/
void Rx(double omega, double* Opp)
{
    double coso = cos(RADIAN(omega));
    double sino = sin(RADIAN(omega));

    Opp[0] = 1;
    Opp[1] = 0;
    Opp[2] = 0;
    Opp[3] = 0;
    Opp[4] = coso;
    Opp[5] = sino;
    Opp[6] = 0;
    Opp[7] = -sino;
    Opp[8] = coso;
}

/* cos    0    sin
0     1     0
-sin  0    cos
*/
void Ry(double omega, double* Opp)
{
    double coso = cos(RADIAN(omega));
    double sino = sin(RADIAN(omega));

    Opp[0] = coso;
    Opp[1] = 0;
    Opp[2] = sino;
    Opp[3] = 0;
    Opp[4] = 1;
    Opp[5] = 0;
    Opp[6] = -sino;
    Opp[7] = 0;
    Opp[8] = coso;
}



/* cos    sin    0
-sin cos  0
0   0     1
*/
void Rz(double omega, double* Opp)
{
    double coso = cos(RADIAN(omega));
    double sino = sin(RADIAN(omega));

    Opp[0] = coso;
    Opp[1] = sino;
    Opp[2] = 0;
    Opp[3] = -sino;
    Opp[4] = coso;
    Opp[5] = 0;
    Opp[6] = 0;
    Opp[7] = 0;
    Opp[8] = 1;
}

// 镜面反射矩阵
void RMirror(PointXYZ normalVec, double* Opp)
{
    //	M = eye(3)-(2*N*N');
    Opp[0] = 1 - 2 * normalVec.X * normalVec.X;
    Opp[1] = -2 * normalVec.X * normalVec.Y;
    Opp[2] = -2 * normalVec.X * normalVec.Z;
    Opp[3] = -2 * normalVec.X * normalVec.Y;
    Opp[4] = 1 - 2 * normalVec.Y * normalVec.Y;
    Opp[5] = -2 * normalVec.Y * normalVec.Z;
    Opp[6] = -2 * normalVec.X * normalVec.Z;
    Opp[7] = -2 * normalVec.Y * normalVec.Z;
    Opp[8] = 1 - 2 * normalVec.Z * normalVec.Z;

}
//*********************************************************************************************
//  矩阵乘法
//*********************************************************************************************
void  MultiMat(double* A, double* B, double* AB, int m, int n, int p)
{
    // A：m*n ; B：n*p  ;
    int	i, j, k;
    double Multisum;

    for (i = 0; i < m; i++) 
    {
        for (j = 0; j < p; j++) 
        {
            Multisum = 0;
            for (k = 0; k < n; k++)
                Multisum += A[i * n + k] * B[k * p + j];

            AB[i * p + j] = Multisum;
        }
    }
}



//*********************************************************************************************
//  
//*********************************************************************************************
void MultiMat33(double* A, double* B, double* AB)
{
    MultiMat(A, B, AB, 3, 3, 3);
}

//*********************************************************************************************
//  
//*********************************************************************************************
void MultiMat333(double* A, double* B, double* C, double* ABC)
{
    double BC[9];
    MultiMat(B, C, BC, 3, 3, 3);
    MultiMat(A, BC, ABC, 3, 3, 3);
}

// 对点/矢量旋转
PointXYZ RtPoint(double* M, PointXYZ Point)
{
    // M * [X,Y,Z]'
    double A[3];
    double G[3];
    A[0] = Point.X;
    A[1] = Point.Y;
    A[2] = Point.Z;
    MultiMat(M, A, G, 3, 3, 1);

    PointXYZ result;
    result.X = G[0];
    result.Y = G[1];
    result.Z = G[2];
    return result;

}

// 标准化矢量
PointXYZ NormPoint(PointXYZ Point)
{
    double Len = sqrt(Point.X * Point.X + Point.Y * Point.Y + Point.Z * Point.Z);
    Point.X /= Len;
    Point.Y /= Len;
    Point.Z /= Len;
    return Point;
}


void eye3(double* M)
{
    memset(M, 0, sizeof(double) * 9);
    M[0] = 1;
    M[4] = 1;
    M[8] = 1;
}

void invMat3(double* A, double* invA)
{
    double a1 = A[0];
    double a2 = A[1];
    double a3 = A[2];
    double b1 = A[3];
    double b2 = A[4];
    double b3 = A[5];
    double c1 = A[6];
    double c2 = A[7];
    double c3 = A[8];

    //A*
    double Astar[9] = { 0 };
    Astar[0] = b2 * c3 - c2 * b3;
    Astar[1] = c1 * b3 - b1 * c3;
    Astar[2] = b1 * c2 - c1 * b2;

    Astar[3] = c2 * a3 - a2 * c3;
    Astar[4] = a1 * c3 - c1 * a3;
    Astar[5] = a2 * c1 - a1 * c2;

    Astar[6] = a2 * b3 - b2 * a3;
    Astar[7] = b1 * a3 - a1 * b3;
    Astar[8] = a1 * b2 - a2 * b1;

    double detA = a1 * (b2 * c3 - c2 * b3) - a2 * (b1 * c3 - c1 * b3) + a3 * (b1 * c2 - c1 * b2);

    for (size_t i = 0; i < 9; i++)
    {
        invA[i] = Astar[i] / detA;
    }

}


// 旋转向量转矩阵
void rotationVectorToMatrix(PointXYZ rotaVec_One, float rotaAgl, double* Mat)
{
    // 
    PointXYZ vec_norm = NormPoint(rotaVec_One);

    // K
    double K[9] = { 0,-vec_norm.Z,vec_norm.Y,
                    vec_norm.Z,0,-vec_norm.X,
                    -vec_norm.Y,vec_norm.X,0 };

    // eye(3)
    double eye[9] = { 0 };
    eye3(eye);

    // eye + sind(r) * K + (1-cos(r)) * K * K
    double sK[9] = { 0 };
    MultiMatScalar(sin(RADIAN(rotaAgl)), K, sK, 9);

    double KK[9] = { 0 };
    MultiMat33(K, K, KK);

    double scK[9] = { 0 };
    MultiMatScalar(1 - cos(RADIAN(rotaAgl)), KK, scK, 9);

    double tmp[9] = { 0 };
    AddMat(eye, sK, 9, tmp);

    AddMat(tmp, scK, 9, Mat);


}


// 初始化哈希表
void initHashMap(HashMap* map, int capacity) 
{
    map->entries = (HashEntry*)malloc(capacity * sizeof(HashEntry));
    if (NULL == map->entries)
    {
        return;
    }
    map->capacity = capacity;
    map->size = 0;
    for (int i = 0; i < capacity; i++) 
    {
        map->entries[i].key = 0;
        map->entries[i].count = 0;
    }
}

// 查找哈希表中的键
int findKey(HashMap* map, int key) 
{
    for (int i = 0; i < map->size; i++) 
    {
        if (map->entries[i].key == key) 
        {
            return i;
        }
    }
    return -1;
}

// 插入键值对到哈希表
void insert(HashMap* map, int key) 
{
    int index = findKey(map, key);
    if (index == -1) 
    {
        if (map->size < map->capacity) 
        {
            map->entries[map->size].key = key;
            map->entries[map->size].count = 1;
            map->size++;
        }
        else 
        {
            printf("Hash map is full.\n");
        }
    }
    else 
    {
        map->entries[index].count++;
    }
}

// 释放哈希表
void freeHashMap(HashMap* map)
{
    free(map->entries);
}


int findMostFrequentNonZero(int arr[], int size, int *clsMaxNum, float *MostFrequentratio)
{
    HashMap map;
    initHashMap(&map, size);  // 假设数组中的数字范围较小

    // 记录每个非0数字出现的次数
    for (int i = 0; i < size; i++) 
    {
        if (arr[i] != 0) 
        {
            insert(&map, arr[i]);
        }
    }

    // 找到出现次数最多的数字
    int mostFrequentNum = 0;
    int maxFrequency = 0;
    int totalNum = 0;
    for (int i = 0; i < map.size; i++) 
    {
        if (map.entries[i].count > maxFrequency) 
        {
            mostFrequentNum = map.entries[i].key;
            maxFrequency = map.entries[i].count;
        }
        if (map.entries[i].count > 0)
        {
            totalNum += map.entries[i].count;
        }
    }
    *clsMaxNum = maxFrequency;
    *MostFrequentratio = maxFrequency / (totalNum + FEPSILON);

    freeHashMap(&map);
    return mostFrequentNum;
}

int count2To0Transitions(int arr[], int size) 
{
    int count = 0;
    for (int i = 0; i < size - 1; i++) 
    {
        if (arr[i] == 2 && arr[i + 1] == 0) 
        {
            count++;
        }
    }
    return count;
}

bool checkXToYCondition(int arr[], int size, int x, int y, int frmThresh)
{
    bool foundTransition = false;
    int consecutiveOnes = 0;

    for (int i = 0; i < size - 1; i++) 
    {
        if (arr[i] == x && arr[i + 1] == y) 
        {
            foundTransition = true;
            consecutiveOnes = 1;  // 从 1 开始计数
            i++;  // 跳过已经检查的 1

            // 继续检查后续的 1
            while (i < size - 1 && arr[i] == y) 
            {
                consecutiveOnes++;
                i++;
            }

            // 检查连续 1 的次数是否大于 阈值
            if (consecutiveOnes > frmThresh)
            {
                return true;
            }
        }
    }

    return false;
}

int countNumberFreq(int arr[], int size, int number)
{
    int count = 0;
    for (int i = 0; i < size; i++) 
    {
        if (arr[i] == 2) 
        {
            count++;
        }
    }
    return count;
}

static float get_dot_prod(float* A, float* B, int size)
{
    int i;
    float dot_prod = 0.f;
    for (i = 0; i < size; i++)
    {
        dot_prod += A[i] * B[i];
    }

    return dot_prod;
}

static float get_eucl_magn(float* A, float* B, int size)
{
    int i;
    double A_len = 0;
    double B_len = 0;
    float eucl_magn = 0;
    for (i = 0; i < size; i++)
    {
        A_len += pow(A[i], 2);
        B_len += pow(B[i], 2);
    }
    eucl_magn = (float)sqrt(A_len * B_len);
    return eucl_magn;
}

float Cosine_Similarity(float* A, float* B, int size)
{
    // Vectors size can't be 0
    if (size == 0)
    {
        //throw std::logic_error("Vector size can not be 0."); 
        return 0;
    }
    float value = 0;
    float dot_prod = 0;
    // 计算欧式距离
    float eucl_magn = get_eucl_magn(A, B, size);
    // If eucledian distance is 0 skip calculation
    if (eucl_magn == 0)
    {
        value = 0;
    }
    else
    {
        dot_prod = get_dot_prod(A, B, size);
        value = dot_prod / eucl_magn;
    }
    return value;
}


#ifdef _WIN32  // Windows平台

#include <Windows.h>

std::string GetDynamicLibraryPath()
{
    HMODULE hModule = GetModuleHandle(NULL);
    char path[MAX_PATH];
    GetModuleFileName(hModule, path, MAX_PATH);
    std::string fullPath(path);
    size_t pos = fullPath.find_last_of("\\/");
    return fullPath.substr(0, pos);
}

#elif __linux__  // Linux平台


#include <dlfcn.h>
#include <unistd.h>

std::string GetDynamicLibraryPath()
{
    Dl_info dlInfo;
    dladdr((void*)GetDynamicLibraryPath, &dlInfo);
    char* path = realpath(dlInfo.dli_fname, NULL);
    std::string fullPath(path);
    free(path);
    size_t pos = fullPath.find_last_of("/");
    return fullPath.substr(0, pos);
}

#else

#error Unsupported platform

#endif


void Save_RunImg(GD_VIDEO_FRAME_S img, SINT32 nControlerFrmNum)
{
    char resPath[256] = { 0 };
    std::string path = GetDynamicLibraryPath();
    snprintf(resPath, 256, "%s/%s", path.c_str(), "SaveImg");
    if (ACCESS(resPath, 0) == 0)
    {// SaveImg文件存在,写数据,默认在第10帧写数据
        if (10 == nControlerFrmNum)
        {
            std::string filename = std::string(path) + "/frame_" + std::to_string(nControlerFrmNum);
            std::ofstream outFile(filename, std::ios::binary);
            if (!outFile)
            {
                std::cerr << "Failed to open file for writing: " << filename << std::endl;
            }

            switch (img.enPixelFormat)
            {
            case GD_PIXEL_FORMAT_GRAY_Y8:
                for (unsigned int y = 0; y < img.u32Height; ++y)
                {
                    outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width);
                }
                break;
            case GD_PIXEL_FORMAT_GRAY_Y16:
                for (unsigned int y = 0; y < img.u32Height; ++y)
                {
                    outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width * 2);
                }
                break;
            case GD_PIXEL_FORMAT_NV12:
                // 写入Y平面
                for (unsigned int y = 0; y < img.u32Height; ++y)
                {
                    outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width);
                }
                // 写入UV平面
                for (unsigned int y = 0; y < img.u32Height / 2; ++y)
                {
                    outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[1] + y * img.u32Stride[1]), img.u32Stride[1]);
                }
                break;
            case GD_PIXEL_FORMAT_RGB_PACKED:
                for (unsigned int y = 0; y < img.u32Height; ++y)
                {
                    outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width * 3);
                }
                break;
            default:
                std::cerr << "Unsupported pixel format: " << img.enPixelFormat << std::endl;
                outFile.close();
            }

            outFile.close();
        }
    }
}


FLOAT32 SERVO_CorrectAngleScale(FLOAT32 fAngle)
{
    FLOAT32 fAngleCorrect = fAngle;

    //若角度小于伺服角度下限（-180°），则其实际值已经跨入0°～180°范围的右侧
    if (fAngle < SERVO_ANGLE_MIN)
        fAngleCorrect = fAngle + SERVO_ANGLE_RANGE;

    //若角度大于伺服角度上限（180°），则其实际值已经跨入-180°～0°的范围左侧
    if (fAngle >= SERVO_ANGLE_MAX)
        fAngleCorrect = fAngle - SERVO_ANGLE_RANGE;

    return fAngleCorrect;
}