from ![]()
CWRUtil.hpp:41
struct CWRUtil
{
class Q10;
class Q15;
class Q5
{
public:
Q5() : v(0) { }
explicit Q5(int i) : v(i) { }
Q5(const Q10 q10) : v(int(q10) / Rasterizer::POLY_BASE_SIZE) { }
operator int() const
{
return v;
}
Q5 operator-() const
{
return Q5(-v);
}
Q5 operator+(const Q5& q5) const
{
return Q5(v + q5.v);
}
Q5 operator-(const Q5& q5) const
{
return Q5(v - q5.v);
}
Q10 operator*(const Q5& q5) const
{
return Q10(v * q5.v);
}
Q5 operator*(const Q15& q15) const
{
int32_t remainder;
return Q5(muldiv(v, int(q15), Rasterizer::POLY_BASE_SIZE * Rasterizer::POLY_BASE_SIZE * Rasterizer::POLY_BASE_SIZE, remainder));
}
Q5 operator*(const int i) const
{
return Q5(v * i);
}
Q5 operator/(const int i) const
{
return Q5(v / i);
}
Q5 operator/(const Q5 q5) const
{
return Q5(int(v) * Rasterizer::POLY_BASE_SIZE / q5.v);
}
template <typename T>
T to() const
{
return v / (T)Rasterizer::POLY_BASE_SIZE;
}
private:
int32_t v;
};
class Q10
{
public:
Q10() : v(0) { }
explicit Q10(int i) : v(i) { }
operator int() const
{
return v;
}
Q10 operator-() const
{
return Q10(-v);
}
Q10 operator+(const Q10& q10) const
{
return Q10(v + q10.v);
}
Q15 operator*(const Q5& q5) const
{
return Q15(v * int(q5));
}
Q5 operator/(const Q5& q5) const
{
return Q5(v / int(q5));
}
private:
int32_t v;
};
class Q15
{
public:
explicit Q15(int i) : v(i) { }
operator int() const
{
return v;
}
Q15 operator-() const
{
return Q15(-v);
}
Q15 operator+(const Q15& q15) const
{
return Q15(v + q15.v);
}
Q10 operator/(const Q5& q5) const
{
return Q10(v / int(q5));
}
private:
int32_t v;
};
#ifdef __ICCARM__
FORCE_INLINE_FUNCTION
#endif
template <typename T>
#ifndef __ICCARM__
FORCE_INLINE_FUNCTION
#endif
static Q5 toQ5(T value)
{
return Q5(int(value * Rasterizer::POLY_BASE_SIZE));
}
#ifdef __ICCARM__
FORCE_INLINE_FUNCTION
#endif
template <typename T>
#ifndef __ICCARM__
FORCE_INLINE_FUNCTION
#endif
static Q10 toQ10(T value)
{
return Q10(int(value * Rasterizer::POLY_BASE_SIZE * Rasterizer::POLY_BASE_SIZE));
}
static Q15 sine(int i)
{
const static uint16_t sineTable[91] =
{
0x0000, 0x023C, 0x0478, 0x06B3, 0x08EE, 0x0B28, 0x0D61, 0x0F99, 0x11D0, 0x1406,
0x163A, 0x186C, 0x1A9D, 0x1CCB, 0x1EF7, 0x2121, 0x2348, 0x256C, 0x278E, 0x29AC,
0x2BC7, 0x2DDF, 0x2FF3, 0x3203, 0x3410, 0x3618, 0x381D, 0x3A1C, 0x3C18, 0x3E0E,
0x4000, 0x41ED, 0x43D4, 0x45B7, 0x4794, 0x496B, 0x4B3D, 0x4D08, 0x4ECE, 0x508E,
0x5247, 0x53FA, 0x55A6, 0x574C, 0x58EB, 0x5A82, 0x5C13, 0x5D9D, 0x5F1F, 0x609A,
0x620E, 0x637A, 0x64DE, 0x663A, 0x678E, 0x68DA, 0x6A1E, 0x6B5A, 0x6C8D, 0x6DB8,
0x6EDA, 0x6FF4, 0x7104, 0x720D, 0x730C, 0x7402, 0x74EF, 0x75D3, 0x76AE, 0x7780,
0x7848, 0x7907, 0x79BC, 0x7A68, 0x7B0B, 0x7BA3, 0x7C33, 0x7CB8, 0x7D34, 0x7DA6,
0x7E0E, 0x7E6D, 0x7EC1, 0x7F0C, 0x7F4C, 0x7F83, 0x7FB0, 0x7FD3, 0x7FEC, 0x7FFB, 0x8000
};
i = ((i % 360) + 360) % 360;
if (i <= 90)
{
return Q15(sineTable[i]);
}
if (i <= 180)
{
return Q15(sineTable[180 - i]);
}
if (i <= 270)
{
return Q15(-int32_t(sineTable[i - 180]));
}
return Q15(-int32_t(sineTable[360 - i]));
}
static Q15 sine(Q5 i)
{
Q5 _360 = toQ5<int>(360);
i = Q5(((i % _360) + _360) % _360);
int16_t fraction = i % Rasterizer::POLY_BASE_SIZE;
Q15 sineLow = sine(i.to<int>());
if (fraction == 0)
{
return sineLow;
}
Q15 sineHigh = sine(i.to<int>() + 1);
int32_t remainder;
return Q15(muldiv(int(sineHigh - sineLow), fraction, Rasterizer::POLY_BASE_SIZE, remainder)) + sineLow;
}
static Q15 cosine(int i)
{
return sine(90 - i);
}
static Q15 cosine(Q5 i)
{
return sine(CWRUtil::toQ5<int>(90) - i);
}
static int8_t arcsine(Q10 q10)
{
const static uint8_t arcsineTable[91] =
{
0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 5, 5, 6, 6,
7, 7, 8, 8, 9, 9, 9, 10, 10, 11, 11, 12, 12, 13, 13,
14, 14, 14, 15, 15, 16, 16, 17, 17, 18, 18, 19, 19, 20, 20,
21, 21, 22, 22, 23, 23, 23, 24, 24, 25, 25, 26, 26, 27, 27,
28, 28, 29, 29, 30, 31, 31, 32, 32, 33, 33, 34, 34, 35, 35,
36, 36, 37, 38, 38, 39, 39, 40, 40, 41, 42, 42, 43, 43, 44, 45
};
if (q10 < 0)
{
return -arcsine(-q10);
}
if (q10 > toQ10<int>(1))
{
return 0;
}
if (int(q10) > 724)
{
return 90 - arcsine(Q10(isqrt((1 << (Rasterizer::POLY_BASE_SHIFT * 4)) - int(q10) * int(q10))));
}
int q7 = (int(q10) + 3) >> 3;
return arcsineTable[q7];
}
template <typename T>
static int angle(T x, T y)
{
Q5 dist;
return angle(toQ5<T>(x), toQ5<T>(y), dist);
}
template <typename T>
static int angle(T x, T y, T& d)
{
Q5 dist;
int a = angle(toQ5<T>(x), toQ5<T>(y), dist);
d = dist.to<T>();
return a;
}
static int angle(Q5 x, Q5 y)
{
Q5 dist;
return angle(x, y, dist);
}
static int angle(Q5 x, Q5 y, Q5& d)
{
if (x >= 0)
{
if (y >= 0)
{
return 90 + _angle(x, y, d);
}
else
{
return 90 - _angle(x, -y, d);
}
}
if (y >= 0)
{
return 270 - _angle(-x, y, d);
}
return 270 + _angle(-x, -y, d);
}
static Q5 sqrtQ10(Q10 value)
{
return Q5(isqrt(uint32_t(int(value))));
}
static Q5 muldivQ5(Q5 factor1, Q5 factor2, Q5 divisor)
{
int32_t remainder;
return Q5(muldiv(int(factor1), int(factor2), int(divisor), remainder));
}
static Q5 mulQ5(Q5 factor1, Q5 factor2)
{
return muldivQ5(factor1, factor2, CWRUtil::toQ5<int>(1));
}
private:
static int _angle(Q5 x, Q5 y, Q5& d)
{
assert(x >= 0 && y >= 0);
if (x < y)
{
return 90 - _angle(y, x, d);
}
Q5 _1 = toQ5<int>(1);
d = sqrtQ10(x * x + y * y);
if (d == 0)
{
return 0;
}
int32_t remainder;
Q10 dy = Q10(muldiv(int(y), int(_1 * _1), int(d), remainder));
return arcsine(dy);
}
static uint32_t isqrt(uint32_t n)
{
uint32_t root = 0, bit, trial;
bit = (n >= 0x10000) ? 1 << 30 : 1 << 14;
do
{
trial = root + bit;
if (n >= trial)
{
n -= trial;
root = trial + bit;
}
root >>= 1;
bit >>= 2;
}
while (bit);
return root;
}
};