You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
255 lines
5.4 KiB
255 lines
5.4 KiB
#pragma once
|
|
|
|
|
|
#include "gp/algorithms/repeat.hpp"
|
|
#include "gp/math/details/math_definitions.hpp"
|
|
|
|
#include <limits>
|
|
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
|
|
namespace gp{
|
|
namespace math{
|
|
|
|
template<>
|
|
constexpr float pi<float> = 3.1415926535897932384626433832795028841971693993751058209749445923078164062;
|
|
|
|
template<>
|
|
constexpr double pi<double> = 3.1415926535897932384626433832795028841971693993751058209749445923078164062L;
|
|
|
|
|
|
template<>
|
|
float abs<float>(float value) {
|
|
static_assert(sizeof(float) == 4, "bad float size");
|
|
union {
|
|
float fp;
|
|
uint32_t ab;
|
|
} p;
|
|
p.fp = value;
|
|
p.ab &= 0x7fFFffFF;
|
|
return p.fp;
|
|
}
|
|
|
|
template<>
|
|
double abs<double>(double value) {
|
|
static_assert(sizeof(double) == 8, "bad double size");
|
|
union {
|
|
double fp;
|
|
uint64_t ab;
|
|
} p;
|
|
p.fp = value;
|
|
p.ab &= 0x7fFFffFFffFFffFF;
|
|
return p.fp;
|
|
}
|
|
|
|
template<typename T>
|
|
T floor(T);
|
|
|
|
template<>
|
|
float floor<float>(float value) {
|
|
static_assert(sizeof(float) == 4, "bad float size");
|
|
if(
|
|
value >= 16777216
|
|
|| value <= std::numeric_limits<int32_t>::min()
|
|
|| value != value
|
|
) {
|
|
return value;
|
|
}
|
|
int32_t ret = value;
|
|
float ret_d = ret;
|
|
if(value == ret_d || value >= 0) {
|
|
return ret;
|
|
} else {
|
|
return ret-1;
|
|
}
|
|
}
|
|
|
|
template<>
|
|
double floor<double>(double value) {
|
|
static_assert(sizeof(double) == 8, "bad double size");
|
|
if(
|
|
value >= 9007199254740992
|
|
|| value <= std::numeric_limits<int64_t>::min()
|
|
|| value != value
|
|
) {
|
|
return value;
|
|
}
|
|
int64_t ret = value;
|
|
double ret_d = ret;
|
|
if(value == ret_d || value >= 0) {
|
|
return ret;
|
|
} else {
|
|
return ret-1;
|
|
}
|
|
}
|
|
|
|
template<>
|
|
float sign<float>(float value) {
|
|
static_assert(sizeof(float) == 4, "bad float size");
|
|
if(!value) return 0;
|
|
union {
|
|
float fp;
|
|
uint32_t ab;
|
|
} p;
|
|
p.fp = value;
|
|
p.ab &= 0x7fFFffFF;
|
|
return value/p.fp;
|
|
}
|
|
|
|
template<>
|
|
double sign<double>(double value) {
|
|
static_assert(sizeof(double) == 8, "bad double size");
|
|
if(!value) return 0;
|
|
union {
|
|
double fp;
|
|
uint64_t ab;
|
|
} p;
|
|
p.fp = value;
|
|
p.ab &= 0x7fFFffFFffFFffFF;
|
|
return value/p.fp;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Calculate the sin of a value using Taylor's method
|
|
*
|
|
* @tparam steps The number of steps to do at the maximum
|
|
* @tparam T the type of value and the return type expected
|
|
* @tparam accuracy the maximum accuracy to shoot for (early stopping)
|
|
* @param value The value to calculate the sin of. Works better for values close to 0.
|
|
* @return T the sin of the value (the sign may be off idk I don't remember)
|
|
*/
|
|
template<size_t steps, typename T, size_t accuracy = 1000000>
|
|
T sin_taylor(T value) {
|
|
const T acc = T{1}/T{accuracy};
|
|
T B = value;
|
|
T C = 1;
|
|
T ret = B/C;
|
|
for(size_t i = 1; (i < steps) && (abs<>(B/C) > acc); ++i) {
|
|
B *= -1*value*value;
|
|
C *= 2*i*(2*i+1);
|
|
ret += B/C;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
/**
|
|
* @brief General purpose sin function
|
|
*
|
|
* @param v
|
|
* @return float
|
|
*/
|
|
inline float sin(float v) {
|
|
// limit the range between -pi and +pi
|
|
v += pi<float>;
|
|
v = v - 2*pi<float>*floor(v/(2*pi<float>));
|
|
v -= pi<float>;
|
|
float s = sign(v);
|
|
v *= s;
|
|
|
|
// use taylor's method on the value
|
|
return sin_taylor<10>(v)*s;
|
|
}
|
|
|
|
/**
|
|
* @brief General purpose sin function
|
|
*
|
|
* @param v
|
|
* @return double
|
|
*/
|
|
inline double sin(double v) {
|
|
v += pi<double>;
|
|
v = v - 2*pi<double>*floor(v/(2*pi<double>));
|
|
v -= pi<double>;
|
|
float s = sign(v);
|
|
v *= s;
|
|
return sin_taylor<10>(v)*s;
|
|
}
|
|
|
|
|
|
// TODO: replace with an actual implementation
|
|
inline float cos(float v) {
|
|
return sin(v+pi<float>/2);
|
|
}
|
|
|
|
// TODO: replace with an actual implementation
|
|
inline double cos(double v) {
|
|
return sin(v+pi<double>/2);
|
|
}
|
|
|
|
// TODO: replace with an actual implementation
|
|
inline float tan(float v) {
|
|
return sin(v)/cos(v);
|
|
}
|
|
|
|
// TODO: replace with an actual implementation
|
|
inline double tan(double v) {
|
|
return sin(v)/cos(v);
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief Quake isqrt (x) -> 1/sqrt(x)
|
|
*
|
|
* @tparam cycles the number of newton method cycles to apply
|
|
* @param v the value to apply the function on
|
|
* @return float \f$\frac{1}{\sqrt{v}}\f$
|
|
*/
|
|
template<size_t cycles = 5>
|
|
float isqrt(float v) {
|
|
int32_t i;
|
|
float x2, y;
|
|
constexpr float threehalfs = 1.5F;
|
|
|
|
x2 = v * 0.5F;
|
|
y = v;
|
|
i = * ( int32_t * ) &y;
|
|
i = 0x5F375A86 - ( i >> 1 );
|
|
y = * ( float * ) &i;
|
|
gp::repeat(cycles, [&](){
|
|
y = y * ( threehalfs - ( x2 * y * y ) );
|
|
});
|
|
return y;
|
|
}
|
|
|
|
/**
|
|
* @brief Quake isqrt (x) -> 1/sqrt(x) but for doubles
|
|
*
|
|
* @tparam cycles the number of newton method cycles to apply
|
|
* @param v the value to apply the function on
|
|
* @return double \f$\frac{1}{\sqrt{v}}\f$
|
|
*/
|
|
template<size_t cycles = 5>
|
|
double isqrt(double v) {
|
|
int64_t i;
|
|
double x2, y;
|
|
constexpr double threehalfs = 1.5F;
|
|
|
|
x2 = v * 0.5F;
|
|
y = v;
|
|
i = * ( int64_t * ) &y;
|
|
i = 0x5FE6EB50C7B537A9 - ( i >> 1 );
|
|
y = * ( double * ) &i;
|
|
gp::repeat(cycles, [&](){
|
|
y = y * ( threehalfs - ( x2 * y * y ) );
|
|
});
|
|
return y;
|
|
}
|
|
|
|
/**
|
|
* @brief A faster version of the Quake isqrt (actually the same but with defined cycles)
|
|
*
|
|
* @param v
|
|
* @return float
|
|
*/
|
|
inline float fast_isqrt(float v) {return isqrt<1>(v);}
|
|
/**
|
|
* @brief A faster version of the Quake isqrt (actually the same but with defined cycles)
|
|
*
|
|
* @param v
|
|
* @return double
|
|
*/
|
|
inline double fast_isqrt(double v) {return isqrt<1>(v);}
|
|
}
|
|
}
|