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
12static 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
48static 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
117static 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
151static int16 ST010_Sin (int16 Theta)
152{
153 return (ST010_SinTable[(Theta >> 8) & 0xff]);
154}
155
156static int16 ST010_Cos (int16 Theta)
157{
158 return (ST010_SinTable[((Theta + 0x4000) >> 8) & 0xff]);
159}
160
161void ST010_Compass(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
200static 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
206static void ST010_Multiply (int16 Multiplicand, int16 Multiplier, int32 &Product)
207{
208 Product = Multiplicand * Multiplier << 1;
209}
210
211static void ST010_Rotate (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
217static 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
250static 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
279static 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
294static void ST010_Navigation(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
353uint8 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
367void 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