1 | /*****************************************************************************\ |
2 | Snes9x - Portable Super Nintendo Entertainment System (TM) emulator. |
3 | This file is licensed under the Snes9x License. |
4 | For further information, consult the LICENSE file in the root directory. |
5 | \*****************************************************************************/ |
6 | |
7 | #include <math.h> |
8 | #include "snes9x.h" |
9 | #include "memmap.h" |
10 | #include "seta.h" |
11 | |
12 | static const int16 ST010_SinTable[256] = |
13 | { |
14 | 0x0000, 0x0324, 0x0648, 0x096a, 0x0c8c, 0x0fab, 0x12c8, 0x15e2, |
15 | 0x18f9, 0x1c0b, 0x1f1a, 0x2223, 0x2528, 0x2826, 0x2b1f, 0x2e11, |
16 | 0x30fb, 0x33df, 0x36ba, 0x398c, 0x3c56, 0x3f17, 0x41ce, 0x447a, |
17 | 0x471c, 0x49b4, 0x4c3f, 0x4ebf, 0x5133, 0x539b, 0x55f5, 0x5842, |
18 | 0x5a82, 0x5cb3, 0x5ed7, 0x60eb, 0x62f1, 0x64e8, 0x66cf, 0x68a6, |
19 | 0x6a6d, 0x6c23, 0x6dc9, 0x6f5e, 0x70e2, 0x7254, 0x73b5, 0x7504, |
20 | 0x7641, 0x776b, 0x7884, 0x7989, 0x7a7c, 0x7b5c, 0x7c29, 0x7ce3, |
21 | 0x7d89, 0x7e1d, 0x7e9c, 0x7f09, 0x7f61, 0x7fa6, 0x7fd8, 0x7ff5, |
22 | 0x7fff, 0x7ff5, 0x7fd8, 0x7fa6, 0x7f61, 0x7f09, 0x7e9c, 0x7e1d, |
23 | 0x7d89, 0x7ce3, 0x7c29, 0x7b5c, 0x7a7c, 0x7989, 0x7884, 0x776b, |
24 | 0x7641, 0x7504, 0x73b5, 0x7254, 0x70e2, 0x6f5e, 0x6dc9, 0x6c23, |
25 | 0x6a6d, 0x68a6, 0x66cf, 0x64e8, 0x62f1, 0x60eb, 0x5ed7, 0x5cb3, |
26 | 0x5a82, 0x5842, 0x55f5, 0x539b, 0x5133, 0x4ebf, 0x4c3f, 0x49b4, |
27 | 0x471c, 0x447a, 0x41ce, 0x3f17, 0x3c56, 0x398c, 0x36ba, 0x33df, |
28 | 0x30fb, 0x2e11, 0x2b1f, 0x2826, 0x2528, 0x2223, 0x1f1a, 0x1c0b, |
29 | 0x18f8, 0x15e2, 0x12c8, 0x0fab, 0x0c8c, 0x096a, 0x0648, 0x0324, |
30 | 0x0000, -0x0324, -0x0648, -0x096b, -0x0c8c, -0x0fab, -0x12c8, -0x15e2, |
31 | -0x18f9, -0x1c0b, -0x1f1a, -0x2223, -0x2528, -0x2826, -0x2b1f, -0x2e11, |
32 | -0x30fb, -0x33df, -0x36ba, -0x398d, -0x3c56, -0x3f17, -0x41ce, -0x447a, |
33 | -0x471c, -0x49b4, -0x4c3f, -0x4ebf, -0x5133, -0x539b, -0x55f5, -0x5842, |
34 | -0x5a82, -0x5cb3, -0x5ed7, -0x60ec, -0x62f1, -0x64e8, -0x66cf, -0x68a6, |
35 | -0x6a6d, -0x6c23, -0x6dc9, -0x6f5e, -0x70e2, -0x7254, -0x73b5, -0x7504, |
36 | -0x7641, -0x776b, -0x7884, -0x7989, -0x7a7c, -0x7b5c, -0x7c29, -0x7ce3, |
37 | -0x7d89, -0x7e1d, -0x7e9c, -0x7f09, -0x7f61, -0x7fa6, -0x7fd8, -0x7ff5, |
38 | -0x7fff, -0x7ff5, -0x7fd8, -0x7fa6, -0x7f61, -0x7f09, -0x7e9c, -0x7e1d, |
39 | -0x7d89, -0x7ce3, -0x7c29, -0x7b5c, -0x7a7c, -0x7989, -0x7883, -0x776b, |
40 | -0x7641, -0x7504, -0x73b5, -0x7254, -0x70e2, -0x6f5e, -0x6dc9, -0x6c23, |
41 | -0x6a6d, -0x68a6, -0x66cf, -0x64e8, -0x62f1, -0x60eb, -0x5ed7, -0x5cb3, |
42 | -0x5a82, -0x5842, -0x55f5, -0x539a, -0x5133, -0x4ebf, -0x4c3f, -0x49b3, |
43 | -0x471c, -0x447a, -0x41cd, -0x3f17, -0x3c56, -0x398c, -0x36b9, -0x33de, |
44 | -0x30fb, -0x2e10, -0x2b1f, -0x2826, -0x2527, -0x2223, -0x1f19, -0x1c0b, |
45 | -0x18f8, -0x15e2, -0x12c8, -0x0fab, -0x0c8b, -0x096a, -0x0647, -0x0324 |
46 | }; |
47 | |
48 | static const uint8 ST010_ArcTan[32][32] = |
49 | { |
50 | { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
51 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, |
52 | { 0x40, 0x20, 0x13, 0x0D, 0x0A, 0x08, 0x07, 0x06, 0x05, 0x05, 0x04, 0x04, 0x03, 0x03, 0x03, 0x03, |
53 | 0x03, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x01, 0x01, 0x01, 0x01 }, |
54 | { 0x40, 0x2D, 0x20, 0x18, 0x13, 0x10, 0x0D, 0x0B, 0x0A, 0x09, 0x08, 0x07, 0x07, 0x06, 0x06, 0x05, |
55 | 0x05, 0x05, 0x05, 0x04, 0x04, 0x04, 0x04, 0x04, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }, |
56 | { 0x40, 0x33, 0x28, 0x20, 0x1A, 0x16, 0x13, 0x10, 0x0F, 0x0D, 0x0C, 0x0B, 0x0A, 0x09, 0x09, 0x08, |
57 | 0x08, 0x07, 0x07, 0x06, 0x06, 0x06, 0x06, 0x05, 0x05, 0x05, 0x05, 0x05, 0x04, 0x04, 0x04, 0x04 }, |
58 | { 0x40, 0x36, 0x2D, 0x26, 0x20, 0x1B, 0x18, 0x15, 0x13, 0x11, 0x10, 0x0E, 0x0D, 0x0C, 0x0B, 0x0B, |
59 | 0x0A, 0x09, 0x09, 0x08, 0x08, 0x08, 0x07, 0x07, 0x07, 0x06, 0x06, 0x06, 0x06, 0x06, 0x05, 0x05 }, |
60 | { 0x40, 0x38, 0x30, 0x2A, 0x25, 0x20, 0x1C, 0x19, 0x17, 0x15, 0x13, 0x11, 0x10, 0x0F, 0x0E, 0x0D, |
61 | 0x0C, 0x0C, 0x0B, 0x0A, 0x0A, 0x0A, 0x09, 0x09, 0x08, 0x08, 0x08, 0x07, 0x07, 0x07, 0x07, 0x07 }, |
62 | { 0x40, 0x39, 0x33, 0x2D, 0x28, 0x24, 0x20, 0x1D, 0x1A, 0x18, 0x16, 0x14, 0x13, 0x12, 0x10, 0x10, |
63 | 0x0F, 0x0E, 0x0D, 0x0C, 0x0C, 0x0B, 0x0B, 0x0A, 0x0A, 0x0A, 0x09, 0x09, 0x09, 0x08, 0x08, 0x08 }, |
64 | { 0x40, 0x3A, 0x35, 0x30, 0x2B, 0x27, 0x23, 0x20, 0x1D, 0x1B, 0x19, 0x17, 0x16, 0x14, 0x13, 0x12, |
65 | 0x11, 0x10, 0x0F, 0x0E, 0x0E, 0x0D, 0x0D, 0x0C, 0x0C, 0x0B, 0x0B, 0x0A, 0x0A, 0x0A, 0x09, 0x09 }, |
66 | { 0x40, 0x3B, 0x36, 0x31, 0x2D, 0x29, 0x26, 0x23, 0x20, 0x1E, 0x1B, 0x1A, 0x18, 0x16, 0x15, 0x14, |
67 | 0x13, 0x12, 0x11, 0x10, 0x10, 0x0F, 0x0E, 0x0E, 0x0D, 0x0D, 0x0C, 0x0C, 0x0B, 0x0B, 0x0B, 0x0A }, |
68 | { 0x40, 0x3B, 0x37, 0x33, 0x2F, 0x2B, 0x28, 0x25, 0x22, 0x20, 0x1E, 0x1C, 0x1A, 0x19, 0x17, 0x16, |
69 | 0x15, 0x14, 0x13, 0x12, 0x11, 0x10, 0x10, 0x0F, 0x0F, 0x0E, 0x0E, 0x0D, 0x0D, 0x0C, 0x0C, 0x0C }, |
70 | { 0x40, 0x3C, 0x38, 0x34, 0x30, 0x2D, 0x2A, 0x27, 0x25, 0x20, 0x20, 0x1E, 0x1C, 0x1B, 0x19, 0x18, |
71 | 0x17, 0x16, 0x15, 0x14, 0x13, 0x12, 0x11, 0x11, 0x10, 0x10, 0x0F, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D }, |
72 | { 0x40, 0x3C, 0x39, 0x35, 0x32, 0x2F, 0x2C, 0x29, 0x26, 0x24, 0x22, 0x20, 0x1E, 0x1D, 0x1B, 0x1A, |
73 | 0x19, 0x17, 0x16, 0x15, 0x14, 0x14, 0x13, 0x12, 0x12, 0x11, 0x10, 0x10, 0x0F, 0x0F, 0x0E, 0x0E }, |
74 | { 0x40, 0x3D, 0x39, 0x36, 0x33, 0x30, 0x2D, 0x2A, 0x28, 0x26, 0x24, 0x22, 0x20, 0x1E, 0x1D, 0x1B, |
75 | 0x1A, 0x19, 0x18, 0x17, 0x16, 0x15, 0x14, 0x14, 0x13, 0x12, 0x12, 0x11, 0x10, 0x10, 0x10, 0x0F }, |
76 | { 0x40, 0x3D, 0x3A, 0x37, 0x34, 0x31, 0x2E, 0x2C, 0x2A, 0x27, 0x25, 0x23, 0x22, 0x20, 0x1E, 0x1D, |
77 | 0x1C, 0x1B, 0x19, 0x18, 0x17, 0x17, 0x16, 0x15, 0x14, 0x14, 0x13, 0x12, 0x12, 0x11, 0x11, 0x10 }, |
78 | { 0x40, 0x3D, 0x3A, 0x37, 0x35, 0x32, 0x30, 0x2D, 0x2B, 0x29, 0x27, 0x25, 0x23, 0x22, 0x20, 0x1F, |
79 | 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x17, 0x16, 0x16, 0x15, 0x14, 0x13, 0x13, 0x12, 0x12, 0x11 }, |
80 | { 0x40, 0x3D, 0x3B, 0x38, 0x35, 0x33, 0x30, 0x2E, 0x2C, 0x2A, 0x28, 0x26, 0x25, 0x23, 0x21, 0x20, |
81 | 0x1F, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x18, 0x17, 0x16, 0x15, 0x15, 0x14, 0x13, 0x13, 0x12 }, |
82 | { 0x40, 0x3D, 0x3B, 0x38, 0x36, 0x34, 0x31, 0x2F, 0x2D, 0x2B, 0x29, 0x27, 0x26, 0x24, 0x23, 0x21, |
83 | 0x20, 0x1F, 0x1E, 0x1D, 0x1B, 0x1B, 0x1A, 0x19, 0x18, 0x17, 0x16, 0x16, 0x15, 0x15, 0x14, 0x13 }, |
84 | { 0x40, 0x3E, 0x3B, 0x39, 0x37, 0x34, 0x32, 0x30, 0x2E, 0x2C, 0x2A, 0x29, 0x27, 0x25, 0x24, 0x23, |
85 | 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x18, 0x18, 0x17, 0x16, 0x16, 0x15, 0x14 }, |
86 | { 0x40, 0x3E, 0x3B, 0x39, 0x37, 0x35, 0x33, 0x31, 0x2F, 0x2D, 0x2B, 0x2A, 0x28, 0x27, 0x25, 0x24, |
87 | 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x19, 0x19, 0x18, 0x17, 0x17, 0x16, 0x15 }, |
88 | { 0x40, 0x3E, 0x3C, 0x3A, 0x38, 0x36, 0x34, 0x32, 0x30, 0x2E, 0x2C, 0x2B, 0x29, 0x28, 0x26, 0x25, |
89 | 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1A, 0x1A, 0x19, 0x18, 0x18, 0x17, 0x16 }, |
90 | { 0x40, 0x3E, 0x3C, 0x3A, 0x38, 0x36, 0x34, 0x32, 0x30, 0x2F, 0x2D, 0x2C, 0x2A, 0x29, 0x27, 0x26, |
91 | 0x25, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1B, 0x1B, 0x1A, 0x19, 0x19, 0x18, 0x17 }, |
92 | { 0x40, 0x3E, 0x3C, 0x3A, 0x38, 0x36, 0x35, 0x33, 0x31, 0x30, 0x2E, 0x2C, 0x2B, 0x29, 0x28, 0x27, |
93 | 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1C, 0x1C, 0x1B, 0x1A, 0x1A, 0x19, 0x18 }, |
94 | { 0x40, 0x3E, 0x3C, 0x3A, 0x39, 0x37, 0x35, 0x33, 0x32, 0x30, 0x2F, 0x2D, 0x2C, 0x2A, 0x29, 0x28, |
95 | 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1D, 0x1D, 0x1C, 0x1B, 0x1A, 0x1A, 0x19 }, |
96 | { 0x40, 0x3E, 0x3C, 0x3B, 0x39, 0x37, 0x36, 0x34, 0x32, 0x31, 0x2F, 0x2E, 0x2C, 0x2B, 0x2A, 0x28, |
97 | 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C, 0x1B, 0x1B, 0x1A }, |
98 | { 0x40, 0x3E, 0x3D, 0x3B, 0x39, 0x38, 0x36, 0x34, 0x33, 0x31, 0x30, 0x2E, 0x2D, 0x2C, 0x2A, 0x29, |
99 | 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C, 0x1B, 0x1B }, |
100 | { 0x40, 0x3E, 0x3D, 0x3B, 0x3A, 0x38, 0x36, 0x35, 0x33, 0x32, 0x30, 0x2F, 0x2E, 0x2C, 0x2B, 0x2A, |
101 | 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C, 0x1C }, |
102 | { 0x40, 0x3E, 0x3D, 0x3B, 0x3A, 0x38, 0x37, 0x35, 0x34, 0x32, 0x31, 0x30, 0x2E, 0x2D, 0x2C, 0x2B, |
103 | 0x2A, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x22, 0x21, 0x20, 0x1F, 0x1E, 0x1E, 0x1D, 0x1C }, |
104 | { 0x40, 0x3E, 0x3D, 0x3B, 0x3A, 0x39, 0x37, 0x36, 0x34, 0x33, 0x32, 0x30, 0x2F, 0x2E, 0x2D, 0x2B, |
105 | 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x22, 0x21, 0x20, 0x1F, 0x1F, 0x1E, 0x1D }, |
106 | { 0x40, 0x3F, 0x3D, 0x3C, 0x3A, 0x39, 0x37, 0x36, 0x35, 0x33, 0x32, 0x31, 0x30, 0x2E, 0x2D, 0x2C, |
107 | 0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x23, 0x22, 0x22, 0x21, 0x20, 0x1F, 0x1F, 0x1E }, |
108 | { 0x40, 0x3F, 0x3D, 0x3C, 0x3A, 0x39, 0x38, 0x36, 0x35, 0x34, 0x32, 0x31, 0x30, 0x2F, 0x2E, 0x2D, |
109 | 0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x26, 0x25, 0x24, 0x23, 0x22, 0x21, 0x21, 0x20, 0x1F, 0x1F }, |
110 | { 0x40, 0x3F, 0x3D, 0x3C, 0x3B, 0x39, 0x38, 0x37, 0x35, 0x34, 0x33, 0x32, 0x30, 0x2F, 0x2E, 0x2D, |
111 | 0x2C, 0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x25, 0x24, 0x23, 0x22, 0x21, 0x21, 0x20, 0x1F }, |
112 | { 0x40, 0x3F, 0x3D, 0x3C, 0x3B, 0x39, 0x38, 0x37, 0x35, 0x34, 0x33, 0x32, 0x31, 0x30, 0x2F, 0x2E, |
113 | 0x2D, 0x2C, 0x2B, 0x2A, 0x29, 0x28, 0x27, 0x26, 0x25, 0x24, 0x24, 0x23, 0x22, 0x21, 0x21, 0x20 } |
114 | }; |
115 | |
116 | // Mode 7 scaling constants for all raster lines |
117 | static const int16 ST010_M7Scale[176] = |
118 | { |
119 | 0x0380, 0x0325, 0x02da, 0x029c, 0x0268, 0x023b, 0x0215, 0x01f3, |
120 | 0x01d5, 0x01bb, 0x01a3, 0x018e, 0x017b, 0x016a, 0x015a, 0x014b, |
121 | 0x013e, 0x0132, 0x0126, 0x011c, 0x0112, 0x0109, 0x0100, 0x00f8, |
122 | 0x00f0, 0x00e9, 0x00e3, 0x00dc, 0x00d6, 0x00d1, 0x00cb, 0x00c6, |
123 | 0x00c1, 0x00bd, 0x00b8, 0x00b4, 0x00b0, 0x00ac, 0x00a8, 0x00a5, |
124 | 0x00a2, 0x009e, 0x009b, 0x0098, 0x0095, 0x0093, 0x0090, 0x008d, |
125 | 0x008b, 0x0088, 0x0086, 0x0084, 0x0082, 0x0080, 0x007e, 0x007c, |
126 | 0x007a, 0x0078, 0x0076, 0x0074, 0x0073, 0x0071, 0x006f, 0x006e, |
127 | 0x006c, 0x006b, 0x0069, 0x0068, 0x0067, 0x0065, 0x0064, 0x0063, |
128 | 0x0062, 0x0060, 0x005f, 0x005e, 0x005d, 0x005c, 0x005b, 0x005a, |
129 | 0x0059, 0x0058, 0x0057, 0x0056, 0x0055, 0x0054, 0x0053, 0x0052, |
130 | 0x0051, 0x0051, 0x0050, 0x004f, 0x004e, 0x004d, 0x004d, 0x004c, |
131 | 0x004b, 0x004b, 0x004a, 0x0049, 0x0048, 0x0048, 0x0047, 0x0047, |
132 | 0x0046, 0x0045, 0x0045, 0x0044, 0x0044, 0x0043, 0x0042, 0x0042, |
133 | 0x0041, 0x0041, 0x0040, 0x0040, 0x003f, 0x003f, 0x003e, 0x003e, |
134 | 0x003d, 0x003d, 0x003c, 0x003c, 0x003b, 0x003b, 0x003a, 0x003a, |
135 | 0x003a, 0x0039, 0x0039, 0x0038, 0x0038, 0x0038, 0x0037, 0x0037, |
136 | 0x0036, 0x0036, 0x0036, 0x0035, 0x0035, 0x0035, 0x0034, 0x0034, |
137 | 0x0034, 0x0033, 0x0033, 0x0033, 0x0032, 0x0032, 0x0032, 0x0031, |
138 | 0x0031, 0x0031, 0x0030, 0x0030, 0x0030, 0x0030, 0x002f, 0x002f, |
139 | 0x002f, 0x002e, 0x002e, 0x002e, 0x002e, 0x002d, 0x002d, 0x002d, |
140 | 0x002d, 0x002c, 0x002c, 0x002c, 0x002c, 0x002b, 0x002b, 0x002b |
141 | }; |
142 | |
143 | #ifndef PI |
144 | #define PI 3.1415926535897932384626433832795 |
145 | #endif |
146 | |
147 | #define ST010_WORD(offset) (Memory.SRAM[offset + 1] << 8) | Memory.SRAM[offset] |
148 | #define ST010_DWORD(offset) (Memory.SRAM[offset + 3] << 24) | (Memory.SRAM[offset + 2] << 16) | (Memory.SRAM[offset + 1] << 8) | Memory.SRAM[offset] |
149 | |
150 | |
151 | static int16 ST010_Sin (int16 Theta) |
152 | { |
153 | return (ST010_SinTable[(Theta >> 8) & 0xff]); |
154 | } |
155 | |
156 | static int16 ST010_Cos (int16 Theta) |
157 | { |
158 | return (ST010_SinTable[((Theta + 0x4000) >> 8) & 0xff]); |
159 | } |
160 | |
161 | void (int16 x0, int16 y0, int16 &x1, int16 &y1, int16 &Quadrant, int16 &Theta) |
162 | { |
163 | if ((x0 <= 0) && (y0 < 0)) |
164 | { |
165 | x1 = -x0; |
166 | y1 = -y0; |
167 | Quadrant = -0x8000; |
168 | } |
169 | else if (x0 < 0) |
170 | { |
171 | x1 = y0; |
172 | y1 = -x0; |
173 | Quadrant = -0x4000; |
174 | } |
175 | else if (y0 < 0) |
176 | { |
177 | x1 = -y0; |
178 | y1 = x0; |
179 | Quadrant = 0x4000; |
180 | } |
181 | else |
182 | { |
183 | x1 = x0; |
184 | y1 = y0; |
185 | Quadrant = 0x0000; |
186 | } |
187 | |
188 | while ((x1 > 0x1f) || (y1 > 0x1f)) |
189 | { |
190 | if (x1 > 1) x1 >>= 1; |
191 | if (y1 > 1) y1 >>= 1; |
192 | } |
193 | |
194 | Theta = ST010_ArcTan[x1 & 0x1f][y1 & 0x1f] << 8; |
195 | Theta = (Theta | Quadrant) ^ 0x8000; |
196 | |
197 | if ((x0 == 0) && (y0 < 0)) Quadrant = 0x4000; |
198 | } |
199 | |
200 | static void ST010_Scale (int16 Multiplier, int16 X0, int16 Y0, int32 &X1, int32 &Y1) |
201 | { |
202 | X1 = X0 * Multiplier << 1; |
203 | Y1 = Y0 * Multiplier << 1; |
204 | } |
205 | |
206 | static void ST010_Multiply (int16 Multiplicand, int16 Multiplier, int32 &Product) |
207 | { |
208 | Product = Multiplicand * Multiplier << 1; |
209 | } |
210 | |
211 | static void (int16 Theta, int16 X0, int16 Y0, int16 &X1, int16 &Y1) |
212 | { |
213 | X1 = (Y0 * ST010_Sin(Theta) >> 15) + (X0 * ST010_Cos(Theta) >> 15); |
214 | Y1 = (Y0 * ST010_Cos(Theta) >> 15) - (X0 * ST010_Sin(Theta) >> 15); |
215 | } |
216 | |
217 | static void ST010_SortDrivers (uint16 Positions, uint16 Places[32], uint16 Drivers[32]) |
218 | { |
219 | bool Sorted; |
220 | uint16 Temp; |
221 | |
222 | if (Positions > 1) |
223 | { |
224 | do |
225 | { |
226 | Sorted = true; |
227 | |
228 | for (int i = 0; i < Positions - 1; i++) |
229 | { |
230 | if (Places[i] < Places[i + 1]) |
231 | { |
232 | Temp = Places[i + 1]; |
233 | Places[i + 1] = Places[i]; |
234 | Places[i] = Temp; |
235 | |
236 | Temp = Drivers[i + 1]; |
237 | Drivers[i + 1] = Drivers[i]; |
238 | Drivers[i] = Temp; |
239 | |
240 | Sorted = false; |
241 | } |
242 | } |
243 | |
244 | Positions--; |
245 | } |
246 | while (!Sorted); |
247 | } |
248 | } |
249 | |
250 | static void ST010_Raster(int16 Theta) |
251 | { |
252 | int16 data; |
253 | int offset = 0; |
254 | |
255 | for (int i = 0; i < 176; i++) |
256 | { |
257 | data = ST010_M7Scale[i] * ST010_Cos(Theta) >> 15; |
258 | |
259 | Memory.SRAM[0x00f0 + offset] = data; |
260 | Memory.SRAM[0x00f1 + offset] = data >> 8; |
261 | |
262 | Memory.SRAM[0x0510 + offset] = data; |
263 | Memory.SRAM[0x0511 + offset] = data >> 8; |
264 | |
265 | data = ST010_M7Scale[i] * ST010_Sin(Theta) >> 15; |
266 | |
267 | Memory.SRAM[0x0250 + offset] = data; |
268 | Memory.SRAM[0x0251 + offset] = data >> 8; |
269 | |
270 | if (data) data = ~data; |
271 | |
272 | Memory.SRAM[0x03b0 + offset] = data; |
273 | Memory.SRAM[0x03b1 + offset] = data >> 8; |
274 | |
275 | offset += 2; |
276 | } |
277 | } |
278 | |
279 | static void ST010_Distance(int16 x0, int16 y0, int16 &Distance) |
280 | { |
281 | int32 Product; |
282 | |
283 | x0 = (x0 < 0) ? -x0 : x0; |
284 | y0 = (y0 < 0) ? -y0 : y0; |
285 | |
286 | if((uint16) x0 >= ((uint16) y0)) |
287 | Product = ((x0 * 0x3d78 << 1) + (y0 * 0x1976 << 1)) << 1; |
288 | else |
289 | Product = ((y0 * 0x3d78 << 1) + (x0 * 0x1976 << 1)) << 1; |
290 | |
291 | Distance = (Product + 0x8000) >> 16; |
292 | } |
293 | |
294 | static void (int16 &MaxX, int16 &MaxY, int32 &x0, int32 &y0, int16 &Theta0, int16 &Theta1, int16 &x1, int16 &y1, uint16 &Radius, uint16 Increment, uint16 MaxRadius, int16 &Compass, int16 &Flags, int16 NewMaxX, int16 NewMaxY) |
295 | { |
296 | int16 dummy1,dummy2,dummy3; |
297 | uint16 utemp16; |
298 | int32 temp32; |
299 | |
300 | |
301 | x1 = MaxX - (x0 >> 16); |
302 | y1 = MaxY - (y0 >> 16); |
303 | |
304 | ST010_Compass(x1, y1, dummy1, dummy2, dummy3, Theta1); |
305 | Theta1 -= Theta0; |
306 | |
307 | if (Theta1 & 0xff00) |
308 | Theta0 += (Theta1 & 0x8000) ? 0xfd80 : 0x0280; |
309 | |
310 | // compiler notice: -0x8000 ==> +0x8000 |
311 | utemp16 = ((Theta1 < 0) ? (int16) -Theta1 : Theta1) >> 4; |
312 | |
313 | if (utemp16 < 0x0100) |
314 | { |
315 | temp32 = Radius + Increment; |
316 | Radius = (temp32 >= MaxRadius) ? MaxRadius : (uint16) temp32; |
317 | } |
318 | |
319 | else |
320 | { |
321 | temp32 = Radius - utemp16; |
322 | Radius = (temp32 <= 0) ? 0 : (uint16) temp32; |
323 | } |
324 | |
325 | x0 -= ((ST010_Sin(Theta0) >> 5) * (Radius >> 8)) << 1; |
326 | y0 -= ((ST010_Cos(Theta0) >> 5) * (Radius >> 8)) << 1; |
327 | |
328 | x0 &= 0x1fffffff; |
329 | y0 &= 0x1fffffff; |
330 | |
331 | |
332 | int16 MaxRadiusX, MaxRadiusY; |
333 | if (Compass & 0x8000) |
334 | { |
335 | MaxRadiusX = 0x0008; |
336 | MaxRadiusY = 0x0080; |
337 | } |
338 | else |
339 | { |
340 | MaxRadiusX = 0x0080; |
341 | MaxRadiusY = 0x0008; |
342 | } |
343 | |
344 | if ((abs(x1) < MaxRadiusX) && (abs(y1) < MaxRadiusY)) |
345 | { |
346 | MaxX = NewMaxX; |
347 | MaxY = NewMaxY & 0x0fff; |
348 | Compass = (NewMaxY & 0x8000) ? 0xffff : 0x0000; |
349 | Flags |= 0x0008; |
350 | } |
351 | } |
352 | |
353 | uint8 S9xGetST010 (uint32 Address) |
354 | { |
355 | if (!(Address & 0x80000)) |
356 | return (0x80); |
357 | |
358 | if ((Address & 0xFFF) == 0x20) |
359 | return (ST010.op_reg); |
360 | |
361 | if ((Address & 0xFFF) == 0x21) |
362 | return (ST010.execute); |
363 | |
364 | return (Memory.SRAM[Address & Memory.SRAMMask]); |
365 | } |
366 | |
367 | void S9xSetST010 (uint32 Address, uint8 Byte) |
368 | { |
369 | if (!(Address & 0x80000)) |
370 | { |
371 | ST010.control_enable = TRUE; |
372 | return; |
373 | } |
374 | |
375 | #ifdef DEBUGGER |
376 | printf("Write %06X:%02X\n" , Address, Byte); |
377 | #endif |
378 | |
379 | if ((Address & 0xFFF) == 0x20 && ST010.control_enable) |
380 | ST010.op_reg = Byte; |
381 | |
382 | if ((Address & 0xFFF) == 0x21 && ST010.control_enable) |
383 | ST010.execute = Byte; |
384 | else |
385 | Memory.SRAM[Address & Memory.SRAMMask] = Byte; |
386 | |
387 | if (ST010.execute & 0x80) |
388 | { |
389 | switch (ST010.op_reg) |
390 | { |
391 | // Heading |
392 | case 0x01: |
393 | { |
394 | Memory.SRAM[0x0006] = Memory.SRAM[0x0002]; |
395 | Memory.SRAM[0x0007] = Memory.SRAM[0x0003]; |
396 | |
397 | #ifdef FAST_LSB_WORD_ACCESS |
398 | ST010_Compass(*(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0000], (int16 &) Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0004], (int16 &) Memory.SRAM[0x0010]); |
399 | #else |
400 | int16 x1, y1, Quadrant, Theta; |
401 | |
402 | ST010_Compass(ST010_WORD(0x0000), ST010_WORD(0x0002), x1, y1, Quadrant, Theta); |
403 | |
404 | Memory.SRAM[0x0000] = (uint8) (x1); |
405 | Memory.SRAM[0x0001] = (uint8) (x1 >> 8); |
406 | Memory.SRAM[0x0002] = (uint8) (y1); |
407 | Memory.SRAM[0x0003] = (uint8) (y1 >> 8); |
408 | Memory.SRAM[0x0004] = (uint8) (Quadrant); |
409 | Memory.SRAM[0x0005] = (uint8) (Quadrant >> 8); |
410 | Memory.SRAM[0x0010] = (uint8) (Theta); |
411 | Memory.SRAM[0x0011] = (uint8) (Theta >> 8); |
412 | #endif |
413 | break; |
414 | } |
415 | |
416 | // Sorts Driver Placements |
417 | case 0x02: |
418 | { |
419 | #ifdef FAST_LSB_WORD_ACCESS |
420 | ST010_SortDrivers(*(uint16 *) (Memory.SRAM + 0x0024), (uint16 *) (Memory.SRAM + 0x0040), (uint16 *) (Memory.SRAM + 0x0080)); |
421 | #else |
422 | uint16 Places[32]; |
423 | uint16 Positions = ST010_WORD(0x0024); |
424 | int Pos, Offset; |
425 | |
426 | Offset = 0; |
427 | |
428 | for (Pos = 0; Pos < Positions; Pos++) |
429 | { |
430 | Places[Pos] = ST010_WORD(0x0040 + Offset); |
431 | Offset += 2; |
432 | } |
433 | |
434 | ST010_SortDrivers(Positions, Places, (uint16 *) (Memory.SRAM + 0x0080)); |
435 | |
436 | Offset = 0; |
437 | |
438 | for (Pos = 0; Pos < Positions; Pos++) |
439 | { |
440 | Memory.SRAM[0x0040 + Offset] = (uint8) (Places[Pos]); |
441 | Memory.SRAM[0x0041 + Offset] = (uint8) (Places[Pos] >> 8); |
442 | Offset += 2; |
443 | } |
444 | #endif |
445 | break; |
446 | } |
447 | |
448 | // Two Dimensional Coordinate Scale |
449 | case 0x03: |
450 | { |
451 | #ifdef FAST_LSB_WORD_ACCESS |
452 | ST010_Scale(*(int16 *) &Memory.SRAM[0x0004], *(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int32 &) Memory.SRAM[0x0010], (int32 &) Memory.SRAM[0x0014]); |
453 | #else |
454 | int32 x1, y1; |
455 | |
456 | ST010_Scale(ST010_WORD(0x0004), ST010_WORD(0x0000), ST010_WORD(0x0002), x1, y1); |
457 | |
458 | Memory.SRAM[0x0010] = (uint8) (x1); |
459 | Memory.SRAM[0x0011] = (uint8) (x1 >> 8); |
460 | Memory.SRAM[0x0012] = (uint8) (x1 >> 16); |
461 | Memory.SRAM[0x0013] = (uint8) (x1 >> 24); |
462 | Memory.SRAM[0x0014] = (uint8) (y1); |
463 | Memory.SRAM[0x0015] = (uint8) (y1 >> 8); |
464 | Memory.SRAM[0x0016] = (uint8) (y1 >> 16); |
465 | Memory.SRAM[0x0017] = (uint8) (y1 >> 24); |
466 | #endif |
467 | break; |
468 | } |
469 | |
470 | // calculate the vector length of (x, y) |
471 | case 0x04: |
472 | { |
473 | #ifdef FAST_LSB_WORD_ACCESS |
474 | ST010_Distance(*(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0010]); |
475 | #else |
476 | int16 square; |
477 | |
478 | ST010_Distance(ST010_WORD(0x0000), ST010_WORD(0x0002), square); |
479 | |
480 | Memory.SRAM[0x10] = (uint8) (square); |
481 | Memory.SRAM[0x11] = (uint8) (square >> 8); |
482 | #endif |
483 | break; |
484 | } |
485 | |
486 | // calculate AI orientation based on specific guidelines |
487 | case 0x05: |
488 | { |
489 | #ifdef FAST_LSB_WORD_ACCESS |
490 | ST010_Navigation((int16 &) Memory.SRAM[0x00c0], (int16 &) Memory.SRAM[0x00c2], (int32 &) Memory.SRAM[0x00c4], (int32 &) Memory.SRAM[0x00c8], (int16 &) Memory.SRAM[0x00cc], (int16 &) Memory.SRAM[0x00ce], (int16 &) Memory.SRAM[0x00d0], (int16 &) Memory.SRAM[0x00d2], (uint16 &) Memory.SRAM[0x00d4], *(uint16 *) &Memory.SRAM[0x00d6], *(uint16 *) &Memory.SRAM[0x00d8], (int16 &) Memory.SRAM[0x00da], (int16 &) Memory.SRAM[0x00dc], *(int16 *) &Memory.SRAM[0x00de], *(int16 *) &Memory.SRAM[0x00e0]); |
491 | #else |
492 | int32 x0,y0; |
493 | int16 MaxX,MaxY,Theta0,Theta1,x1,y1,Compass,Flags; |
494 | uint16 Radius; |
495 | |
496 | MaxX = ST010_WORD(0x00c0); |
497 | MaxY = ST010_WORD(0x00c2); |
498 | x0 = ST010_DWORD(0x00c4); |
499 | y0 = ST010_DWORD(0x00c8); |
500 | Theta0 = ST010_WORD(0x00cc); |
501 | Radius = ST010_WORD(0x00d4); |
502 | Compass = ST010_WORD(0x00da); |
503 | Flags = ST010_WORD(0x00dc); |
504 | |
505 | ST010_Navigation(MaxX, MaxY, x0, y0, Theta0, Theta1, x1, y1, Radius, ST010_WORD(0x00d6), ST010_WORD(0x00d8), Compass, Flags, ST010_WORD(0x00de), ST010_WORD(0x00e0)); |
506 | |
507 | Memory.SRAM[0x00c0] = (uint8) (MaxX); |
508 | Memory.SRAM[0x00c1] = (uint8) (MaxX >> 8); |
509 | Memory.SRAM[0x00c2] = (uint8) (MaxY); |
510 | Memory.SRAM[0x00c3] = (uint8) (MaxY >> 8); |
511 | Memory.SRAM[0x00c4] = (uint8) (x0); |
512 | Memory.SRAM[0x00c5] = (uint8) (x0 >> 8); |
513 | Memory.SRAM[0x00c6] = (uint8) (x0 >> 16); |
514 | Memory.SRAM[0x00c7] = (uint8) (x0 >> 24); |
515 | Memory.SRAM[0x00c8] = (uint8) (y0); |
516 | Memory.SRAM[0x00c9] = (uint8) (y0 >> 8); |
517 | Memory.SRAM[0x00ca] = (uint8) (y0 >> 16); |
518 | Memory.SRAM[0x00cb] = (uint8) (y0 >> 24); |
519 | Memory.SRAM[0x00cc] = (uint8) (Theta0); |
520 | Memory.SRAM[0x00cd] = (uint8) (Theta0 >> 8); |
521 | Memory.SRAM[0x00ce] = (uint8) (Theta1); |
522 | Memory.SRAM[0x00cf] = (uint8) (Theta1 >> 8); |
523 | Memory.SRAM[0x00d0] = (uint8) (x1); |
524 | Memory.SRAM[0x00d1] = (uint8) (x1 >> 8); |
525 | Memory.SRAM[0x00d2] = (uint8) (y1); |
526 | Memory.SRAM[0x00d3] = (uint8) (y1 >> 8); |
527 | Memory.SRAM[0x00d4] = (uint8) (Radius); |
528 | Memory.SRAM[0x00d5] = (uint8) (Radius >> 8); |
529 | Memory.SRAM[0x00da] = (uint8) (Compass); |
530 | Memory.SRAM[0x00db] = (uint8) (Compass >> 8); |
531 | Memory.SRAM[0x00dc] = (uint8) (Flags); |
532 | Memory.SRAM[0x00dd] = (uint8) (Flags >> 8); |
533 | #endif |
534 | break; |
535 | } |
536 | |
537 | // 16-bit Multiplication |
538 | case 0x06: |
539 | { |
540 | #ifdef FAST_LSB_WORD_ACCESS |
541 | ST010_Multiply(*(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int32 &) Memory.SRAM[0x0010]); |
542 | #else |
543 | int32 Product; |
544 | |
545 | ST010_Multiply(ST010_WORD(0x0000), ST010_WORD(0x0002), Product); |
546 | |
547 | Memory.SRAM[0x0010] = (uint8) (Product); |
548 | Memory.SRAM[0x0011] = (uint8) (Product >> 8); |
549 | Memory.SRAM[0x0012] = (uint8) (Product >> 16); |
550 | Memory.SRAM[0x0013] = (uint8) (Product >> 24); |
551 | #endif |
552 | break; |
553 | } |
554 | |
555 | // Mode 7 Raster Data Calculation |
556 | case 0x07: |
557 | { |
558 | #ifdef FAST_LSB_WORD_ACCESS |
559 | ST010_Raster(*(int16 *) &Memory.SRAM[0x0000]); |
560 | #else |
561 | ST010_Raster(ST010_WORD(0x0000)); |
562 | #endif |
563 | |
564 | // Shift Angle for use with Lookup table |
565 | Memory.SRAM[0x00] = Memory.SRAM[0x01]; |
566 | Memory.SRAM[0x01] = 0x00; |
567 | |
568 | break; |
569 | } |
570 | |
571 | // Two dimensional Coordinate Rotation |
572 | case 0x08: |
573 | { |
574 | #ifdef FAST_LSB_WORD_ACCESS |
575 | ST010_Rotate(*(int16 *) &Memory.SRAM[0x0004], *(int16 *) &Memory.SRAM[0x0000], *(int16 *) &Memory.SRAM[0x0002], (int16 &) Memory.SRAM[0x0010], (int16 &) Memory.SRAM[0x0012]); |
576 | #else |
577 | int16 x1, y1; |
578 | |
579 | ST010_Rotate(ST010_WORD(0x0004), ST010_WORD(0x0000), ST010_WORD(0x0002), x1, y1); |
580 | |
581 | Memory.SRAM[0x0010] = (uint8) (x1); |
582 | Memory.SRAM[0x0011] = (uint8) (x1 >> 8); |
583 | Memory.SRAM[0x0012] = (uint8) (y1); |
584 | Memory.SRAM[0x0013] = (uint8) (y1 >> 8); |
585 | #endif |
586 | break; |
587 | } |
588 | |
589 | default: |
590 | #ifdef DEBUGGER |
591 | printf("Unknown Op\n" ); |
592 | #endif |
593 | break; |
594 | } |
595 | |
596 | // lower signal: op processed |
597 | ST010.op_reg = 0; |
598 | ST010.execute = 0; |
599 | } |
600 | } |
601 | |