diff --git a/source/maths/Fixed.cpp b/source/maths/Fixed.cpp index 76d9941467..445985e926 100644 --- a/source/maths/Fixed.cpp +++ b/source/maths/Fixed.cpp @@ -133,8 +133,51 @@ CFixed_15_16 CFixed_15_16::Pi() void sincos_approx(CFixed_15_16 a, CFixed_15_16& sin_out, CFixed_15_16& cos_out) { - // XXX: mustn't use floating-point here - need a fixed-point emulation + // Based on http://www.coranac.com/2009/07/sines/ - sin_out = CFixed_15_16::FromDouble(sin(a.ToDouble())); - cos_out = CFixed_15_16::FromDouble(cos(a.ToDouble())); + // TODO: this could be made a bit more precise by being careful about scaling + + typedef CFixed_15_16 fixed; + + fixed c2_pi; + c2_pi.SetInternalValue(41721); // = 2/pi << 16 + + // Map radians onto the range [0, 4) + fixed z = a.Multiply(c2_pi) % fixed::FromInt(4); + + // Map z onto the range [-1, +1] for sin, and the same with z+1 to compute cos + fixed sz, cz; + if (z >= fixed::FromInt(3)) // [3, 4) + { + sz = z - fixed::FromInt(4); + cz = z - fixed::FromInt(3); + } + else if (z >= fixed::FromInt(2)) // [2, 3) + { + sz = fixed::FromInt(2) - z; + cz = z - fixed::FromInt(3); + } + else if (z >= fixed::FromInt(1)) // [1, 2) + { + sz = fixed::FromInt(2) - z; + cz = fixed::FromInt(1) - z; + } + else // [0, 1) + { + sz = z; + cz = fixed::FromInt(1) - z; + } + + // Third-order (max absolute error ~0.02) + +// sin_out = (sz / 2).Multiply(fixed::FromInt(3) - sz.Multiply(sz)); +// cos_out = (cz / 2).Multiply(fixed::FromInt(3) - cz.Multiply(cz)); + + // Fifth-order (max absolute error ~0.0005) + + fixed sz2 = sz.Multiply(sz); + sin_out = sz.Multiply(fixed::Pi() - sz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - sz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2; + + fixed cz2 = cz.Multiply(cz); + cos_out = cz.Multiply(fixed::Pi() - cz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - cz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2; } diff --git a/source/maths/Fixed.h b/source/maths/Fixed.h index 2a07e429f6..859fba99c3 100644 --- a/source/maths/Fixed.h +++ b/source/maths/Fixed.h @@ -234,6 +234,18 @@ public: return CFixed(value / n); } + /// Mod by a fixed. Must not have n == 0. Result has the same sign as n. + CFixed operator%(CFixed n) const + { + T t = value % n.value; + if (n.value > 0 && t < 0) + t += n.value; + else if (n.value < 0 && t > 0) + t += n.value; + + return CFixed(t); + } + CFixed Absolute() const { return CFixed(abs(value)); } /** @@ -307,6 +319,7 @@ CFixed_15_16 atan2_approx(CFixed_15_16 y, CFixed_15_16 x); /** * Compute sin(a) and cos(a). + * Maximum error for -2pi < a < 2pi is almost 0.0005. */ void sincos_approx(CFixed_15_16 a, CFixed_15_16& sin_out, CFixed_15_16& cos_out); diff --git a/source/maths/tests/test_Fixed.h b/source/maths/tests/test_Fixed.h index ba4f85d2de..d78a628ccc 100644 --- a/source/maths/tests/test_Fixed.h +++ b/source/maths/tests/test_Fixed.h @@ -134,6 +134,20 @@ public: // TODO: test all the arithmetic operators + void test_Mod() + { + TS_ASSERT_EQUALS(fixed::FromInt(0) % fixed::FromInt(4), fixed::FromInt(0)); + TS_ASSERT_EQUALS(fixed::FromInt(0) % fixed::FromInt(-4), fixed::FromInt(0)); + TS_ASSERT_EQUALS(fixed::FromInt(5) % fixed::FromInt(4), fixed::FromInt(1)); + TS_ASSERT_EQUALS(fixed::FromInt(5) % fixed::FromInt(-4), fixed::FromInt(-3)); + TS_ASSERT_EQUALS(fixed::FromInt(-5) % fixed::FromInt(4), fixed::FromInt(3)); + TS_ASSERT_EQUALS(fixed::FromInt(-5) % fixed::FromInt(-4), fixed::FromInt(-1)); + + TS_ASSERT_EQUALS((fixed::FromDouble(5.5) % fixed::FromInt(4)).ToDouble(), 1.5); + + TS_ASSERT_EQUALS((fixed::FromDouble(1.75) % fixed::FromDouble(0.5)).ToDouble(), 0.25); + } + void test_Sqrt() { TS_ASSERT_EQUALS(fixed::FromDouble(1.0).Sqrt().ToDouble(), 1.0); @@ -188,4 +202,48 @@ public: #undef T } + + void test_SinCos() + { + fixed s, c; + + sincos_approx(fixed::FromInt(0), s, c); + TS_ASSERT_EQUALS(s, fixed::FromInt(0)); + TS_ASSERT_EQUALS(c, fixed::FromInt(1)); + + sincos_approx(fixed::Pi(), s, c); + TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.00005); + TS_ASSERT_EQUALS(c, fixed::FromInt(-1)); + + sincos_approx(fixed::FromDouble(M_PI*2.0), s, c); + TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.0001); + TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.0001); + + sincos_approx(fixed::FromDouble(M_PI*100.0), s, c); + TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.004); + TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.004); + + sincos_approx(fixed::FromDouble(M_PI*-100.0), s, c); + TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.004); + TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.004); + +/* + for (double a = 0.0; a < 6.28; a += 0.1) + { + sincos_approx(fixed::FromDouble(a), s, c); + printf("%f: sin = %f / %f; cos = %f / %f\n", a, s.ToDouble(), sin(a), c.ToDouble(), cos(a)); + } +*/ + + double err = 0.0; + for (double a = -6.28; a < 6.28; a += 0.001) + { + sincos_approx(fixed::FromDouble(a), s, c); + err = std::max(err, fabs(sin(a) - s.ToDouble())); + err = std::max(err, fabs(cos(a) - c.ToDouble())); + } +// printf("### Max error %f = %f/2^16\n", err, err*65536.0); + + TS_ASSERT_LESS_THAN(err, 0.00046); + } };