diff --git a/source/maths/Fixed.cpp b/source/maths/Fixed.cpp
index e9e3c8c..38b163e 100644
a
|
b
|
|
1 | | /* Copyright (C) 2010 Wildfire Games. |
| 1 | /* Copyright (C) 2016 Wildfire Games. |
2 | 2 | * This file is part of 0 A.D. |
3 | 3 | * |
4 | 4 | * 0 A.D. is free software: you can redistribute it and/or modify |
… |
… |
CStr8 CFixed_15_16::ToString() const
|
143 | 143 | return r.str(); |
144 | 144 | } |
145 | 145 | |
146 | | // Based on http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization |
147 | 146 | CFixed_15_16 atan2_approx(CFixed_15_16 y, CFixed_15_16 x) |
148 | 147 | { |
149 | | CFixed_15_16 zero; |
| 148 | // Overflow for length of (x,y) > 19910.8. |
| 149 | // Error <= arctan(1/16)/2 ~ 1.788 degrees. |
150 | 150 | |
151 | | // Special case to avoid division-by-zero |
152 | | if (x.IsZero() && y.IsZero()) |
153 | | return zero; |
| 151 | // Binary search using the following angles: |
| 152 | // 45, 26.565, 14.036, 7.125, 3.576 degrees (= arctan(1/2^n)) |
| 153 | // Idea: [1, 2^(-n); -2^(-n), 1] are multiples of the rotation matrices for these angles. |
| 154 | i32 tan_angle_table_rad[5] = {51472, 30386, 16055, 8150, 4091}; |
154 | 155 | |
155 | | CFixed_15_16 c1; |
156 | | c1.SetInternalValue(51472); // pi/4 << 16 |
| 156 | i32 angleint = 2045; // = 1/2*arctan(1/16) << 16 |
| 157 | i32 xint = x.GetInternalValue(), yint = y.GetInternalValue(), tmpint; |
157 | 158 | |
158 | | CFixed_15_16 c2; |
159 | | c2.SetInternalValue(154415); // 3*pi/4 << 16 |
| 159 | // Handle the case (0,0). |
| 160 | if (xint == 0 && yint == 0) |
| 161 | return CFixed_15_16::FromInt(0); |
160 | 162 | |
161 | | CFixed_15_16 abs_y = y.Absolute(); |
162 | | |
163 | | CFixed_15_16 angle; |
164 | | if (x >= zero) |
| 163 | // Transform the problem into the case x,y >= 0, i.e. 0 <= angle <= pi/2 |
| 164 | if (yint >= 0) // (0, pi) |
165 | 165 | { |
166 | | CFixed_15_16 r = (x - abs_y) / (x + abs_y); |
167 | | angle = c1 - c1.Multiply(r); |
| 166 | if (xint < 0) // (pi/2, pi) |
| 167 | { |
| 168 | // (x, y) -> (y, -x) |
| 169 | tmpint = xint; |
| 170 | xint = yint; |
| 171 | yint = -tmpint; |
| 172 | angleint = 102944+2045; // pi/2 << 16 |
| 173 | } |
168 | 174 | } |
169 | | else |
| 175 | else // (-pi, 0) |
170 | 176 | { |
171 | | CFixed_15_16 r = (x + abs_y) / (abs_y - x); |
172 | | angle = c2 - c1.Multiply(r); |
| 177 | if (xint < 0) // (-pi, -pi/2) |
| 178 | { |
| 179 | // (x, y) -> (-x, -y) |
| 180 | xint *= -1; |
| 181 | yint *= -1; |
| 182 | angleint = -205887+2045; // -pi << 16 |
| 183 | } |
| 184 | else // (-pi/2, 0) |
| 185 | { |
| 186 | // (x, y) -> (-y, x) |
| 187 | tmpint = xint; |
| 188 | xint = -yint; |
| 189 | yint = tmpint; |
| 190 | angleint = -102944+2045; // -pi/2 << 16 |
| 191 | } |
173 | 192 | } |
174 | 193 | |
175 | | if (y < zero) |
176 | | return -angle; |
177 | | else |
178 | | return angle; |
179 | | } |
| 194 | // The following will have massive rounding errors for small x, y. If needed, scale them up. E.g.: |
| 195 | // if (xint < 128 && yint < 128) |
| 196 | // { |
| 197 | // xint << 7; |
| 198 | // yint << 7; |
| 199 | // } |
| 200 | for (u8 loop_var = 0; loop_var < 5; ++loop_var) |
| 201 | { |
| 202 | tmpint = xint >> loop_var; |
| 203 | // If remaining angle > arctan(1/2^n) "rotate" counterclockwise. |
| 204 | if (yint >= tmpint) |
| 205 | { |
| 206 | // (x,y) -> (x,y) + 1/2^n * (-y,x) |
| 207 | xint += yint >> loop_var; |
| 208 | yint -= tmpint; |
| 209 | angleint += tan_angle_table_rad[loop_var]; |
| 210 | } |
| 211 | } |
| 212 | // Remaining angle is in [0,arctan(1/16)], arctan(1/16)/2 is the best possible approximation. |
180 | 213 | |
181 | | template<> |
182 | | CFixed_15_16 CFixed_15_16::Pi() |
183 | | { |
184 | | return CFixed_15_16(205887); // = pi << 16 |
| 214 | // If needed, get the value into the desired interval of length 2*pi. |
| 215 | |
| 216 | CFixed_15_16 angle; |
| 217 | angle.SetInternalValue(angleint); |
| 218 | return angle; |
185 | 219 | } |
186 | 220 | |
187 | 221 | void sincos_approx(CFixed_15_16 a, CFixed_15_16& sin_out, CFixed_15_16& cos_out) |
188 | 222 | { |
189 | | // Based on http://www.coranac.com/2009/07/sines/ |
| 223 | // Based on roughly sup-norm optimal approximations of sin+cos, sin-cos on [0,pi/2] |
| 224 | // 2^16*(sin(pi/4*x)-cos(pi/4*x)) ~ 72730*(x-1) - 7210*(x-1)^3 |
| 225 | // 2^16*(sin(pi/4*x)+cos(pi/4*x)) ~ 92682 - 28562*(x-1)^2 + 1416 * (x-1)^4 |
| 226 | // The idea is to use these symmetric terms in order to get both sin and cos |
| 227 | // Errors: e ~ 1.3*10^-4 in both cases. |
190 | 228 | |
191 | | // TODO: this could be made a bit more precise by being careful about scaling |
| 229 | // The error could be decreased to around 1.1*10^-4 with further tweaking. |
| 230 | // TODO : Check if this breaks parts of the program that rely on specifics. |
192 | 231 | |
193 | 232 | typedef CFixed_15_16 fixed; |
194 | 233 | |
195 | | fixed c2_pi; |
196 | | c2_pi.SetInternalValue(41721); // = 2/pi << 16 |
| 234 | fixed c4_pi; |
| 235 | c4_pi.SetInternalValue(83443); // = 4/pi << 16 |
197 | 236 | |
198 | | // Map radians onto the range [0, 4) |
199 | | fixed z = a.Multiply(c2_pi) % fixed::FromInt(4); |
| 237 | // Map radians onto the range [0, 8) |
| 238 | fixed z = a.Multiply(c4_pi) % fixed::FromInt(8); |
200 | 239 | |
201 | | // Map z onto the range [-1, +1] for sin, and the same with z+1 to compute cos |
202 | | fixed sz, cz; |
203 | | if (z >= fixed::FromInt(3)) // [3, 4) |
204 | | { |
205 | | sz = z - fixed::FromInt(4); |
206 | | cz = z - fixed::FromInt(3); |
207 | | } |
208 | | else if (z >= fixed::FromInt(2)) // [2, 3) |
209 | | { |
210 | | sz = fixed::FromInt(2) - z; |
211 | | cz = z - fixed::FromInt(3); |
212 | | } |
213 | | else if (z >= fixed::FromInt(1)) // [1, 2) |
| 240 | // Map z onto the range [0, 2], compute sin+cos and sin-cos |
| 241 | // Actually compute half of the above. |
| 242 | fixed c1,c2,c3,c4,c5; |
| 243 | c1.SetInternalValue(36365); // ~ 0.5549 |
| 244 | c2.SetInternalValue(3605); // ~ 0.055 |
| 245 | c3.SetInternalValue(46341); // ~ 0.7071 |
| 246 | c4.SetInternalValue(14281); // ~ 0.2179 |
| 247 | c5.SetInternalValue(708); // ~ 0.0108 |
| 248 | |
| 249 | if (z >= fixed::FromInt(4)) // [4,8) |
214 | 250 | { |
215 | | sz = fixed::FromInt(2) - z; |
216 | | cz = fixed::FromInt(1) - z; |
| 251 | if (z >= fixed::FromInt(6)) // [6,8) |
| 252 | { |
| 253 | // sin(3*pi/2+x) = -cos(x), cos(3*pi/2+x) = sin(x) |
| 254 | fixed zn = z - fixed::FromInt(7); |
| 255 | z = zn.Multiply(zn); |
| 256 | fixed scdif = zn.Multiply(c1 - z.Multiply(c2)); |
| 257 | fixed scsum = c3 - z.Multiply(c4 - z.Multiply(c5)); |
| 258 | cos_out = scsum + scdif; |
| 259 | sin_out = scdif - scsum; |
| 260 | } |
| 261 | else // [4,6) |
| 262 | { |
| 263 | // sin(pi+x) = -sin(x), cos(pi+x) = -cos(x) |
| 264 | fixed zn = z - fixed::FromInt(5); |
| 265 | z = zn.Multiply(zn); |
| 266 | fixed scdif= zn.Multiply(c1 - z.Multiply(c2)); |
| 267 | fixed scsum = c3 - z.Multiply(c4 - z.Multiply(c5)); |
| 268 | sin_out = -scsum - scdif; |
| 269 | cos_out = scdif - scsum; |
| 270 | } |
217 | 271 | } |
218 | | else // [0, 1) |
| 272 | else // [0,4) |
219 | 273 | { |
220 | | sz = z; |
221 | | cz = fixed::FromInt(1) - z; |
| 274 | if (z >= fixed::FromInt(2)) // [2,4) |
| 275 | { |
| 276 | // sin (pi/2+x) = cos(x), cos(pi/2+x) = -sin(x) |
| 277 | fixed zn = z - fixed::FromInt(3); |
| 278 | z = zn.Multiply(zn); |
| 279 | fixed scdif = zn.Multiply(c1 - z.Multiply(c2)); |
| 280 | fixed scsum = c3 - z.Multiply(c4 - z.Multiply(c5)); |
| 281 | cos_out = -scsum - scdif; |
| 282 | sin_out = scsum - scdif; |
| 283 | } |
| 284 | else // [0,2) |
| 285 | { |
| 286 | fixed zn = z - fixed::FromInt(1); |
| 287 | z = zn.Multiply(zn); |
| 288 | fixed scdif = zn.Multiply(c1 - z.Multiply(c2)); |
| 289 | fixed scsum = c3 - z.Multiply(c4 - z.Multiply(c5)); |
| 290 | sin_out = scsum + scdif; |
| 291 | cos_out = scsum - scdif; |
| 292 | } |
222 | 293 | } |
223 | | |
224 | | // Third-order (max absolute error ~0.02) |
225 | | |
226 | | // sin_out = (sz / 2).Multiply(fixed::FromInt(3) - sz.Multiply(sz)); |
227 | | // cos_out = (cz / 2).Multiply(fixed::FromInt(3) - cz.Multiply(cz)); |
228 | | |
229 | | // Fifth-order (max absolute error ~0.0005) |
230 | | |
231 | | fixed sz2 = sz.Multiply(sz); |
232 | | sin_out = sz.Multiply(fixed::Pi() - sz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - sz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2; |
233 | | |
234 | | fixed cz2 = cz.Multiply(cz); |
235 | | cos_out = cz.Multiply(fixed::Pi() - cz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - cz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2; |
236 | 294 | } |
diff --git a/source/maths/Fixed.h b/source/maths/Fixed.h
index 68a4cd0..59cc7d1 100644
a
|
b
|
public:
|
126 | 126 | |
127 | 127 | static CFixed Zero() { return CFixed(0); } |
128 | 128 | static CFixed Epsilon() { return CFixed(1); } |
129 | | static CFixed Pi(); |
| 129 | static CFixed Pi() { return CFixed(205887); } // = pi << 16 |
| 130 | |
130 | 131 | |
131 | 132 | T GetInternalValue() const { return value; } |
132 | 133 | void SetInternalValue(T n) { value = n; } |
diff --git a/source/maths/tests/test_Fixed.h b/source/maths/tests/test_Fixed.h
index 389723c..dca2161 100644
a
|
b
|
public:
|
281 | 281 | fixed s, c; |
282 | 282 | |
283 | 283 | sincos_approx(fixed::FromInt(0), s, c); |
284 | | TS_ASSERT_EQUALS(s, fixed::FromInt(0)); |
285 | | TS_ASSERT_EQUALS(c, fixed::FromInt(1)); |
| 284 | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.00015); |
| 285 | TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.00015); |
286 | 286 | |
287 | 287 | sincos_approx(fixed::Pi(), s, c); |
288 | | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.00005); |
289 | | TS_ASSERT_EQUALS(c, fixed::FromInt(-1)); |
| 288 | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.00015); |
| 289 | TS_ASSERT_DELTA(c.ToDouble(),-1.0, 0.00015); |
290 | 290 | |
291 | 291 | sincos_approx(fixed::FromDouble(M_PI*2.0), s, c); |
292 | | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.0001); |
293 | | TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.0001); |
| 292 | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.00015); |
| 293 | TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.00015); |
294 | 294 | |
295 | 295 | sincos_approx(fixed::FromDouble(M_PI*100.0), s, c); |
296 | | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.004); |
297 | | TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.004); |
| 296 | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.0004); |
| 297 | TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.0004); |
298 | 298 | |
299 | 299 | sincos_approx(fixed::FromDouble(M_PI*-100.0), s, c); |
300 | | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.004); |
301 | | TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.004); |
| 300 | TS_ASSERT_DELTA(s.ToDouble(), 0.0, 0.0004); |
| 301 | TS_ASSERT_DELTA(c.ToDouble(), 1.0, 0.0004); |
302 | 302 | |
303 | 303 | /* |
304 | 304 | for (double a = 0.0; a < 6.28; a += 0.1) |
… |
… |
public:
|
317 | 317 | } |
318 | 318 | // printf("### Max error %f = %f/2^16\n", err, err*65536.0); |
319 | 319 | |
320 | | TS_ASSERT_LESS_THAN(err, 0.00046); |
| 320 | TS_ASSERT_LESS_THAN(err, 0.00015); |
321 | 321 | } |
322 | 322 | }; |