#include <vectrex.h>
#include "controller.h"
#define int8_t int
#define int16_t long
typedef long fp14; // 16 bits on Vectrex (fp2.14 not fp18.14)
//----------------------------------------------
//
// 2.14 fixed point sine and cosine tables.
//
// These tables assume that there are 256 angles
// in a circle.
//
#define int2fp(x) ((x) << 14)
#define fp2int(x) ((x) >> 14)
const fp14 qsine[65] __attribute__ ((section(".text"))) = {
0, 402, 803, 1205, 1605, 2005, 2404, 2801,
3196, 3589, 3980, 4369, 4756, 5139, 5519, 5896,
6269, 6639, 7005, 7366, 7723, 8075, 8423, 8765,
9102, 9434, 9759, 10079, 10393, 10701, 11002, 11297,
11585, 11866, 12139, 12406, 12665, 12916, 13159, 13395,
13622, 13842, 14053, 14255, 14449, 14634, 14810, 14978,
15136, 15286, 15426, 15557, 15678, 15790, 15892, 15985,
16069, 16142, 16206, 16260, 16305, 16339, 16364, 16379,
16384,
};
static inline fp14 hsine(unsigned int8_t x) {
return x>=64 ? qsine[128-x] : qsine[x];
}
static inline fp14 sine(unsigned int8_t x) {
return x&0x80 ? -hsine(x&0x7f) : hsine(x);
}
static inline fp14 fpSin(unsigned int8_t x) {
return sine((unsigned int8_t)(x));
}
static inline fp14 fpCos(unsigned int8_t x) {
return sine((unsigned int8_t)((x)+64));
}
static inline int8_t sin127(int8_t x) {
return (int8_t)(sine((unsigned int8_t)x&255U)>>7);
}
static inline int8_t cos127(int8_t x) {
return (int8_t)(sine(((unsigned int8_t)x+64U)&255U)>>7);
}
static inline fp14 mult_sin(fp14 x, unsigned int8_t angle) {
long long product;
product = x * sine(angle);
product >>= 14L;
return (fp14)product;
}
static inline fp14 mult_cos(fp14 x, unsigned int8_t angle) {
long long product;
angle += 64;
product = x * sine(angle);
product >>= 14L;
return (fp14)product;
}
// fast integer approximation to atan2(x,y) for 6809 - no divides used.
// x,y in range -128:127
static inline unsigned int8_t Map (int quadrant, int angle) { // angle is 0..64
if (quadrant == 0) return (unsigned int8_t)angle;
if (quadrant == 1) return (unsigned int8_t)(128L - (long)angle);
if (quadrant == 2) return (unsigned int8_t)(256L - angle);
/*if (quadrant == 3)*/ return (unsigned int8_t)(128L + angle);
}
unsigned int8_t iatan2_256 (int8_t x, int8_t y) {
// Much faster binary search (compared to original linear skip-chain version)...
// returns angle in 256 units per circle
int quadrant = 0;
if (x > 0) x = -x; else quadrant |= 2; // make both negative avoids issue with negating -128
if (y > 0) y = -y; else quadrant |= 1;
if (y) {
int16_t x128 = x * 128L;
if (x128 <= y * 131L) {
int16_t x64 = x128>>1;
if (x64 <= y * 160L) {
int16_t x32 = x64>>1;
if (x32 <= y * 172L) {
int16_t x16 = x32>>1;
if (x16 <= y * 186L) {
int16_t x8 = x16>>1;
if (x8 <= y * 217L) {
int16_t x2 = x8>>2;
if (x2 <= y * 163L)
return Map (quadrant, 64); // tan(64.5)*2 Is this ever reached?
else
return Map (quadrant, 63); // tan(63.5)*2
} else {
if (x8 <= y * 130L)
return Map (quadrant, 62); // tan(62.5)*8
else
return Map (quadrant, 61); // tan(61.5)*8
}
} else {
if (x32 <= y * 236L) {
if (x16 <= y * 144L)
return Map (quadrant, 60); // tan(60.5)*16
else
return Map (quadrant, 59); // tan(59.5)*16
} else {
if (x32 <= y * 199L)
return Map (quadrant, 58); // tan(58.5)*32
else
return Map (quadrant, 57); // tan(57.5)*32
}
}
} else {
if (x64 <= y * 221L) {
if (x32 <= y * 135L) {
if (x32 <= y * 151L)
return Map (quadrant, 56); // tan(56.5)*32
else
return Map (quadrant, 55); // tan(55.5)*32
} else {
if (x64 <= y * 243L)
return Map (quadrant, 54); // tan(54.5)*64
else
return Map (quadrant, 53); // tan(53.5)*64
}
} else {
if (x64 <= y * 186L) {
if (x64 <= y * 202L)
return Map (quadrant, 52); // tan(52.5)*64
else
return Map (quadrant, 51); // tan(51.5)*64
} else {
if (x64 <= y * 172L)
return Map (quadrant, 50); // tan(50.5)*64
else
return Map (quadrant, 49); // tan(49.5)*64
}
}
}
} else {
if (x128 <= y * 197L) {
if (x128 <= y * 247L) {
if (x64 <= y * 140L) {
if (x64 <= y * 149L)
return Map (quadrant, 48); // tan(48.5)*64
else
return Map (quadrant, 47); // tan(47.5)*64
} else {
if (x64 <= y * 131L)
return Map (quadrant, 46); // tan(46.5)*64
else
return Map (quadrant, 45); // tan(45.5)*64
}
} else {
if (x128 <= y * 220L) {
if (x128 <= y * 233L)
return Map (quadrant, 44); // tan(44.5)*128
else
return Map (quadrant, 43); // tan(43.5)*128
} else {
if (x128 <= y * 208L)
return Map (quadrant, 42); // tan(42.5)*128
else
return Map (quadrant, 41); // tan(41.5)*128
}
}
} else {
if (x128 <= y * 160L) {
if (x128 <= y * 177L) {
if (x128 <= y * 187L)
return Map (quadrant, 40); // tan(40.5)*128
else
return Map (quadrant, 39); // tan(39.5)*128
} else {
if (x128 <= y * 168L)
return Map (quadrant, 38); // tan(38.5)*128
else
return Map (quadrant, 37); // tan(37.5)*128
}
} else {
if (x128 <= y * 145L) {
if (x128 <= y * 152L)
return Map (quadrant, 36); // tan(36.5)*128
else
return Map (quadrant, 35); // tan(35.5)*128
} else {
if (x128 <= y * 138L)
return Map (quadrant, 34); // tan(34.5)*128
else
return Map (quadrant, 33); // tan(33.5)*128
}
}
}
}
} else {
if (x128 <= y * 55L) {
if (x128 <= y * 88L) {
if (x128 <= y * 108L) {
if (x128 <= y * 119L) {
if (x128 <= y * 125L)
return Map (quadrant, 32); // tan(32.5)*128
else
return Map (quadrant, 31); // tan(31.5)*128
} else {
if (x128 <= y * 113L)
return Map (quadrant, 30); // tan(30.5)*128
else
return Map (quadrant, 29); // tan(29.5)*128
}
} else {
if (x128 <= y * 97L) {
if (x128 <= y * 102L)
return Map (quadrant, 28); // tan(28.5)*128
else
return Map (quadrant, 27); // tan(27.5)*128
} else {
if (x128 <= y * 93L)
return Map (quadrant, 26); // tan(26.5)*128
else
return Map (quadrant, 25); // tan(25.5)*128
}
}
} else {
if (x128 <= y * 70L) {
if (x128 <= y * 79L) {
if (x128 <= y * 83L)
return Map (quadrant, 24); // tan(24.5)*128
else
return Map (quadrant, 23); // tan(23.5)*128
} else {
if (x128 <= y * 75L)
return Map (quadrant, 22); // tan(22.5)*128
else
return Map (quadrant, 21); // tan(21.5)*128
}
} else {
if (x128 <= y * 62L) {
if (x128 <= y * 66L)
return Map (quadrant, 20); // tan(20.5)*128
else
return Map (quadrant, 19); // tan(19.5)*128
} else {
if (x128 <= y * 59L)
return Map (quadrant, 18); // tan(18.5)*128
else
return Map (quadrant, 17); // tan(17.5)*128
}
}
}
} else {
if (x128 <= y * 27L) {
if (x128 <= y * 41L) {
if (x128 <= y * 48L) {
if (x128 <= y * 51L)
return Map (quadrant, 16); // tan(16.5)*128
else
return Map (quadrant, 15); // tan(15.5)*128
} else {
if (x128 <= y * 44L)
return Map (quadrant, 14); // tan(14.5)*128
else
return Map (quadrant, 13); // tan(13.5)*128
}
} else {
if (x128 <= y * 34L) {
if (x128 <= y * 37L)
return Map (quadrant, 12); // tan(12.5)*128
else
return Map (quadrant, 11); // tan(11.5)*128
} else {
if (x128 <= y * 30L)
return Map (quadrant, 10); // tan(10.5)*128
else
return Map (quadrant, 9); // tan(9.5)*128
}
}
} else {
if (x128 <= y * 14L) {
if (x128 <= y * 21L) {
if (x128 <= y * 24L)
return Map (quadrant, 8); // tan(8.5)*128
else
return Map (quadrant, 7); // tan(7.5)*128
} else {
if (x128 <= y * 17L)
return Map (quadrant, 6); // tan(6.5)*128
else
return Map (quadrant, 5); // tan(5.5)*128
}
} else {
if (x128 <= y * 8L) {
if (x128 <= y * 11L)
return Map (quadrant, 4); // tan(4.5)*128
else
return Map (quadrant, 3); // tan(3.5)*128
} else {
if (x128 <= y * 5L) {
return Map (quadrant, 2); // tan(2.5)*128 hi==lo
} else {
if (x128 <= y * 2L) // remember x and y are negative.
return Map (quadrant, 1); // tan(1.5)*128
else
return Map (quadrant, 0); // tan(0.5)*128 - need to check edge conditions.
}
}
}
}
}
}
}
// this logic isn't what was intended because x and y have already been negated
// BUT it does seem to work since I checked it against the atan2 results using doubles.
// Probably means it can be simplified.
if (x < 0) return 192;
if (x) return 64; // does this ever happen? x > 0 should not be possible.
return 0; // x == 0
// originally returned: Map (quadrant, 64);
}
// scales up well to signed 16 bits
unsigned int8_t latan2_256 (int16_t x, int16_t y) { // Much faster binary search... returns angle in 256 units per circle
while (!(((x&0xFF80) == 0 || (x&0xFF80) == 0xFF80) && ((y&0xFF80) == 0 || (y&0xFF80) == 0xFF80))) {
x = x>>1; y = y>>1;
}
return iatan2_256((int8_t)x, (int8_t)y);
}
int main(void) {
int x,y,compass;
enable_controller_1_x();
enable_controller_1_y();
for (;;) {
Joy_Analog();
Wait_Recal();
Intensity_a(0x7f);
Print_Str_d(120, -90, " NEAREST ANGLE \x80");
Print_Str_d(100, -90, "POINT TO JOYSTICK \x80");
x = joystick_1_x(); y = joystick_1_y();
compass = (int)iatan2_256(x,y);
Reset0Ref();
Intensity_a(0x0f);
Moveto_d(0, 0);
Draw_Line_d(y, x);
Reset0Ref();
Intensity_a(0x3f);
Moveto_d(0, 0);
Draw_Line_d(cos127(compass)>>1, sin127(compass)>>1);
// for this demo, no special handling of the dead zone around 0,0
// while testing robustness of x/y check when y=0.
}
return 0;
}