1 /*
2 This file is part of Warzone 2100.
3 Copyright (C) 1999-2004 Eidos Interactive
4 Copyright (C) 2005-2020 Warzone 2100 Project
5
6 Warzone 2100 is free software; you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation; either version 2 of the License, or
9 (at your option) any later version.
10
11 Warzone 2100 is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along with Warzone 2100; if not, write to the Free Software
18 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
19 */
20 /*
21 * Trig.c
22 *
23 * Trig lookup tables
24 *
25 */
26
27 #if defined( _MSC_VER )
28 #pragma warning( disable : 4244 ) // warning C4244: conversion from 'type1' to 'type2', possible loss of data // FIXME?
29 #endif
30
31 /* Allow frame header files to be singly included */
32 #define FRAME_LIB_INCLUDE
33
34 #include "types.h"
35 #include "trig.h"
36 #include "crc.h"
37
38 #include <assert.h>
39 #include <stdlib.h>
40 // for M_PI
41 #if defined(WZ_CC_MSVC)
42 #define _USE_MATH_DEFINES
43 #endif
44
45 #include <math.h>
46 #include <algorithm>
47
48 static uint16_t trigSinTable[0x4001];
49 static uint16_t trigAtanTable[0x2001];
50
51 /* Initialise the Trig tables */
trigInitialise(void)52 bool trigInitialise(void)
53 {
54 uint32_t crc;
55 uint32_t i;
56
57 // Generate tables.
58 STATIC_ASSERT(sizeof(trigSinTable) / sizeof(*trigSinTable) == 0x4001);
59 for (i = 0; i != 0x4001; ++i)
60 {
61 trigSinTable[i] = (int)(0x10000 * sin(i * (M_PI / 0x8000)) + 0.5) - !!i; // -!!i = subtract 1, unless i == 0.
62 }
63 STATIC_ASSERT(sizeof(trigAtanTable) / sizeof(*trigAtanTable) == 0x2001);
64 for (i = 0; i != 0x2001; ++i)
65 {
66 trigAtanTable[i] = (int)(0x8000 / M_PI * atan((double)i / 0x2000) + 0.5);
67 }
68
69 // Check tables are correct.
70 crc = ~crcSumU16(0, trigSinTable, sizeof(trigSinTable) / sizeof(*trigSinTable));
71 ASSERT(crc == 0x6D3C8DB5, "Bad trigSinTable CRC = 0x%08X, sin function is broken.", crc);
72 crc = ~crcSumU16(0, trigAtanTable, sizeof(trigAtanTable) / sizeof(*trigAtanTable));
73 ASSERT(crc == 0xD2797F85, "Bad trigAtanTable CRC = 0x%08X, atan function is broken.", crc);
74
75 // Test large and small square roots.
76 for (uint32_t test = 0x0000; test != 0x10000; ++test)
77 {
78 uint32_t lower = test * test;
79 uint32_t upper = (test + 1) * (test + 1) - 1;
80 ASSERT((uint32_t)iSqrt(lower) == test, "Sanity check failed, iSqrt(%" PRIu32") gave %" PRIu32" instead of %" PRIu32"!", lower, (uint32_t)iSqrt(lower), test);
81 ASSERT((uint32_t)iSqrt(upper) == test, "Sanity check failed, iSqrt(%" PRIu32") gave %" PRIu32" instead of %" PRIu32"!", upper, (uint32_t)iSqrt(upper), test);
82 }
83 for (uint64_t test = 0xFFFE0000; test != 0x00020000; test = (test + 1) & 0xFFFFFFFF)
84 {
85 uint64_t lower = test * test;
86 uint64_t upper = (test + 1) * (test + 1) - 1;
87 ASSERT((uint32_t)i64Sqrt(lower) == test, "Sanity check failed, i64Sqrt(%" PRIu64") gave %" PRIu32" instead of %" PRIu64"!", lower, (uint32_t)i64Sqrt(lower), test);
88 ASSERT((uint32_t)i64Sqrt(upper) == test, "Sanity check failed, i64Sqrt(%" PRIu64") gave %" PRIu32" instead of %" PRIu64"!", upper, (uint32_t)i64Sqrt(upper), test);
89 }
90
91 return true;
92 }
93
iSin(uint16_t a)94 int32_t iSin(uint16_t a)
95 {
96 int sign[4] = {1, 1, -1, -1};
97 bool reverse[4] = {false, true, false, true};
98 int q = a >> 14;
99 uint16_t r = a & 0x3FFF;
100 uint16_t rvr = reverse[q] ? 0x4000 - r : r;
101 return sign[q] * (trigSinTable[rvr] + !!rvr); // +!!rvr = add 1, unless rvr == 0.
102 }
103
iCos(uint16_t a)104 int32_t iCos(uint16_t a)
105 {
106 int sign[4] = {1, -1, -1, 1};
107 bool reverse[4] = {true, false, true, false};
108 int q = a >> 14;
109 uint16_t r = a & 0x3FFF;
110 uint16_t rvr = reverse[q] ? 0x4000 - r : r;
111 return sign[q] * (trigSinTable[rvr] + !!rvr); // +!!rvr = add 1, unless rvr == 0.
112 }
113
iSinR(uint16_t a,int32_t r)114 int32_t iSinR(uint16_t a, int32_t r)
115 {
116 return ((int64_t)r * iSin(a)) / 65536;
117 }
118
iCosR(uint16_t a,int32_t r)119 int32_t iCosR(uint16_t a, int32_t r)
120 {
121 return ((int64_t)r * iCos(a)) / 65536;
122 }
123
iSinSR(int32_t a,int32_t s,int32_t r)124 int32_t iSinSR(int32_t a, int32_t s, int32_t r)
125 {
126 return ((int64_t)r * iSin(((int64_t)a << 16) / s)) / 65536;
127 }
128
iCosSR(int32_t a,int32_t s,int32_t r)129 int32_t iCosSR(int32_t a, int32_t s, int32_t r)
130 {
131 return ((int64_t)r * iCos(((int64_t)a << 16) / s)) / 65536;
132 }
133
iAtan2(int32_t s,int32_t c)134 uint16_t iAtan2(int32_t s, int32_t c)
135 {
136 uint16_t d = 0; // Dummy initialisation.
137 uint32_t j = 0, k = 0; // Dummy initialisations.
138
139 if (s == 0 && c == 0)
140 {
141 return 0; // All return values equally valid, don't divide by 0.
142 }
143
144 switch (((s < 0) << 1) + (c < 0))
145 {
146 case 0: j = s; k = c; d = 0x0000; break;
147 case 1: j = -c; k = s; d = 0x4000; break;
148 case 2: j = c; k = -s; d = 0xC000; break;
149 case 3: j = -s; k = -c; d = 0x8000; break;
150 }
151 return j < k ? d + trigAtanTable[((int64_t)j * 0x2000 + k / 2) / k]
152 : d + 0x4000 - trigAtanTable[((int64_t)k * 0x2000 + j / 2) / j];
153 }
154
iSqrt(uint32_t n)155 int32_t iSqrt(uint32_t n)
156 {
157 uint32_t r = sqrt((double)n); // Calculate square root, rounded down. Excess precision does not change the result.
158
159 // Check that we got the right result.
160 ASSERT((int32_t)(r * r - n) <= 0 && (int32_t)((r + 1) * (r + 1) - n) > 0, "Too badly broken sqrt function, iSqrt(%u) = %u.", (unsigned)n, (unsigned)r);
161
162 return r;
163 }
164
i64Sqrt(uint64_t n)165 int32_t i64Sqrt(uint64_t n)
166 {
167 uint64_t r;
168 if (sizeof(void *) > 4)
169 {
170 r = (uint64_t) sqrt((double)n); // Calculate square root, usually rounded down. Excess precision may result in rounding down instead of up, which is fine.
171 }
172 else
173 {
174 // Bad compiler workaround. On some compilers, sqrt() seems to have somehow been taking 64-bit doubles and returning 80-bit doubles, breaking expected rounding behaviour.
175 r = (uint64_t) sqrtl(n); // Calculate square root, usually rounded down. Excess precision may result in rounding down instead of up, which is fine.
176 }
177
178 // Correct for different rounding & precision
179 // See: https://codereview.stackexchange.com/questions/69069/computing-the-square-root-of-a-64-bit-integer
180 r = std::min(r, (uint64_t)UINT32_MAX);
181 while (r * r > n)
182 r--;
183 while (n - r * r > r * 2)
184 r++;
185
186 // Check that we got the right result.
187 ASSERT((int64_t)(r * r - n) <= 0 && (int64_t)((r + 1) * (r + 1) - n) > 0, "Too badly broken sqrt function, i64Sqrt(%" PRIu64") = %" PRIu64".", n, r);
188
189 return r;
190 }
191
iHypot(int32_t x,int32_t y)192 int32_t iHypot(int32_t x, int32_t y)
193 {
194 return i64Sqrt((uint64_t)x * x + (uint64_t)y * y); // Casting from int32_t to uint64_t does sign extend.
195 }
196
iHypot3(int32_t x,int32_t y,int32_t z)197 int32_t iHypot3(int32_t x, int32_t y, int32_t z)
198 {
199 return i64Sqrt((uint64_t)x * x + (uint64_t)y * y + (uint64_t)z * z); // Casting from int32_t to uint64_t does sign extend.
200 }
201
202 // For testing above functions.
203 #if 0
204 int main()
205 {
206 srand(time(NULL));
207 for (unsigned i = 0; i != 1000000; ++i)
208 {
209 int32_t s = rand() % 2 ? rand() + rand() + rand() + rand() : rand() % 101 - 50;
210 int32_t c = rand() % 2 ? rand() + rand() + rand() + rand() : rand() % 101 - 50;
211 int a1 = uint16_t(round(32768 / M_PI * atan2(s, c))), a2 = iAtan2(s, c);
212 if (a1 + 1 < a2 || a1 > a2 + 1)
213 {
214 std::printf("32768/π atan2(%d, %d) = %d, iAtan2() = %d\n", s, c, a1, a2);
215 }
216 }
217 for (unsigned a = 0; a < 65536; ++a)
218 {
219 int s1 = round(65536 * sin(a * M_PI / 32768)), s2 = iSin(a);
220 int c1 = round(65536 * cos(a * M_PI / 32768)), c2 = iCos(a);
221 if (s1 != s2 || c1 != c2)
222 {
223 std::printf("a = %d, sin = %d, %d, cos = %d, %d\n", a, s1, s2, c1, c2);
224 }
225 }
226 }
227 #endif
228