99 lines
2.5 KiB
C++
99 lines
2.5 KiB
C++
/**
|
|
* @file basic_function.hpp
|
|
* @brief basic function
|
|
* @author DeepRobotics
|
|
* @version 1.0
|
|
* @date 2025-11-07
|
|
*
|
|
* @copyright Copyright (c) 2025 DeepRobotics
|
|
*
|
|
*/
|
|
#pragma once
|
|
#include "common_types.h"
|
|
#include "custom_types.h"
|
|
|
|
using namespace types;
|
|
|
|
inline float Deg2Rad(float deg){
|
|
return deg /180.f * M_PI;
|
|
}
|
|
|
|
inline float Rad2Deg(float rad){
|
|
return rad / M_PI * 180.;
|
|
}
|
|
|
|
inline float LimitNumber(float data, float limit){
|
|
limit = fabs(limit);
|
|
if(data > limit) data = limit;
|
|
else if(data < -limit) data = -limit;
|
|
return data;
|
|
}
|
|
|
|
inline float LimitNumber(float data, float low, float high){
|
|
if(low > high){
|
|
std::cerr << "error limit range" << std::endl;
|
|
return data;
|
|
}
|
|
if(data > high) return high;
|
|
if(data < low) return low;
|
|
return data;
|
|
}
|
|
|
|
inline Mat3f RpyToRm(const Vec3f &rpy){
|
|
Eigen::AngleAxisf yawAngle(rpy(2), Vec3f::UnitZ());
|
|
Eigen::AngleAxisf pitchAngle(rpy(1), Vec3f::UnitY());
|
|
Eigen::AngleAxisf rollAngle(rpy(0), Vec3f::UnitX());
|
|
Eigen::Quaternion<float> q = yawAngle*pitchAngle*rollAngle;
|
|
return q.matrix();
|
|
}
|
|
|
|
inline float NormalizeAngle(float angle) {
|
|
float result = std::fmod(angle, 2 * M_PI);
|
|
if (result < 0) {
|
|
result += 2 * M_PI;
|
|
}
|
|
if (result > M_PI) {
|
|
result -= 2 * M_PI;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
inline float Sign(const float &i){
|
|
if (i > 0) {
|
|
return 1.;
|
|
} else if (i == 0) {
|
|
return 0.;
|
|
} else {
|
|
return -1.;
|
|
}
|
|
}
|
|
|
|
inline double GetTimestampMs(){
|
|
static timespec startup_timestamp;
|
|
timespec now_timestamp;
|
|
if (startup_timestamp.tv_sec + startup_timestamp.tv_nsec == 0) {
|
|
clock_gettime(CLOCK_MONOTONIC,&startup_timestamp);
|
|
}
|
|
clock_gettime(CLOCK_MONOTONIC,&now_timestamp);
|
|
return (now_timestamp.tv_sec-startup_timestamp.tv_sec)*1e3 + (now_timestamp.tv_nsec-startup_timestamp.tv_nsec)/1e6;
|
|
}
|
|
|
|
inline float GetCubicSplinePos(float x0, float v0, float xf, float vf, float t, float T){
|
|
if(t >= T) return xf;
|
|
float a, b, c, d;
|
|
d = x0;
|
|
c = v0;
|
|
a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3);
|
|
b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2);
|
|
return a*pow(t, 3)+b*pow(t, 2)+c*t+d;
|
|
}
|
|
|
|
inline float GetCubicSplineVel(float x0, float v0, float xf, float vf, float t, float T){
|
|
if(t >= T) return 0;
|
|
float a, b, c;
|
|
c = v0;
|
|
a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3);
|
|
b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2);
|
|
return 3.*a*pow(t, 2) + 2.*b*t + c;
|
|
}
|