Line data Source code
1 : /* Copyright (C) 2010 Wildfire Games.
2 : * This file is part of 0 A.D.
3 : *
4 : * 0 A.D. is free software: you can redistribute it and/or modify
5 : * it under the terms of the GNU General Public License as published by
6 : * the Free Software Foundation, either version 2 of the License, or
7 : * (at your option) any later version.
8 : *
9 : * 0 A.D. is distributed in the hope that it will be useful,
10 : * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 : * GNU General Public License for more details.
13 : *
14 : * You should have received a copy of the GNU General Public License
15 : * along with 0 A.D. If not, see <http://www.gnu.org/licenses/>.
16 : */
17 :
18 : #include "precompiled.h"
19 :
20 : #include "Fixed.h"
21 :
22 : #include "ps/CStr.h"
23 :
24 : #include <sstream>
25 :
26 : template<>
27 65617 : CFixed_15_16 CFixed_15_16::FromString(const CStr8& s)
28 : {
29 : // Parse a superset of the xsd:decimal syntax: [-+]?\d*(\.\d*)?
30 :
31 65617 : if (s.empty())
32 13 : return CFixed_15_16::Zero();
33 :
34 65604 : bool neg = false;
35 65604 : CFixed_15_16 r;
36 65604 : const char* c = &s[0];
37 :
38 65604 : if (*c == '+')
39 : {
40 2 : ++c;
41 : }
42 65602 : else if (*c == '-')
43 : {
44 5 : ++c;
45 5 : neg = true;
46 : }
47 :
48 : while (true)
49 : {
50 : // Integer part:
51 131237 : if (*c >= '0' && *c <= '9')
52 : {
53 65633 : r = r * 10; // TODO: handle overflow gracefully, maybe
54 65633 : r += CFixed_15_16::FromInt(*c - '0');
55 65633 : ++c;
56 : }
57 65604 : else if (*c == '.')
58 : {
59 65575 : ++c;
60 65575 : u32 frac = 0;
61 65575 : u32 div = 1;
62 : // Fractional part
63 587803 : while (*c >= '0' && *c <= '9')
64 : {
65 316658 : frac *= 10;
66 316658 : frac += (*c - '0');
67 316658 : div *= 10;
68 316658 : ++c;
69 316658 : if (div >= 100000)
70 : {
71 : // any further digits will be too small to have any major effect
72 55544 : break;
73 : }
74 : }
75 : // too many digits or invalid character or end of string - add the fractional part and stop
76 65575 : r += CFixed_15_16(((u64)frac << 16) / div);
77 65575 : break;
78 : }
79 : else
80 : {
81 : // invalid character or end of string
82 29 : break;
83 : }
84 65633 : }
85 :
86 65604 : return (neg ? -r : r);
87 : }
88 :
89 : template<>
90 0 : CFixed_15_16 CFixed_15_16::FromString(const CStrW& s)
91 : {
92 0 : return FromString(s.ToUTF8());
93 : }
94 :
95 : template<>
96 65617 : CStr8 CFixed_15_16::ToString() const
97 : {
98 131234 : std::stringstream r;
99 :
100 65617 : u32 posvalue = abs(value);
101 65617 : if (value < 0)
102 6 : r << "-";
103 :
104 65617 : r << (posvalue >> fract_bits);
105 :
106 65617 : u16 fraction = posvalue & ((1 << fract_bits) - 1);
107 65617 : if (fraction)
108 : {
109 65552 : r << ".";
110 :
111 65552 : u32 frac = 0;
112 65552 : u32 div = 1;
113 :
114 : // Do the inverse of FromString: Keep adding digits until (frac<<16)/div == expected fraction
115 : while (true)
116 : {
117 316628 : frac *= 10;
118 316628 : div *= 10;
119 :
120 : // Low estimate of d such that ((frac+d)<<16)/div == fraction
121 316628 : u32 digit = (((u64)fraction*div) >> 16) - frac;
122 316628 : frac += digit;
123 :
124 : // If this gives the exact target, then add the digit and stop
125 316628 : if (((u64)frac << 16) / div == fraction)
126 : {
127 38 : r << digit;
128 38 : break;
129 : }
130 :
131 : // If the next higher digit gives the exact target, then add that digit and stop
132 316590 : if (digit <= 8 && (((u64)frac+1) << 16) / div == fraction)
133 : {
134 65514 : r << digit+1;
135 65514 : break;
136 : }
137 :
138 : // Otherwise add the digit and continue
139 251076 : r << digit;
140 251076 : }
141 : }
142 :
143 131234 : return r.str();
144 : }
145 :
146 : // Based on http://www.dspguru.com/dsp/tricks/fixed-point-atan2-with-self-normalization
147 224 : CFixed_15_16 atan2_approx(CFixed_15_16 y, CFixed_15_16 x)
148 : {
149 224 : CFixed_15_16 zero;
150 :
151 : // Special case to avoid division-by-zero
152 224 : if (x.IsZero() && y.IsZero())
153 1 : return zero;
154 :
155 223 : CFixed_15_16 c1;
156 223 : c1.SetInternalValue(51472); // pi/4 << 16
157 :
158 223 : CFixed_15_16 c2;
159 223 : c2.SetInternalValue(154415); // 3*pi/4 << 16
160 :
161 223 : CFixed_15_16 abs_y = y.Absolute();
162 :
163 223 : CFixed_15_16 angle;
164 223 : if (x >= zero)
165 : {
166 220 : CFixed_15_16 r = (x - abs_y) / (x + abs_y);
167 220 : angle = c1 - c1.Multiply(r);
168 : }
169 : else
170 : {
171 3 : CFixed_15_16 r = (x + abs_y) / (abs_y - x);
172 3 : angle = c2 - c1.Multiply(r);
173 : }
174 :
175 223 : if (y < zero)
176 3 : return -angle;
177 : else
178 220 : return angle;
179 : }
180 :
181 : template<>
182 75562 : CFixed_15_16 CFixed_15_16::Pi()
183 : {
184 75562 : return CFixed_15_16(205887); // = pi << 16
185 : }
186 :
187 12593 : void sincos_approx(CFixed_15_16 a, CFixed_15_16& sin_out, CFixed_15_16& cos_out)
188 : {
189 : // Based on http://www.coranac.com/2009/07/sines/
190 :
191 : // TODO: this could be made a bit more precise by being careful about scaling
192 :
193 : typedef CFixed_15_16 fixed;
194 :
195 12593 : fixed c2_pi;
196 12593 : c2_pi.SetInternalValue(41721); // = 2/pi << 16
197 :
198 : // Map radians onto the range [0, 4)
199 12593 : fixed z = a.Multiply(c2_pi) % fixed::FromInt(4);
200 :
201 : // Map z onto the range [-1, +1] for sin, and the same with z+1 to compute cos
202 12593 : fixed sz, cz;
203 12593 : if (z >= fixed::FromInt(3)) // [3, 4)
204 : {
205 3139 : sz = z - fixed::FromInt(4);
206 3139 : cz = z - fixed::FromInt(3);
207 : }
208 9454 : else if (z >= fixed::FromInt(2)) // [2, 3)
209 : {
210 3142 : sz = fixed::FromInt(2) - z;
211 3142 : cz = z - fixed::FromInt(3);
212 : }
213 6312 : else if (z >= fixed::FromInt(1)) // [1, 2)
214 : {
215 3145 : sz = fixed::FromInt(2) - z;
216 3145 : cz = fixed::FromInt(1) - z;
217 : }
218 : else // [0, 1)
219 : {
220 3167 : sz = z;
221 3167 : cz = fixed::FromInt(1) - z;
222 : }
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 12593 : fixed sz2 = sz.Multiply(sz);
232 12593 : sin_out = sz.Multiply(fixed::Pi() - sz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - sz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2;
233 :
234 12593 : fixed cz2 = cz.Multiply(cz);
235 12593 : cos_out = cz.Multiply(fixed::Pi() - cz2.Multiply(fixed::Pi()*2 - fixed::FromInt(5) - cz2.Multiply(fixed::Pi() - fixed::FromInt(3)))) / 2;
236 12596 : }
|
