diff --git a/common/include/maths.h b/common/include/maths.h index 2df6db764..b74864121 100644 --- a/common/include/maths.h +++ b/common/include/maths.h @@ -125,24 +125,31 @@ u_int32_t quad_sqrt (quadint); __attribute_warn_unused_result fix fix_sqrt (fix a); +struct fix_sincos_result +{ + fix sin, cos; +}; + +__attribute_warn_unused_result +fix_sincos_result fix_sincos(fix); + //compute sine and cosine of an angle, filling in the variables //either of the pointers can be NULL -void fix_sincos (fix a, fix * s, fix * c); //with interpolation +static inline void fix_sincos (fix a, fix *s, fix *c) //with interpolation +{ + const auto &r = fix_sincos(a); + if (s) + *s = r.sin; + if (c) + *c = r.cos; +} void fix_sincos(fix, std::nullptr_t, std::nullptr_t) = delete; __attribute_warn_unused_result -static inline fix fix_sin(fix a) -{ - fix s; - return fix_sincos(a, &s, nullptr), s; -} +fix fix_sin(fix a); __attribute_warn_unused_result -static inline fix fix_cos(fix a) -{ - fix c; - return fix_sincos(a, nullptr, &c), c; -} +fix fix_cos(fix a); __attribute_warn_unused_result fix fix_fastsin(fix a); //no interpolation diff --git a/common/maths/fixc.cpp b/common/maths/fixc.cpp index f8c24a979..d55aaa812 100644 --- a/common/maths/fixc.cpp +++ b/common/maths/fixc.cpp @@ -20,6 +20,21 @@ #define EPSILON (F1_0/100) +namespace { + +class fix_sincos_input +{ +public: + const unsigned m_idx, m_mul; + fix_sincos_input(fix a) : + m_idx(static_cast(a >> 8)), + m_mul(static_cast(a)) + { + } +}; + +} + fix64 fixmul64(fix a, fix b) { const fix64 a64 = a; @@ -199,28 +214,36 @@ fix fix_sqrt(fix a) return static_cast(long_sqrt(a)) << 8; } +static fix fix_sin(const fix_sincos_input sci) +{ + fix ss = sincos_table[sci.m_idx]; + return (ss + (((sincos_table[sci.m_idx + 1] - ss) * sci.m_mul) >> 8)) << 2; +} + +__attribute_warn_unused_result +static fix fix_cos(const fix_sincos_input sci) +{ + fix cc = sincos_table[sci.m_idx + 64]; + return (cc + (((sincos_table[sci.m_idx + 64 + 1] - cc) * sci.m_mul) >> 8)) << 2; +} //compute sine and cosine of an angle, filling in the variables //either of the pointers can be NULL //with interpolation -void fix_sincos(fix a,fix *s,fix *c) +fix_sincos_result fix_sincos(fix a) { - int i,f; + fix_sincos_input i(a); + return {fix_sin(i), fix_cos(i)}; +} - i = (a>>8)&0xff; - f = a&0xff; +fix fix_sin(fix a) +{ + return fix_sin(fix_sincos_input{a}); +} - if (s) - { - fix ss = sincos_table[i]; - *s = (ss + (((sincos_table[i+1] - ss) * f)>>8))<<2; - } - - if (c) - { - fix cc = sincos_table[i+64]; - *c = (cc + (((sincos_table[i+64+1] - cc) * f)>>8))<<2; - } +fix fix_cos(fix a) +{ + return fix_cos(fix_sincos_input{a}); } //compute sine and cosine of an angle, filling in the variables