4d_ObstacleRadarDroneCAN/math/helpler_funtions.h

#pragma once

#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>

/* PI 宏定义 */
#ifdef M_PI
#undef M_PI
#endif
#define M_PI 3.14159265358979323846

/* 重力加速度宏定义 */
#ifdef GRAVITY_MSS
#undef GRAVITY_MSS
#endif
#define GRAVITY_MSS 9.80665F

#ifdef __cplusplus
extern "C" {
#endif

/**
 * @brief 取两个数中较小的那个
 *
 */
#define min(x, y)                                                              \
  ({                                                                           \
    typeof(x) _min1 = x;                                                       \
    typeof(y) _min2 = y;                                                       \
    (void)(&_min1 == &_min2);                                                  \
    _min1 < _min2 ? _min1 : _min2;                                             \
  })

/**
 * @brief 取两个数中较大的那个
 *
 */
#define max(x, y)                                                              \
  ({                                                                           \
    typeof(x) _max1 = x;                                                       \
    typeof(y) _max2 = y;                                                       \
    (void)(&_max1 == &_max2);                                                  \
    _max1 > _max2 ? _max1 : _max2;                                             \
  })

#if defined DEG_TO_RAD
#undef DEG_TO_RAD
#endif
#define DEG_TO_RAD 0.017453292519943295769236907684886 // 度换算弧度

#if defined RAD_TO_DEG
#undef RAD_TO_DEG
#endif
#define RAD_TO_DEG 57.295779513082320876798154814105 // 弧度换算度

/* 弧度转角度 */
#define rad_to_deg(x) ((x) * 180.0 / M_PI)

/* 角度转弧度 */
#define deg_to_rad(x) ((x) * M_PI / 180.0)

/**
 * @brief 检查浮点数是否为 0
 *
 * @param fVal1 输入浮点数
 * @return true 浮点数为 0
 * @return false 浮点数不为 0
 */
static inline bool is_zerof(const float fVal1) {
  return fabsf(fVal1) < FLT_EPSILON ? true : false;
}

/**
 * @brief 检查双精度浮点数是否为 0
 *
 * @param dVal1 输入双精度浮点数
 * @return true 浮点数为 0
 * @return false 浮点数不为 0
 */
static inline bool is_zerod(const double dVal1) {
  return fabs(dVal1) < DBL_EPSILON ? true : false;
}

/**
 * @brief 检查两个浮点数是否相等
 *
 * @param fVal1 输入浮点数1
 * @param fVal2 输入浮点数2
 * @return true 两数相等
 * @return false 两数不相等
 */
static inline bool is_equalf(const float fVal1, const float fVal2) {
  return fabsf(fVal1 - fVal2) < FLT_EPSILON ? true : false;
}

/**
 * @brief 检查两个双精度浮点数是否相等
 *
 * @param dVal1 输入浮点数1
 * @param dVal2 输入浮点数2
 * @return true 两数相等
 * @return false 两数不相等
 */
static inline bool is_equald(const double dVal1, const double dVal2) {
  return fabs(dVal1 - dVal2) < DBL_EPSILON ? true : false;
}

/**
 * @brief 取浮点数的符号
 *
 * @param val 浮点数
 * @return int 符号 -1 或 +1
 */
static inline int sign(float val) {
  return (FLT_EPSILON < val) - (val < FLT_EPSILON);
}

/**
 * @brief 将浮点 wrap 数限制在某个范围内, 常用于如角度限制在 0~360 度等
 *
 * @param x 双精度浮点数
 * @param low 范围下限
 * @param high 范围上限
 * @return double wrap 限制输出结果
 */
static inline double wrap_double(double x, double low, double high) {
  if (low <= x && x < high) {
    return x;
  }

  const double range = high - low;
  const double inv_range = 1.0 / range;
  const double num_wraps = floor((x - low) * inv_range);
  return x - range * num_wraps;
}

/**
 * @brief 将浮点 wrap 数限制在某个范围内, 常用于如角度限制在 0~360 度等
 *
 * @param x 单精度浮点数
 * @param low  范围下限
 * @param high 范围上限
 * @return float wrap 限制输出结果
 */
static inline float wrap_float(float x, float low, float high) {
  if (low <= x && x < high) {
    return x;
  }

  const float range = high - low;
  const float inv_range = 1.0f / range;
  const float num_wraps = floorf((x - low) * inv_range);
  return x - range * num_wraps;
}

static inline int wrap_int(int x, int low, int high) {
  if (low <= x && x < high) {
    return x;
  }
  const int range_size = high - low + 1;
  int wrapped_value = (x - low) % range_size;
  if (wrapped_value < 0) {
    wrapped_value += range_size;
  }
  return wrapped_value + range_size;
}

/**
 * @brief 将浮点 wrap 数限制在 -PI ~ PI 范围内
 *
 * @param x 单精度浮点数
 * @return float wrap 限制输出结果
 */
static inline float wrap_f_pi(float x) { return wrap_float(x, -M_PI, M_PI); }

/**
 * @brief 将浮点 wrap 数限制在 -PI ~ PI 范围内
 *
 * @param x 双精度浮点数
 * @return double wrap 限制输出结果
 */
static inline double wrap_d_pi(double x) { return wrap_double(x, -M_PI, M_PI); }

/**
 * @brief 将浮点 wrap 数限制在 -180 ~ 180 范围内
 *
 * @param x 单精度浮点数
 * @return float wrap 限制输出结果
 */
static inline float wrap_f_180(float x) {
  return wrap_float(x, -180.0f, 180.0f);
}

/**
 * @brief 将浮点 wrap 数限制在 -180 ~ 180 范围内
 *
 * @param x 双精度浮点数
 * @return double wrap 限制输出结果
 */
static inline double wrap_d_180(double x) {
  return wrap_double(x, -180.0, 180.0);
}

/**
 * @brief 将浮点数限制在给定范围内
 *
 * @param amt 输入单精度浮点数
 * @param low 范围下限
 * @param high 范围上限
 * @return float 限幅输出结果
 */
static float constrain_float(float amt, float low, float high) {
  if (isnan(amt)) {
    return (low + high) * 0.5f;
  }

  return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

/**
 * @brief 将浮点数限制在给定范围内
 *
 * @param amt 输入双精度浮点数
 * @param low 范围下限
 * @param high 范围上限
 * @return double 限幅输出结果
 */
static double constrain_double(double amt, double low, double high) {
  if (isnan(amt)) {
    return (low + high) * 0.5;
  }
  return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

/**
 * @brief 将整数限制在给定范围内
 *
 * @param amt 输入无符号整数
 * @param low 范围下限
 * @param high 范围上限
 * @return uint16_t 限幅输出结果
 */
static uint16_t constrain_uint16(uint16_t amt, uint16_t low, uint16_t high) {
  return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

/**
 * @brief 将整数限制在给定范围内
 *
 * @param amt 输入无符号整数
 * @param low 范围下限
 * @param high 范围上限
 * @return uint32_t 限幅输出结果
 */
static uint32_t constrain_uint32(uint32_t amt, uint32_t low, uint32_t high) {
  return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

/**
 * @brief 将整数限制在给定范围内
 *
 * @param amt 输入无符号整数
 * @param low 范围下限
 * @param high 范围上限
 * @return uint32_t 限幅输出结果
 */
static int32_t constrain_int32(int32_t amt, int32_t low, int32_t high) {
  return ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)));
}

/**
 * @brief 计算两个数的平方和开根号
 *
 * @param x
 * @param y
 * @return float 输出结果
 */
static inline float get_norm2(float x, float y) { return sqrtf(x * x + y * y); }

/**
 * @brief 计算三个数的平方和开根号
 *
 * @param x
 * @param y
 * @param z
 * @return float 输出结果
 */
static inline float get_norm3(float x, float y, float z) {
  return sqrtf(x * x + y * y + z * z);
}

/** @brief 插入排序算法
 * @param int arr[]被排数列，int n 排序窗宽
 * @retval none
 */
static inline void insert_sort(int arr[], int n) {
  int temp;
  int i, j;
  for (i = 1; i < n; ++i) {
    temp = arr[i];
    for (j = i; j > 0 && temp < arr[j - 1]; --j) {
      arr[j] = arr[j - 1];
    }
    arr[j] = temp;
  }
}

#ifdef __cplusplus
}
#endif