128 lines
4.1 KiB
C++
128 lines
4.1 KiB
C++
/*
|
||
* math_inc.h
|
||
*
|
||
* Created on: 20 дек. 2017 г.
|
||
* Author: titov
|
||
*/
|
||
|
||
#ifndef SOURCE_COMMON_MATH_INC_H_
|
||
#define SOURCE_COMMON_MATH_INC_H_
|
||
|
||
#include <cmath>
|
||
#include <utility>
|
||
|
||
namespace math {
|
||
namespace constants {
|
||
|
||
constexpr float pi2 = 3.141592653589793238462643383279502884197169399375105820974944f * 2.0f;
|
||
constexpr float pi = 3.141592653589793238462643383279502884197169399375105820974944f * 1.0f;
|
||
constexpr float pi_2 = 3.141592653589793238462643383279502884197169399375105820974944f / 2.0f;
|
||
constexpr float pi_3 = 3.141592653589793238462643383279502884197169399375105820974944f / 3.0f;
|
||
constexpr float pi_4 = 3.141592653589793238462643383279502884197169399375105820974944f / 4.0f;
|
||
constexpr float pi_6 = 3.141592653589793238462643383279502884197169399375105820974944f / 6.0f;
|
||
|
||
constexpr float pi0_3 = 3.141592653589793238462643383279502884197169399375105820974944f * 0.0f / 3.0f;
|
||
constexpr float pi1_3 = pi_3;
|
||
constexpr float pi2_3 = 3.141592653589793238462643383279502884197169399375105820974944f * 2.0f / 3.0f;
|
||
constexpr float pi3_3 = pi;
|
||
constexpr float pi4_3 = 3.141592653589793238462643383279502884197169399375105820974944f * 4.0f / 3.0f;
|
||
constexpr float pi5_3 = 3.141592653589793238462643383279502884197169399375105820974944f * 5.0f / 3.0f;
|
||
constexpr float pi6_3 = pi2;
|
||
|
||
constexpr float _1_pi = 0.3183098861837906715f;
|
||
constexpr float _2_pi = 0.6366197723675813430f;
|
||
|
||
constexpr float sqrt3 = 1.73205080757f;
|
||
constexpr float sqrt3_2 = 1.73205080757f / 2.0f;
|
||
|
||
constexpr float sqrt2 = 1.4142135623730950488f;
|
||
constexpr float _1_sqrt2 = 0.7071067811865475244f;
|
||
|
||
constexpr float _2_sqrtpi = 1.1283791670955125739f;
|
||
|
||
constexpr float e = 2.7182818284590452354f;
|
||
constexpr float log2e = 1.4426950408889634074f;
|
||
constexpr float log10e = 0.4342944819032518276f;
|
||
constexpr float ln2 = 0.6931471805599453094f;
|
||
constexpr float ln10 = 2.3025850929940456840f;
|
||
|
||
}
|
||
|
||
namespace function {
|
||
|
||
//!Функция вычисляет время для заданного перемещения с заданными ограничениями скорости и ускорения для равноускоренного профиля движения.
|
||
inline float get_time( float s, float v, float a ) {
|
||
using namespace std;
|
||
|
||
float v_max = sqrt( s / a ) * a;
|
||
|
||
if( v_max > v )
|
||
return s / v + v / a;
|
||
else
|
||
return sqrt( s / a ) * 2;
|
||
}
|
||
|
||
//!Функция
|
||
inline void sincos(float angle, float * sin_val, float * cos_val) {
|
||
|
||
using namespace std;
|
||
|
||
*sin_val = sin(angle);
|
||
*cos_val = cos(angle);
|
||
|
||
}
|
||
|
||
inline float isqrt(float val ) {
|
||
|
||
using namespace std;
|
||
|
||
return 1.0f / sqrt(val);
|
||
|
||
}
|
||
|
||
inline float q_rsqrt( float number ) {
|
||
|
||
using namespace std;
|
||
|
||
long i;
|
||
float x2, y;
|
||
const float threehalfs = 1.5f;
|
||
|
||
x2 = number * 0.5f;
|
||
y = number;
|
||
i = * ( long * ) &y; // evil floating point bit level hacking
|
||
i = 0x5f3759dfl - ( i >> 1 );
|
||
y = * ( float * ) &i;
|
||
y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration
|
||
y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed
|
||
|
||
return y;
|
||
}
|
||
|
||
inline float saturation( float value, float max, float min ) {
|
||
|
||
using namespace std;
|
||
|
||
return fmaxf( fminf( value, max ), min );
|
||
|
||
}
|
||
|
||
inline float turn( float value ) {
|
||
return value > math::constants::pi ? value - math::constants::pi2 :
|
||
value <= -math::constants::pi ? value + math::constants::pi2 :
|
||
value;
|
||
}
|
||
|
||
inline std::pair<float, float> calculate_kb( float x1, float x2, float y1, float y2 ) {
|
||
|
||
float coefficient = ( y2 - y1 ) / ( x2 - x1 );
|
||
float offset = y1 - x1 * coefficient;
|
||
|
||
return { coefficient, offset };
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
#endif /* SOURCE_COMMON_MATH_INC_H_ */
|