/* Initialise the Trig tables */
bool trigInitialise(void)
{
uint64_t test;
uint32_t crc;
uint32_t i;
// Generate tables.
STATIC_ASSERT(sizeof(trigSinTable)/sizeof(*trigSinTable) == 0x4001);
for (i = 0; i != 0x4001; ++i)
{
trigSinTable[i] = (int)(0x10000*sin(i*(M_PI/0x8000)) + 0.5) - !!i; // -!!i = subtract 1, unless i == 0.
}
STATIC_ASSERT(sizeof(trigAtanTable)/sizeof(*trigAtanTable) == 0x2001);
for (i = 0; i != 0x2001; ++i)
{
trigAtanTable[i] = (int)(0x8000/M_PI*atan((double)i/0x2000) + 0.5);
}
// Check tables are correct.
crc = ~crcSumU16(0, trigSinTable, sizeof(trigSinTable)/sizeof(*trigSinTable));
ASSERT(crc == 0x6D3C8DB5, "Bad trigSinTable CRC = 0x%08X, sin function is broken.", crc);
crc = ~crcSumU16(0, trigAtanTable, sizeof(trigAtanTable)/sizeof(*trigAtanTable));
ASSERT(crc == 0xD2797F85, "Bad trigAtanTable CRC = 0x%08X, atan function is broken.", crc);
// Test large and small square roots.
for (test = 0x0000; test != 0x10000; ++test)
{
uint64_t lower = test*test;
uint64_t upper = (test + 1)*(test + 1) - 1;
ASSERT((uint32_t)iSqrt(lower) == test, "Sanity check failed, sqrt(%"PRIu64") gave %"PRIu32" instead of %"PRIu64"!", lower, i64Sqrt(lower), test);
ASSERT((uint32_t)iSqrt(upper) == test, "Sanity check failed, sqrt(%"PRIu64") gave %"PRIu32" instead of %"PRIu64"!", upper, i64Sqrt(upper), test);
}
for (test = 0xFFFE0000; test != 0x00020000; test = (test + 1)&0xFFFFFFFF)
{
uint64_t lower = test*test;
uint64_t upper = (test + 1)*(test + 1) - 1;
ASSERT((uint32_t)i64Sqrt(lower) == test, "Sanity check failed, sqrt(%"PRIu64") gave %"PRIu32" instead of %"PRIu64"!", lower, i64Sqrt(lower), test);
ASSERT((uint32_t)i64Sqrt(upper) == test, "Sanity check failed, sqrt(%"PRIu64") gave %"PRIu32" instead of %"PRIu64"!", upper, i64Sqrt(upper), test);
}
return true;
}
int32_t iHypot3(int32_t x, int32_t y, int32_t z)
{
return i64Sqrt((uint64_t)x*x + (uint64_t)y*y + (uint64_t)z*z); // Casting from int32_t to uint64_t does sign extend.
}
// Create a list of all tiles within the given radius, and the leftmost and rightmost angles of the tiles, for field of vision.
// Radius in tiles*TILE_UNITS.
// The droid is assumed to be at (dx = 0.5, dy = 0.5)*TILE_UNITS, not at the origin, since tile data is for the top left of the tile.
static std::vector<WavecastTile> generateWavecastTable(unsigned radius)
{
std::vector<WavecastTile> tiles;
std::vector<RationalAngle> unsortedAngles;
for (unsigned diamond = 1; diamond*TILE_UNITS < radius*2; ++diamond) // Factor is a bit more than needed to surround the circle with diamonds.
{
for (unsigned quadrant = 0; quadrant < 4; ++quadrant)
{
for (unsigned s = 0; s < diamond; ++s)
{
WavecastTile tile;
switch (quadrant)
{
case 0: tile.dx = diamond - s; tile.dy = s + 1; break;
case 1: tile.dx = - s; tile.dy = diamond - s; break;
case 2: tile.dx = -diamond + s + 1; tile.dy = - s; break;
case 3: tile.dx = s + 1; tile.dy = -diamond + s + 1; break;
}
const int sdx = tile.dx*2 - 1, sdy = tile.dy*2 - 1; // 2*distance from sensor located at (0.5, 0.5)
const unsigned tileRadiusSq = sdx*sdx + sdy*sdy;
if (tileRadiusSq*(TILE_UNITS*TILE_UNITS/4) > radius*radius)
{
continue;
}
tile.invRadius = i64Sqrt((uint64_t)2*65536*65536 / tileRadiusSq); // Result is at most 65536, inversely proportional to the radius.
const int minCorner[4][2] = {{1, 0}, {1, 1}, {0, 1}, {0, 0}};
const int mcdx = minCorner[quadrant][0], mcdy = minCorner[quadrant][1]; // Corner of the tile which the minimum angle.
RationalAngle minAngle = RationalAngle(tile.dx - 1 + mcdx, tile.dy - 1 + mcdy), maxAngle = RationalAngle(tile.dx - mcdx, tile.dy - mcdy);
if (maxAngle < minAngle)
{
maxAngle = RationalAngle(0, 0); // Special case, like RationalAngle(1, 0), except that it compares greater than all other angles instead of less than all other angles.
}
tile.angBegin = unsortedAngles.size();
unsortedAngles.push_back(minAngle);
tile.angEnd = unsortedAngles.size();
unsortedAngles.push_back(maxAngle);
tiles.push_back(tile);
}
}
}
// Sort the angles and remove duplicates.
std::vector<RationalAngle> sortedAngles = unsortedAngles;
std::sort(sortedAngles.begin(), sortedAngles.end());
sortedAngles.erase(std::unique(sortedAngles.begin(), sortedAngles.end()), sortedAngles.end());
// Subtitute the angle values angBegin and angEnd with ones that can be compared to each other, so that
// the angles can be compared without using the unsortedAngles lookup table. (And without using the
// sortedAngles lookup table either.)
for (std::vector<WavecastTile>::iterator i = tiles.begin(); i != tiles.end(); ++i)
{
i->angBegin = std::lower_bound(sortedAngles.begin(), sortedAngles.end(), unsortedAngles[i->angBegin]) - sortedAngles.begin();
i->angEnd = std::lower_bound(sortedAngles.begin(), sortedAngles.end(), unsortedAngles[i->angEnd ]) - sortedAngles.begin();
}
#if 0 // Prints wavecast table.
if (radius == 8*TILE_UNITS)
{
printf("Table for radius %f:\n", radius/(float)TILE_UNITS);
for (std::vector<WavecastTile>::iterator i = tiles.begin(); i != tiles.end(); ++i)
printf("Tile%5d: (%3d,%3d): angle %3d-%3d, invRadius %5d\n", (int)(i-/*>*/tiles.begin()), i->dx, i->dy, i->angBegin, i->angEnd, i->invRadius);
printf("Unsorted angles for radius %f:\n", radius/(float)TILE_UNITS);
for (std::vector<RationalAngle>::iterator i = unsortedAngles.begin(); i != unsortedAngles.end(); ++i)
printf("Unsorted angle%5d: (%3d,%3d) = %11.6lf°\n", (int)(i-/*>*/unsortedAngles.begin()), ((int *)&*i)[0], ((int *)&*i)[1], atan2(((int *)&*i)[1], ((int *)&*i)[0])*180/M_PI); // ((int *)&*i)[0] = very hacky bypass of "private:".
printf("Sorted angles for radius %f:\n", radius/(float)TILE_UNITS);
for (std::vector<RationalAngle>::iterator i = sortedAngles.begin(); i != sortedAngles.end(); ++i)
printf("Sorted angle%5d: (%3d,%3d) = %11.6lf°\n", (int)(i-/*>*/sortedAngles.begin()), ((int *)&*i)[0], ((int *)&*i)[1], atan2(((int *)&*i)[1], ((int *)&*i)[0])*180/M_PI); // ((int *)&*i)[0] = very hacky bypass of "private:".
}
printf("Radius %11.6f summary: %5d tiles, %5d angles (%5d with duplicates)\n", radius/(float)TILE_UNITS, (int)tiles.size(), (int)sortedAngles.size(), (int)unsortedAngles.size());
#endif
return tiles;
}
