← Back to branch summary

~ubuntu-branches/debian/sid/mame/sid

« back to all changes in this revision

Viewing changes to src/mame/machine/snesdsp1.c

  • Committer: Bazaar Package Importer
  • Author(s): Jordi Mallach, Emmanuel Kasper, Félix Arreola Rodríguez, Jordi Mallach
  • Date: 2011-05-11 21:06:50 UTC
  • mfrom: (1.1.1 upstream)
  • Revision ID: james.westby@ubuntu.com-20110511210650-jizvh8a6x117y9hr
Tags: 0.142-1
[ Emmanuel Kasper ]
* New upstream release
* Set NOWERROR=1 to allow compiling with gcc-4.6
* Remove fix_powerpc_build.patch, as upstream has taken it in this release
* Add gnome-video-arcade front end as a suggested package

[ Félix Arreola Rodríguez ]
* Add kfreebsd-build.patch to quilt series, to fix build on kfreebsd

[ Jordi Mallach ]
* Remove unneeded and bogus addition of --with-quilt to the dh invocation.
* Add Cesare Falco (long time Ubuntu maintainer) to Uploaders, and wrap
  them into multiple lines.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/mame/machine/snesdsp1.c
1
 
/***************************************************************************
2
 
 
3
 
  snesdsp1.c
4
 
 
5
 
  File to handle emulation of the SNES "DSP-1" add-on chip.
6
 
 
7
 
  Original C++ "dsp1emul.cpp" by Andreas Naive
8
 
  Based on research by Overload, The Dumper, Neviksti and Andreas Naive
9
 
  MAME/MESS C conversion by R. Belmont
10
 
 
11
 
  This is up to date with the source version dated June 2006.
12
 
 
13
 
***************************************************************************/
14
 
 
15
 
#define dsp1_VERSION 0x0102
16
 
 
17
 
// The DSP-1 status register has 16 bits, but only
18
 
// the upper 8 bits can be accessed from an external device, so all these
19
 
// positions are referred to the upper byte (bits D8 to D15)
20
 
 
21
 
enum SrFlags { DRC = 0x04, DRS = 0x10, RQM = 0x80 };
22
 
 
23
 
// According to Overload's docs, these are the meanings of the flags:
24
 
// DRC: The Data Register Control (DRC) bit specifies the data transfer length to and from the host CPU.
25
 
//  0: Data transfer to and from the DSP-1 is 16 bits.
26
 
//  1: Data transfer to and from the DSP-1 is 8 bits.
27
 
// DRS: The Data Register Status (DRS) bit indicates the data transfer status in the case of transfering 16-bit data.
28
 
//  0: Data transfer has terminated.
29
 
//  1: Data transfer in progress.
30
 
// RQM: The Request for Master (RQM) indicates that the DSP1 is requesting host CPU for data read/write.
31
 
//  0: Internal Data Register Transfer.
32
 
//  1: External Data Register Transfer.
33
 
 
34
 
enum Fsm_Major_State { WAIT_COMMAND, READ_DATA, WRITE_DATA };
35
 
enum Max_Data_Accesses { MAX_READS = 7, MAX_WRITES = 1024 };
36
 
 
37
 
struct dsp1_Command
38
 
{
39
 
        void (*callback)(INT16 *, INT16 *);
40
 
        unsigned int reads;
41
 
        unsigned int writes;
42
 
};
43
 
 
44
 
struct SharedData
45
 
{
46
 
        // some RAM variables shared between commands
47
 
        INT16 MatrixA[3][3];          // attitude matrix A
48
 
        INT16 MatrixB[3][3];
49
 
        INT16 MatrixC[3][3];
50
 
        INT16 CentreX, CentreY, CentreZ;   // center of projection
51
 
        INT16 CentreZ_C, CentreZ_E;
52
 
        INT16 VOffset;                     // vertical offset of the screen with regard to the centre of projection
53
 
        INT16 Les, C_Les, E_Les;
54
 
        INT16 SinAas, CosAas;
55
 
        INT16 SinAzs, CosAzs;
56
 
        INT16 SinAZS, CosAZS;
57
 
        INT16 SecAZS_C1, SecAZS_E1;
58
 
        INT16 SecAZS_C2, SecAZS_E2;
59
 
        INT16 Nx, Ny, Nz;    // normal vector to the screen (norm 1, points toward the center of projection)
60
 
        INT16 Gx, Gy, Gz;    // center of the screen (global coordinates)
61
 
        INT16 Hx, Hy;        // horizontal vector of the screen (Hz=0, norm 1, points toward the right of the screen)
62
 
        INT16 Vx, Vy, Vz;    // vertical vector of the screen (norm 1, points toward the top of the screen)
63
 
};
64
 
 
65
 
struct _snes_dsp1_state
66
 
{
67
 
        SharedData       shared;
68
 
        UINT8            Sr;                       // status register
69
 
        int              SrLowByteAccess;
70
 
        UINT16           Dr;                       // "internal" representation of the data register
71
 
        Fsm_Major_State  FsmMajorState;            // current major state of the FSM
72
 
        UINT8            Command;                  // current command processed by the FSM
73
 
        UINT8            DataCounter;              // #UINT16 read/writes counter used by the FSM
74
 
        INT16            ReadBuffer[MAX_READS];
75
 
        INT16            WriteBuffer[MAX_WRITES];
76
 
        UINT8            Freeze;                   // need explanation?  ;)
77
 
 
78
 
 
79
 
 
80
 
//////////////////////////////////////////////////////////////////
81
 
 
82
 
// Data ROM, as logged from a DSP-1B with the 0x1f command;
83
 
// it contains the tables and constants used by the commands.
84
 
// The tables used are: two shift tables (0x022-0x031 and 0x031-0x040 -this last one
85
 
// with an error in 0x03c which has survived to all the DSP-1 revisions-); a inverse
86
 
// table (used as initial guess) at 0x065-0x0e4; a square root table (used also
87
 
// as initial guess) at 0x0e5-0x115; two sin and cos tables (used as nodes to construct
88
 
// a interpolation curve) at, respectively, 0x116-0x197 and 0x196-0x215.
89
 
// As a curiosity, in the positions 0x21c-0x31c it's contained a
90
 
// 257-points arccos table that, apparently, have been not used anywhere
91
 
// (maybe for the MaxAZS_Exp table?).
92
 
 
93
 
        UINT16         DataRom[1024];
94
 
};
95
 
 
96
 
static struct _snes_dsp1_state  dsp1_state;
97
 
 
98
 
 
99
 
/* Prototypes */
100
 
 
101
 
static void dsp1_fsm_step(UINT8 read, UINT8 *data);            // FSM logic
102
 
 
103
 
// commands
104
 
static void dsp1_memory_test(INT16 *input, INT16 *output);
105
 
static void dsp1_memory_dump(INT16 *input, INT16 *output);
106
 
static void dsp1_memory_size(INT16 *input, INT16 *output);
107
 
static void dsp1_multiply(INT16* input, INT16* output);
108
 
static void dsp1_multiply2(INT16* input, INT16* output);
109
 
static void dsp1_inverse(INT16 *input, INT16 *output);
110
 
static void dsp1_triangle(INT16 *input, INT16 *output);
111
 
static void dsp1_radius(INT16 *input, INT16 *output);
112
 
static void dsp1_range(INT16 *input, INT16 *output);
113
 
static void dsp1_range2(INT16 *input, INT16 *output);
114
 
static void dsp1_distance(INT16 *input, INT16 *output);
115
 
static void dsp1_rotate(INT16 *input, INT16 *output);
116
 
static void dsp1_polar(INT16 *input, INT16 *output);
117
 
static void dsp1_attitudeA(INT16 *input, INT16 *output);
118
 
static void dsp1_attitudeB(INT16 *input, INT16 *output);
119
 
static void dsp1_attitudeC(INT16 *input, INT16 *output);
120
 
static void dsp1_objectiveA(INT16 *input, INT16 *output);
121
 
static void dsp1_objectiveB(INT16 *input, INT16 *output);
122
 
static void dsp1_objectiveC(INT16 *input, INT16 *output);
123
 
static void dsp1_subjectiveA(INT16 *input, INT16 *output);
124
 
static void dsp1_subjectiveB(INT16 *input, INT16 *output);
125
 
static void dsp1_subjectiveC(INT16 *input, INT16 *output);
126
 
static void dsp1_scalarA(INT16 *input, INT16 *output);
127
 
static void dsp1_scalarB(INT16 *input, INT16 *output);
128
 
static void dsp1_scalarC(INT16 *input, INT16 *output);
129
 
static void dsp1_gyrate(INT16 *input, INT16 *output);
130
 
static void dsp1_parameter(INT16 *input, INT16 *output);
131
 
static void dsp1_raster(INT16 *input, INT16 *output);
132
 
static void dsp1_target(INT16 *input, INT16 *output);
133
 
static void dsp1_project(INT16 *input, INT16 *output);
134
 
 
135
 
// auxiliar functions
136
 
static INT16 dsp1_sin(INT16 Angle);
137
 
static INT16 dsp1_cos(INT16 Angle);
138
 
static void inverse(INT16 Coefficient, INT16 Exponent, INT16 *iCoefficient, INT16 *iExponent);
139
 
static INT16 denormalize_and_clip(INT16 C, INT16 E);
140
 
static void normalize(INT16 m, INT16 *Coefficient, INT16 *Exponent);
141
 
static void normalize_double(INT32 Product, INT16 *Coefficient, INT16 *Exponent);
142
 
static INT16 shiftR(INT16 C, INT16 E);
143
 
 
144
 
// register saves
145
 
static void dsp1_register_save(running_machine *machine);
146
 
 
147
 
//////////////////////////////////////////////////////////////////
148
 
 
149
 
// The info on this table follows Overload's docs.
150
 
 
151
 
static const struct dsp1_Command mCommandTable[0x40] =
152
 
{
153
 
        {&dsp1_multiply, 2, 1},   //0x00
154
 
        {&dsp1_attitudeA, 4, 0},    //0x01
155
 
        {&dsp1_parameter, 7, 4},   //0x02
156
 
        {&dsp1_subjectiveA, 3, 3},    //0x03
157
 
        {&dsp1_triangle, 2, 2},   //0x04
158
 
        {&dsp1_attitudeA, 4, 0},   //0x01
159
 
        {&dsp1_project, 3, 3},   //0x06
160
 
        {&dsp1_memory_test, 1, 1},   //0x0f
161
 
        {&dsp1_radius, 3, 2},   //0x08
162
 
        {&dsp1_objectiveA, 3, 3},   //0x0d
163
 
        {&dsp1_raster, 1, 4},   // 0x0a. This will normally work in continuous mode
164
 
        {&dsp1_scalarA, 3, 1},   //0x0b
165
 
        {&dsp1_rotate, 3, 2},   //0x0c
166
 
        {&dsp1_objectiveA, 3, 3},   //0x0d
167
 
        {&dsp1_target, 2, 2},   //0x0e
168
 
        {&dsp1_memory_test, 1, 1},   //0x0f
169
 
 
170
 
        {&dsp1_inverse, 2, 2},   //0x10
171
 
        {&dsp1_attitudeB, 4, 0},   //0x11
172
 
        {&dsp1_parameter, 7, 4},   //0x02
173
 
        {&dsp1_subjectiveB, 3, 3},   //0x13
174
 
        {&dsp1_gyrate, 6, 3},   //0x14
175
 
        {&dsp1_attitudeB, 4, 0},   //0x11
176
 
        {&dsp1_project, 3, 3},   //0x06
177
 
        {&dsp1_memory_dump, 1, 1024},   //0x1f
178
 
        {&dsp1_range, 4, 1},   //0x18
179
 
        {&dsp1_objectiveB, 3, 3},   //0x1d
180
 
        {0, 0, 0},   // 0x1a; the chip freezes
181
 
        {&dsp1_scalarB, 3, 1},   //0x1b
182
 
        {&dsp1_polar, 6, 3},   //0x1c
183
 
        {&dsp1_objectiveB, 3, 3},   //0x1d
184
 
        {&dsp1_target, 2, 2},   //0x0e
185
 
        {&dsp1_memory_dump, 1, 1024},   //0x1f
186
 
 
187
 
        {&dsp1_multiply2, 2, 1},   //0x20
188
 
        {&dsp1_attitudeC, 4, 0},   //0x21
189
 
        {&dsp1_parameter, 7, 4},   //0x02
190
 
        {&dsp1_subjectiveC, 3, 3},   //0x23
191
 
        {&dsp1_triangle, 2, 2},   //0x04
192
 
        {&dsp1_attitudeC, 4, 0},   //0x21
193
 
        {&dsp1_project, 3, 3},   //0x06
194
 
        {&dsp1_memory_size, 1, 1},    //0x2f
195
 
        {&dsp1_distance, 3, 1},   //0x28
196
 
        {&dsp1_objectiveC, 3, 3},   //0x2d
197
 
        {0, 0, 0},   // 0x1a; the chip freezes
198
 
        {&dsp1_scalarC, 3, 1},   //0x2b
199
 
        {&dsp1_rotate, 3, 2},   //0x0c
200
 
        {&dsp1_objectiveC, 3, 3},   //0x2d
201
 
        {&dsp1_target, 2, 2},   //0x0e
202
 
        {&dsp1_memory_size, 1, 1},   //0x2f
203
 
 
204
 
        {&dsp1_inverse, 2, 2},   //0x10
205
 
        {&dsp1_attitudeA, 4, 0},   //0x01
206
 
        {&dsp1_parameter, 7, 4},   //0x02
207
 
        {&dsp1_subjectiveA, 3, 3},   //0x03
208
 
        {&dsp1_gyrate, 6, 3},   //0x14
209
 
        {&dsp1_attitudeA, 4, 0},   //0x01
210
 
        {&dsp1_project, 3, 3},   //0x06
211
 
        {&dsp1_memory_dump, 1, 1024},   //0x1f
212
 
        {&dsp1_range2, 4, 1},   //0x38
213
 
        {&dsp1_objectiveA, 3, 3},   //0x0d
214
 
        {0, 0, 0},   // 0x1a; the chip freezes
215
 
        {&dsp1_scalarA, 3, 1},   //0x0b
216
 
        {&dsp1_polar, 6, 3},   //0x1c
217
 
        {&dsp1_objectiveA, 3, 3},   //0x0d
218
 
        {&dsp1_target, 2, 2},   //0x0e
219
 
        {&dsp1_memory_dump, 1, 1024},   //0x1f
220
 
};
221
 
 
222
 
//////////////////////////////////////////////////////////////////
223
 
 
224
 
static const INT16 dsp1_MaxAZS_Exp[16] = {
225
 
   0x38b4, 0x38b7, 0x38ba, 0x38be, 0x38c0, 0x38c4, 0x38c7, 0x38ca,
226
 
   0x38ce, 0x38d0, 0x38d4, 0x38d7, 0x38da, 0x38dd, 0x38e0, 0x38e4
227
 
};
228
 
 
229
 
//////////////////////////////////////////////////////////////////
230
 
 
231
 
static const INT16 dsp1_sin_table[256] = {
232
 
   0x0000,  0x0324,  0x0647,  0x096a,  0x0c8b,  0x0fab,  0x12c8,  0x15e2,
233
 
   0x18f8,  0x1c0b,  0x1f19,  0x2223,  0x2528,  0x2826,  0x2b1f,  0x2e11,
234
 
   0x30fb,  0x33de,  0x36ba,  0x398c,  0x3c56,  0x3f17,  0x41ce,  0x447a,
235
 
   0x471c,  0x49b4,  0x4c3f,  0x4ebf,  0x5133,  0x539b,  0x55f5,  0x5842,
236
 
   0x5a82,  0x5cb4,  0x5ed7,  0x60ec,  0x62f2,  0x64e8,  0x66cf,  0x68a6,
237
 
   0x6a6d,  0x6c24,  0x6dca,  0x6f5f,  0x70e2,  0x7255,  0x73b5,  0x7504,
238
 
   0x7641,  0x776c,  0x7884,  0x798a,  0x7a7d,  0x7b5d,  0x7c29,  0x7ce3,
239
 
   0x7d8a,  0x7e1d,  0x7e9d,  0x7f09,  0x7f62,  0x7fa7,  0x7fd8,  0x7ff6,
240
 
   0x7fff,  0x7ff6,  0x7fd8,  0x7fa7,  0x7f62,  0x7f09,  0x7e9d,  0x7e1d,
241
 
   0x7d8a,  0x7ce3,  0x7c29,  0x7b5d,  0x7a7d,  0x798a,  0x7884,  0x776c,
242
 
   0x7641,  0x7504,  0x73b5,  0x7255,  0x70e2,  0x6f5f,  0x6dca,  0x6c24,
243
 
   0x6a6d,  0x68a6,  0x66cf,  0x64e8,  0x62f2,  0x60ec,  0x5ed7,  0x5cb4,
244
 
   0x5a82,  0x5842,  0x55f5,  0x539b,  0x5133,  0x4ebf,  0x4c3f,  0x49b4,
245
 
   0x471c,  0x447a,  0x41ce,  0x3f17,  0x3c56,  0x398c,  0x36ba,  0x33de,
246
 
   0x30fb,  0x2e11,  0x2b1f,  0x2826,  0x2528,  0x2223,  0x1f19,  0x1c0b,
247
 
   0x18f8,  0x15e2,  0x12c8,  0x0fab,  0x0c8b,  0x096a,  0x0647,  0x0324,
248
 
   -0x0000, -0x0324, -0x0647, -0x096a, -0x0c8b, -0x0fab, -0x12c8, -0x15e2,
249
 
   -0x18f8, -0x1c0b, -0x1f19, -0x2223, -0x2528, -0x2826, -0x2b1f, -0x2e11,
250
 
   -0x30fb, -0x33de, -0x36ba, -0x398c, -0x3c56, -0x3f17, -0x41ce, -0x447a,
251
 
   -0x471c, -0x49b4, -0x4c3f, -0x4ebf, -0x5133, -0x539b, -0x55f5, -0x5842,
252
 
   -0x5a82, -0x5cb4, -0x5ed7, -0x60ec, -0x62f2, -0x64e8, -0x66cf, -0x68a6,
253
 
   -0x6a6d, -0x6c24, -0x6dca, -0x6f5f, -0x70e2, -0x7255, -0x73b5, -0x7504,
254
 
   -0x7641, -0x776c, -0x7884, -0x798a, -0x7a7d, -0x7b5d, -0x7c29, -0x7ce3,
255
 
   -0x7d8a, -0x7e1d, -0x7e9d, -0x7f09, -0x7f62, -0x7fa7, -0x7fd8, -0x7ff6,
256
 
   -0x7fff, -0x7ff6, -0x7fd8, -0x7fa7, -0x7f62, -0x7f09, -0x7e9d, -0x7e1d,
257
 
   -0x7d8a, -0x7ce3, -0x7c29, -0x7b5d, -0x7a7d, -0x798a, -0x7884, -0x776c,
258
 
   -0x7641, -0x7504, -0x73b5, -0x7255, -0x70e2, -0x6f5f, -0x6dca, -0x6c24,
259
 
   -0x6a6d, -0x68a6, -0x66cf, -0x64e8, -0x62f2, -0x60ec, -0x5ed7, -0x5cb4,
260
 
   -0x5a82, -0x5842, -0x55f5, -0x539b, -0x5133, -0x4ebf, -0x4c3f, -0x49b4,
261
 
   -0x471c, -0x447a, -0x41ce, -0x3f17, -0x3c56, -0x398c, -0x36ba, -0x33de,
262
 
   -0x30fb, -0x2e11, -0x2b1f, -0x2826, -0x2528, -0x2223, -0x1f19, -0x1c0b,
263
 
   -0x18f8, -0x15e2, -0x12c8, -0x0fab, -0x0c8b, -0x096a, -0x0647, -0x0324};
264
 
 
265
 
   //////////////////////////////////////////////////////////////////
266
 
 
267
 
// Optimised for Performance
268
 
static const INT16 dsp1_mul_table[256] = {
269
 
      0x0000,  0x0003,  0x0006,  0x0009,  0x000c,  0x000f,  0x0012,  0x0015,
270
 
      0x0019,  0x001c,  0x001f,  0x0022,  0x0025,  0x0028,  0x002b,  0x002f,
271
 
      0x0032,  0x0035,  0x0038,  0x003b,  0x003e,  0x0041,  0x0045,  0x0048,
272
 
      0x004b,  0x004e,  0x0051,  0x0054,  0x0057,  0x005b,  0x005e,  0x0061,
273
 
      0x0064,  0x0067,  0x006a,  0x006d,  0x0071,  0x0074,  0x0077,  0x007a,
274
 
      0x007d,  0x0080,  0x0083,  0x0087,  0x008a,  0x008d,  0x0090,  0x0093,
275
 
      0x0096,  0x0099,  0x009d,  0x00a0,  0x00a3,  0x00a6,  0x00a9,  0x00ac,
276
 
      0x00af,  0x00b3,  0x00b6,  0x00b9,  0x00bc,  0x00bf,  0x00c2,  0x00c5,
277
 
      0x00c9,  0x00cc,  0x00cf,  0x00d2,  0x00d5,  0x00d8,  0x00db,  0x00df,
278
 
      0x00e2,  0x00e5,  0x00e8,  0x00eb,  0x00ee,  0x00f1,  0x00f5,  0x00f8,
279
 
      0x00fb,  0x00fe,  0x0101,  0x0104,  0x0107,  0x010b,  0x010e,  0x0111,
280
 
      0x0114,  0x0117,  0x011a,  0x011d,  0x0121,  0x0124,  0x0127,  0x012a,
281
 
      0x012d,  0x0130,  0x0133,  0x0137,  0x013a,  0x013d,  0x0140,  0x0143,
282
 
      0x0146,  0x0149,  0x014d,  0x0150,  0x0153,  0x0156,  0x0159,  0x015c,
283
 
      0x015f,  0x0163,  0x0166,  0x0169,  0x016c,  0x016f,  0x0172,  0x0175,
284
 
      0x0178,  0x017c,  0x017f,  0x0182,  0x0185,  0x0188,  0x018b,  0x018e,
285
 
      0x0192,  0x0195,  0x0198,  0x019b,  0x019e,  0x01a1,  0x01a4,  0x01a8,
286
 
      0x01ab,  0x01ae,  0x01b1,  0x01b4,  0x01b7,  0x01ba,  0x01be,  0x01c1,
287
 
      0x01c4,  0x01c7,  0x01ca,  0x01cd,  0x01d0,  0x01d4,  0x01d7,  0x01da,
288
 
      0x01dd,  0x01e0,  0x01e3,  0x01e6,  0x01ea,  0x01ed,  0x01f0,  0x01f3,
289
 
      0x01f6,  0x01f9,  0x01fc,  0x0200,  0x0203,  0x0206,  0x0209,  0x020c,
290
 
      0x020f,  0x0212,  0x0216,  0x0219,  0x021c,  0x021f,  0x0222,  0x0225,
291
 
      0x0228,  0x022c,  0x022f,  0x0232,  0x0235,  0x0238,  0x023b,  0x023e,
292
 
      0x0242,  0x0245,  0x0248,  0x024b,  0x024e,  0x0251,  0x0254,  0x0258,
293
 
      0x025b,  0x025e,  0x0261,  0x0264,  0x0267,  0x026a,  0x026e,  0x0271,
294
 
      0x0274,  0x0277,  0x027a,  0x027d,  0x0280,  0x0284,  0x0287,  0x028a,
295
 
      0x028d,  0x0290,  0x0293,  0x0296,  0x029a,  0x029d,  0x02a0,  0x02a3,
296
 
      0x02a6,  0x02a9,  0x02ac,  0x02b0,  0x02b3,  0x02b6,  0x02b9,  0x02bc,
297
 
      0x02bf,  0x02c2,  0x02c6,  0x02c9,  0x02cc,  0x02cf,  0x02d2,  0x02d5,
298
 
      0x02d8,  0x02db,  0x02df,  0x02e2,  0x02e5,  0x02e8,  0x02eb,  0x02ee,
299
 
      0x02f1,  0x02f5,  0x02f8,  0x02fb,  0x02fe,  0x0301,  0x0304,  0x0307,
300
 
      0x030b,  0x030e,  0x0311,  0x0314,  0x0317,  0x031a,  0x031d,  0x0321};
301
 
 
302
 
//////////////////////////////////////////////////////////////////
303
 
 
304
 
static UINT8 dsp1_get_sr( void )
305
 
{
306
 
        dsp1_state.SrLowByteAccess = ~dsp1_state.SrLowByteAccess;
307
 
 
308
 
        if (dsp1_state.SrLowByteAccess)
309
 
                return 0;
310
 
        else
311
 
                return dsp1_state.Sr;
312
 
}
313
 
 
314
 
//////////////////////////////////////////////////////////////////
315
 
 
316
 
static UINT8 dsp1_get_dr( void )
317
 
{
318
 
        UINT8 oDr;
319
 
 
320
 
        dsp1_fsm_step(1, &oDr);
321
 
        return oDr;
322
 
}
323
 
 
324
 
//////////////////////////////////////////////////////////////////
325
 
 
326
 
static void dsp1_set_dr( UINT8 iDr )
327
 
{
328
 
        dsp1_fsm_step(0, &iDr);
329
 
}
330
 
 
331
 
//////////////////////////////////////////////////////////////////
332
 
 
333
 
static void dsp1_init( running_machine *machine )
334
 
{
335
 
        UINT32 i;
336
 
        UINT8 *dspin = machine->region("addons")->base();
337
 
 
338
 
        dsp1_state.Sr = DRC|RQM;
339
 
        dsp1_state.SrLowByteAccess = FALSE;
340
 
        dsp1_state.Dr = 0x0080;         // Only a supposition. Is this correct?
341
 
        dsp1_state.Freeze = FALSE;
342
 
        dsp1_state.FsmMajorState = WAIT_COMMAND;
343
 
        memset(&dsp1_state.shared, 0, sizeof(struct SharedData)); // another supposition
344
 
 
345
 
        // expand the DSP-1 data ROM
346
 
        for (i = 0; i < 2048; i += 2)
347
 
        {
348
 
                dsp1_state.DataRom[i / 2] = (dspin[i] << 8) | dspin[i + 1];
349
 
        }
350
 
 
351
 
        dsp1_register_save(machine);
352
 
}
353
 
 
354
 
//////////////////////////////////////////////////////////////////
355
 
 
356
 
// Though the DSP-1 is unaware of the type of operation (read or write)
357
 
// we need to know what is being done by the program, as the class
358
 
// is responsible for maintaining the binding between the
359
 
// "external" and "internal" representations of the DR (data register).
360
 
 
361
 
static void dsp1_fsm_step( UINT8 read, UINT8 *data )
362
 
{
363
 
        if (0 == (dsp1_state.Sr & RQM))
364
 
                return;
365
 
 
366
 
        // Now RQM would be cleared; however, as this code is not to be used in
367
 
        // a multithread environment, we will simply fake RQM operation.
368
 
        // (The only exception would be Op1A's freeze.)
369
 
 
370
 
        // binding
371
 
        if (read)
372
 
        {
373
 
                if (dsp1_state.Sr & DRS)
374
 
                        *data = (UINT8)(dsp1_state.Dr >> 8);
375
 
                else
376
 
                        *data = (UINT8)(dsp1_state.Dr);
377
 
        }
378
 
        else
379
 
        {
380
 
                if (dsp1_state.Sr & DRS)
381
 
                {
382
 
                        dsp1_state.Dr &= 0x00ff;
383
 
                        dsp1_state.Dr |= *data << 8;
384
 
                }
385
 
                else
386
 
                {
387
 
                        dsp1_state.Dr &= 0xff00;
388
 
                        dsp1_state.Dr |= *data;
389
 
                }
390
 
        }
391
 
 
392
 
 
393
 
        switch (dsp1_state.FsmMajorState)
394
 
        {
395
 
        case WAIT_COMMAND:
396
 
                dsp1_state.Command = (UINT8)(dsp1_state.Dr);
397
 
                if (!(dsp1_state.Command & 0xc0))       // valid command?
398
 
                {
399
 
                        switch(dsp1_state.Command)
400
 
                        {
401
 
                        // freeze cases
402
 
                        case 0x1a:
403
 
                        case 0x2a:
404
 
                        case 0x3a:
405
 
                                dsp1_state.Freeze = TRUE;
406
 
                                break;
407
 
 
408
 
                        // normal cases
409
 
                        default:
410
 
                                dsp1_state.DataCounter = 0;
411
 
                                dsp1_state.FsmMajorState = READ_DATA;
412
 
                                dsp1_state.Sr &= ~DRC;
413
 
                                break;
414
 
                        }
415
 
                }
416
 
                break;
417
 
 
418
 
        case READ_DATA:
419
 
                dsp1_state.Sr ^= DRS;
420
 
                if (!(dsp1_state.Sr & DRS))
421
 
                {
422
 
                        dsp1_state.ReadBuffer[dsp1_state.DataCounter++] = (INT16)(dsp1_state.Dr);
423
 
                        if (dsp1_state.DataCounter >= mCommandTable[dsp1_state.Command].reads)
424
 
                        {
425
 
                                (*mCommandTable[dsp1_state.Command].callback)(dsp1_state.ReadBuffer, dsp1_state.WriteBuffer);
426
 
                                if (0 != mCommandTable[dsp1_state.Command].writes)  // any output?
427
 
                                {
428
 
                                        dsp1_state.DataCounter = 0;
429
 
                                        dsp1_state.Dr = (UINT16)(dsp1_state.WriteBuffer[dsp1_state.DataCounter]);
430
 
                                        dsp1_state.FsmMajorState = WRITE_DATA;
431
 
                                }
432
 
                                else
433
 
                                {
434
 
                                        dsp1_state.Dr = 0x0080;  // valid command completion
435
 
                                        dsp1_state.FsmMajorState = WAIT_COMMAND;
436
 
                                        dsp1_state.Sr |= DRC;
437
 
                                }
438
 
                        }
439
 
                }
440
 
                break;
441
 
 
442
 
        case WRITE_DATA:
443
 
                dsp1_state.Sr ^= DRS;
444
 
                if (!(dsp1_state.Sr & DRS))
445
 
                {
446
 
                        ++dsp1_state.DataCounter;
447
 
                        if (dsp1_state.DataCounter >= mCommandTable[dsp1_state.Command].writes)
448
 
                        {
449
 
                                if ((dsp1_state.Command == 0x0a) && (dsp1_state.Dr != 0x8000))
450
 
                                {
451
 
                                        // works in continuous mode
452
 
                                        dsp1_state.ReadBuffer[0]++;     // next raster line
453
 
                                        (*mCommandTable[dsp1_state.Command].callback)(dsp1_state.ReadBuffer, dsp1_state.WriteBuffer);
454
 
                                        dsp1_state.DataCounter = 0;
455
 
                                        dsp1_state.Dr = (UINT16)(dsp1_state.WriteBuffer[dsp1_state.DataCounter]);
456
 
                                }
457
 
                                else
458
 
                                {
459
 
                                        dsp1_state.Dr = 0x0080;  // valid command completion
460
 
                                        dsp1_state.FsmMajorState = WAIT_COMMAND;
461
 
                                        dsp1_state.Sr |= DRC;
462
 
                                }
463
 
                        }
464
 
                        else
465
 
                        {
466
 
                                dsp1_state.Dr = (UINT16)(dsp1_state.WriteBuffer[dsp1_state.DataCounter]);
467
 
                        }
468
 
                }
469
 
                break;
470
 
        }
471
 
 
472
 
 
473
 
 
474
 
        // Now RQM would be set (except when executing Op1A -command equals 0x1a, 0x2a or 0x3a-).
475
 
        if (dsp1_state.Freeze)
476
 
                dsp1_state.Sr &= ~RQM;
477
 
}
478
 
 
479
 
//////////////////////////////////////////////////////////////////
480
 
 
481
 
static void dsp1_memory_test( INT16 *input, INT16 *output )
482
 
{
483
 
//  INT16 *Size = &input[0];
484
 
        INT16 *Result = &output[0];
485
 
 
486
 
        *Result = 0x0000;
487
 
}
488
 
 
489
 
//////////////////////////////////////////////////////////////////
490
 
 
491
 
static void dsp1_memory_dump( INT16 *input, INT16 *output )
492
 
{
493
 
        memcpy(output, dsp1_state.DataRom, 1024);
494
 
}
495
 
 
496
 
//////////////////////////////////////////////////////////////////
497
 
 
498
 
static void dsp1_memory_size( INT16 *input, INT16 *output )
499
 
{
500
 
        INT16* Size = &output[0];
501
 
 
502
 
        *Size = 0x0100;
503
 
}
504
 
 
505
 
//////////////////////////////////////////////////////////////////
506
 
 
507
 
// 16-bit multiplication
508
 
 
509
 
static void dsp1_multiply( INT16 *input, INT16 *output )
510
 
{
511
 
        INT16 Multiplicand = input[0];
512
 
        INT16 Multiplier = input[1];
513
 
        INT16* Product = &output[0];
514
 
 
515
 
        *Product = Multiplicand * Multiplier >> 15;
516
 
}
517
 
 
518
 
//////////////////////////////////////////////////////////////////
519
 
 
520
 
// 16-bit multiplication. 'Alternative' method. Can anyone check this carefully?
521
 
 
522
 
static void dsp1_multiply2( INT16 *input, INT16 *output )
523
 
{
524
 
        INT16 Multiplicand = input[0];
525
 
        INT16 Multiplier = input[1];
526
 
        INT16* Product = &output[0];
527
 
 
528
 
        *Product = (Multiplicand * Multiplier >> 15) + 1;
529
 
}
530
 
 
531
 
//////////////////////////////////////////////////////////////////
532
 
 
533
 
// This command determines the inverse of a floating point decimal number.
534
 
 
535
 
static void dsp1_inverse( INT16 *input, INT16 *output )
536
 
{
537
 
        INT16 Coefficient = input[0];
538
 
        INT16 Exponent = input[1];
539
 
        INT16* iCoefficient = &output[0];
540
 
        INT16* iExponent = &output[1];
541
 
 
542
 
        inverse(Coefficient, Exponent, iCoefficient, iExponent);
543
 
}
544
 
 
545
 
//////////////////////////////////////////////////////////////////
546
 
 
547
 
// Vector component calculation. Determines the X and Y components for a
548
 
// two-dimensional vector whose size and direction is known.
549
 
// Y = Radius * sin(Angle)
550
 
// X = Radius * cos(Angle)
551
 
 
552
 
static void dsp1_triangle( INT16 *input, INT16 *output )
553
 
{
554
 
        INT16 Angle = input[0];
555
 
        INT16 Radius = input[1];
556
 
        INT16* Y = &output[0];
557
 
        INT16* X = &output[1];
558
 
 
559
 
        *Y = dsp1_sin(Angle) * Radius >> 15;
560
 
        *X = dsp1_cos(Angle) * Radius >> 15;
561
 
}
562
 
 
563
 
//////////////////////////////////////////////////////////////////
564
 
 
565
 
// Determines the squared norm of a vector (X,Y,Z)
566
 
// The output is Radius = X^2+Y^2+Z^2 (double integer)
567
 
 
568
 
static void dsp1_radius( INT16 *input, INT16 *output )
569
 
{
570
 
        INT16 X = input[0];
571
 
        INT16 Y = input[1];
572
 
        INT16 Z = input[2];
573
 
        INT16* RadiusLow = &output[0];
574
 
        INT16* RadiusHigh = &output[1];
575
 
 
576
 
        INT32 Radius;
577
 
 
578
 
        Radius = (X * X + Y * Y + Z * Z) << 1;
579
 
        *RadiusLow = (INT16)(Radius);
580
 
        *RadiusHigh = (INT16)(Radius >> 16);
581
 
}
582
 
 
583
 
//////////////////////////////////////////////////////////////////
584
 
 
585
 
// Vector size comparison. This command compares the size of the vector (X,Y,Z) and the distance (R)
586
 
// from a particular point, and so may be used to determine if a point is within the sphere or radius R.
587
 
// The output is D = X^2+Y^2+Z^2-R^2
588
 
 
589
 
static void dsp1_range( INT16 *input, INT16 *output )
590
 
{
591
 
        INT16 X = input[0];
592
 
        INT16 Y = input[1];
593
 
        INT16 Z = input[2];
594
 
        INT16 Radius = input[3];
595
 
        INT16* Range = &output[0];
596
 
 
597
 
        *Range = (X * X + Y * Y + Z * Z - Radius * Radius) >> 15;
598
 
}
599
 
 
600
 
//////////////////////////////////////////////////////////////////
601
 
 
602
 
// Vector size comparison. 'Alternative' method.
603
 
 
604
 
static void dsp1_range2( INT16 *input, INT16 *output )
605
 
{
606
 
        INT16 X = input[0];
607
 
        INT16 Y = input[1];
608
 
        INT16 Z = input[2];
609
 
        INT16 Radius = input[3];
610
 
        INT16* Range = &output[0];
611
 
 
612
 
        *Range = ((X * X + Y * Y + Z * Z - Radius * Radius) >> 15) + 1;
613
 
}
614
 
 
615
 
//////////////////////////////////////////////////////////////////
616
 
 
617
 
// This command calculates the norm of a (X,Y,Z) vector, or the distance from
618
 
// the point (X,Y,Z) to (0,0,0), as you prefer to see it.
619
 
// Distance = sqrt(X^2+Y^2+Z^2)
620
 
// The square root of a number 'a' is calculated by doing this: you
621
 
// write 'a' as b*2^2n, with 'b' between 1/4 and 1; then, you calculate
622
 
// c=sqrt(b) by using lineal interpolation between points of a
623
 
// look-up table and, finally, you output the result as c*2^n.
624
 
 
625
 
static void dsp1_distance( INT16 *input, INT16 *output )
626
 
{
627
 
        INT16 X = input[0];
628
 
        INT16 Y = input[1];
629
 
        INT16 Z = input[2];
630
 
        INT16* Distance = &output[0];
631
 
        INT16 Pos, Node1, Node2;
632
 
 
633
 
        INT32 Radius = X * X + Y * Y + Z * Z;
634
 
 
635
 
 
636
 
        if (Radius == 0)
637
 
                Distance = 0;
638
 
        else
639
 
        {
640
 
                INT16 C, E;
641
 
                normalize_double(Radius, &C, &E);
642
 
                if (E & 1)
643
 
                C = C * 0x4000 >> 15;
644
 
 
645
 
                Pos = C * 0x0040 >> 15;
646
 
 
647
 
                Node1 = dsp1_state.DataRom[0x00d5 + Pos];
648
 
                Node2 = dsp1_state.DataRom[0x00d6 + Pos];
649
 
 
650
 
                *Distance = ((Node2 - Node1) * (C & 0x1ff) >> 9) + Node1;
651
 
 
652
 
#if dsp1_VERSION < 0x0102
653
 
                if (Pos & 1)
654
 
                        *Distance -= (Node2 - Node1);
655
 
#endif
656
 
                *Distance >>= (E >> 1);
657
 
        }
658
 
}
659
 
 
660
 
//////////////////////////////////////////////////////////////////
661
 
 
662
 
// Determines the (X2, Y2) coordinates obtained by rotating (X1, Y1)
663
 
// clockwise for an angle 'Angle'. The official documentation says
664
 
// 'counterclockwise', but it's obviously wrong (surprise! :P)
665
 
//
666
 
// In matrix notation:
667
 
// |X2|    |cos(Angle)    sin(Angle)| |X1|
668
 
// |  | =  |                        | |  |
669
 
// |Y2|    |-sin(Angle    cos(Angle)| |Y1|
670
 
 
671
 
static void dsp1_rotate( INT16 *input, INT16 *output )
672
 
{
673
 
        INT16 Angle = input[0];
674
 
        INT16 X1 = input[1];
675
 
        INT16 Y1 = input[2];
676
 
        INT16* X2 = &output[0];
677
 
        INT16* Y2 = &output[1];
678
 
 
679
 
        *X2 = (Y1 * dsp1_sin(Angle) >> 15) + (X1 * dsp1_cos(Angle) >> 15);
680
 
        *Y2 = (Y1 * dsp1_cos(Angle) >> 15) - (X1 * dsp1_sin(Angle) >> 15);
681
 
}
682
 
 
683
 
//////////////////////////////////////////////////////////////////
684
 
 
685
 
// Calculate the coordinates (X2, Y2, Z2) obtained when rotating (X1, Y1, Z1)
686
 
// three-dimensionally. Rotation is done in the order of Az around the Z axis,
687
 
// Ay around the Y axis and Ax around the X axis. As occur with the "attitude" commands
688
 
// (see comments in the "gyrate" command), qthis doesn't match what explained in
689
 
// the official documentation, but it's coherent with what it is done in the "attitude"
690
 
// command (but not with the "gyrate" command).
691
 
//
692
 
// In matrix notation:
693
 
// |X2|   |1      0       0  | |cosRy   0   -sinRy| | cosRz  sinRz   0| |X1|
694
 
// |Y2| = |0     cosRx  sinRx| |  0     1      0  | |-sinRz  cosRz   0| |Y1|
695
 
// |Z2|   |0    -sinRx  cosRx| |sinRy   0    cosRy| |   0       0    1| |Z1|
696
 
 
697
 
static void dsp1_polar( INT16 *input, INT16 *output )
698
 
{
699
 
        INT16 Az = input[0];
700
 
        INT16 Ay = input[1];
701
 
        INT16 Ax = input[2];
702
 
        INT16 X1 = input[3];
703
 
        INT16 Y1 = input[4];
704
 
        INT16 Z1 = input[5];
705
 
        INT16* X2 = &output[0];
706
 
        INT16* Y2 = &output[1];
707
 
        INT16* Z2 = &output[2];
708
 
 
709
 
        INT16 X, Y, Z;
710
 
 
711
 
        // Rotate Around Z
712
 
        X = (Y1 * dsp1_sin(Az) >> 15) + (X1 * dsp1_cos(Az) >> 15);
713
 
        Y = (Y1 * dsp1_cos(Az) >> 15) - (X1 * dsp1_sin(Az) >> 15);
714
 
        X1 = X; Y1 = Y;
715
 
 
716
 
        // Rotate Around Y
717
 
        Z = (X1 * dsp1_sin(Ay) >> 15) + (Z1 * dsp1_cos(Ay) >> 15);
718
 
        X = (X1 * dsp1_cos(Ay) >> 15) - (Z1 * dsp1_sin(Ay) >> 15);
719
 
        *X2 = X; Z1 = Z;
720
 
 
721
 
        // Rotate Around X
722
 
        Y = (Z1 * dsp1_sin(Ax) >> 15) + (Y1 * dsp1_cos(Ax) >> 15);
723
 
        Z = (Z1 * dsp1_cos(Ax) >> 15) - (Y1 * dsp1_sin(Ax) >> 15);
724
 
        *Y2 = Y; *Z2 = Z;
725
 
}
726
 
 
727
 
//////////////////////////////////////////////////////////////////
728
 
 
729
 
// Set up the elements of an "attitude matrix" (there are other ones):
730
 
//           S | cosRz  sinRz   0| |cosRy  0   -sinRy| |1     0      0  |
731
 
// MatrixA = - |-sinRz  cosRz   0| |  0    1      0  | |0  cosRx   sinRx|
732
 
//           2 |   0       0    1| |sinRy  0    cosRy| |0  -sinRx  cosRx|
733
 
// This matrix is thought to be used within the following framework:
734
 
// let's suppose we define positive rotations around a system of orthogonal axes in this manner:
735
 
// a rotation of +90 degrees around axis3 converts axis2 into axis1
736
 
// a rotation of +90 degrees around axis2 converts axis1 into axis3
737
 
// a rotation of +90 degrees around axis1 converts axis3 into axis2
738
 
// and let's suppose that we have defined a new orthonormal axes system (FLU)
739
 
// by doing the following operations about the standard one (XYZ):
740
 
// first rotating the XYZ system around Z by an angle Rz (obtaining X'Y'Z'),
741
 
// then rotating the resulting system around Y by an angle Ry (obtaining X''Y''Z'')
742
 
// and, finally, rotating the resulting system around X by an angle Rx (obtaining FLU)
743
 
// This FLU (forward/left/up) system represents an "attitude" and, then, the matrix here defined
744
 
// is the change of coordinates matrix that transform coordinates in the FLU
745
 
// system (the "object coordinates") into the standard XYZ system (the "global coordinates"),
746
 
// multiplied by a scale factor S/2, that is:
747
 
// |x|   S             |f|
748
 
// |y| * - = MatrixA * |l|
749
 
// |z|   2             |u|
750
 
// In a similar way, if we use the transpose of the matrix, we can transform global coordinates
751
 
// into object coordinates:
752
 
// |f|   S                        |x|
753
 
// |l| * - = MatrixA_transposed * |y|
754
 
// |u|   2                        |z|
755
 
//
756
 
// input[0]: S
757
 
// input[1]: Rz
758
 
// input[2]: Ry
759
 
// input[3]: Rx
760
 
 
761
 
static void dsp1_attitudeA( INT16 *input, INT16 *output )
762
 
{
763
 
        INT16 S = input[0];
764
 
        INT16 Rz = input[1];
765
 
        INT16 Ry = input[2];
766
 
        INT16 Rx = input[3];
767
 
 
768
 
        INT16 SinRz = dsp1_sin(Rz);
769
 
        INT16 CosRz = dsp1_cos(Rz);
770
 
        INT16 SinRy = dsp1_sin(Ry);
771
 
        INT16 CosRy = dsp1_cos(Ry);
772
 
        INT16 SinRx = dsp1_sin(Rx);
773
 
        INT16 CosRx = dsp1_cos(Rx);
774
 
 
775
 
        S >>= 1;
776
 
 
777
 
        dsp1_state.shared.MatrixA[0][0] = (S * CosRz >> 15) * CosRy >> 15;
778
 
        dsp1_state.shared.MatrixA[0][1] = ((S * SinRz >> 15) * CosRx >> 15) + (((S * CosRz >> 15) * SinRx >> 15) * SinRy >> 15);
779
 
        dsp1_state.shared.MatrixA[0][2] = ((S * SinRz >> 15) * SinRx >> 15) - (((S * CosRz >> 15) * CosRx >> 15) * SinRy >> 15);
780
 
 
781
 
        dsp1_state.shared.MatrixA[1][0] = -((S * SinRz >> 15) * CosRy >> 15);
782
 
        dsp1_state.shared.MatrixA[1][1] = ((S * CosRz >> 15) * CosRx >> 15) - (((S * SinRz >> 15) * SinRx >> 15) * SinRy >> 15);
783
 
        dsp1_state.shared.MatrixA[1][2] = ((S * CosRz >> 15) * SinRx >> 15) + (((S * SinRz >> 15) * CosRx >> 15) * SinRy >> 15);
784
 
 
785
 
        dsp1_state.shared.MatrixA[2][0] = S * SinRy >> 15;
786
 
        dsp1_state.shared.MatrixA[2][1] = -((S * SinRx >> 15) * CosRy >> 15);
787
 
        dsp1_state.shared.MatrixA[2][2] = (S * CosRx >> 15) * CosRy >> 15;
788
 
}
789
 
 
790
 
//////////////////////////////////////////////////////////////////
791
 
 
792
 
// Same than 'attitudeA', but with a difference attitude matrix (matrixB)
793
 
 
794
 
static void dsp1_attitudeB( INT16 *input, INT16 *output )
795
 
{
796
 
        INT16 S = input[0];
797
 
        INT16 Rz = input[1];
798
 
        INT16 Ry = input[2];
799
 
        INT16 Rx = input[3];
800
 
 
801
 
        INT16 SinRz = dsp1_sin(Rz);
802
 
        INT16 CosRz = dsp1_cos(Rz);
803
 
        INT16 SinRy = dsp1_sin(Ry);
804
 
        INT16 CosRy = dsp1_cos(Ry);
805
 
        INT16 SinRx = dsp1_sin(Rx);
806
 
        INT16 CosRx = dsp1_cos(Rx);
807
 
 
808
 
        S >>= 1;
809
 
 
810
 
        dsp1_state.shared.MatrixB[0][0] = (S * CosRz >> 15) * CosRy >> 15;
811
 
        dsp1_state.shared.MatrixB[0][1] = ((S * SinRz >> 15) * CosRx >> 15) + (((S * CosRz >> 15) * SinRx >> 15) * SinRy >> 15);
812
 
        dsp1_state.shared.MatrixB[0][2] = ((S * SinRz >> 15) * SinRx >> 15) - (((S * CosRz >> 15) * CosRx >> 15) * SinRy >> 15);
813
 
 
814
 
        dsp1_state.shared.MatrixB[1][0] = -((S * SinRz >> 15) * CosRy >> 15);
815
 
        dsp1_state.shared.MatrixB[1][1] = ((S * CosRz >> 15) * CosRx >> 15) - (((S * SinRz >> 15) * SinRx >> 15) * SinRy >> 15);
816
 
        dsp1_state.shared.MatrixB[1][2] = ((S * CosRz >> 15) * SinRx >> 15) + (((S * SinRz >> 15) * CosRx >> 15) * SinRy >> 15);
817
 
 
818
 
        dsp1_state.shared.MatrixB[2][0] = S * SinRy >> 15;
819
 
        dsp1_state.shared.MatrixB[2][1] = -((S * SinRx >> 15) * CosRy >> 15);
820
 
        dsp1_state.shared.MatrixB[2][2] = (S * CosRx >> 15) * CosRy >> 15;
821
 
}
822
 
 
823
 
//////////////////////////////////////////////////////////////////
824
 
 
825
 
// Same than 'attitudeA', but with a difference attitude matrix (matrixC)
826
 
 
827
 
static void dsp1_attitudeC( INT16 *input, INT16 *output )
828
 
{
829
 
        INT16 S = input[0];
830
 
        INT16 Rz = input[1];
831
 
        INT16 Ry = input[2];
832
 
        INT16 Rx = input[3];
833
 
 
834
 
        INT16 SinRz = dsp1_sin(Rz);
835
 
        INT16 CosRz = dsp1_cos(Rz);
836
 
        INT16 SinRy = dsp1_sin(Ry);
837
 
        INT16 CosRy = dsp1_cos(Ry);
838
 
        INT16 SinRx = dsp1_sin(Rx);
839
 
        INT16 CosRx = dsp1_cos(Rx);
840
 
 
841
 
        S >>= 1;
842
 
 
843
 
        dsp1_state.shared.MatrixC[0][0] = (S * CosRz >> 15) * CosRy >> 15;
844
 
        dsp1_state.shared.MatrixC[0][1] = ((S * SinRz >> 15) * CosRx >> 15) + (((S * CosRz >> 15) * SinRx >> 15) * SinRy >> 15);
845
 
        dsp1_state.shared.MatrixC[0][2] = ((S * SinRz >> 15) * SinRx >> 15) - (((S * CosRz >> 15) * CosRx >> 15) * SinRy >> 15);
846
 
 
847
 
        dsp1_state.shared.MatrixC[1][0] = -((S * SinRz >> 15) * CosRy >> 15);
848
 
        dsp1_state.shared.MatrixC[1][1] = ((S * CosRz >> 15) * CosRx >> 15) - (((S * SinRz >> 15) * SinRx >> 15) * SinRy >> 15);
849
 
        dsp1_state.shared.MatrixC[1][2] = ((S * CosRz >> 15) * SinRx >> 15) + (((S * SinRz >> 15) * CosRx >> 15) * SinRy >> 15);
850
 
 
851
 
        dsp1_state.shared.MatrixC[2][0] = S * SinRy >> 15;
852
 
        dsp1_state.shared.MatrixC[2][1] = -((S * SinRx >> 15) * CosRy >> 15);
853
 
        dsp1_state.shared.MatrixC[2][2] = (S * CosRx >> 15) * CosRy >> 15;
854
 
}
855
 
 
856
 
//////////////////////////////////////////////////////////////////
857
 
 
858
 
// Convert global coordinates (X,Y,Z) to object coordinates (F,L,U)
859
 
// See the comment in "attitudeA" for a explanation about the calculation.
860
 
//
861
 
// input[0]: X ; input[1]: Y ; input[2]: Z
862
 
// &output[0]: F ; &output[1]: L ; &output[2]: U
863
 
 
864
 
static void dsp1_objectiveA( INT16 *input, INT16 *output )
865
 
{
866
 
        INT16 X = input[0];
867
 
        INT16 Y = input[1];
868
 
        INT16 Z = input[2];
869
 
        INT16* F = &output[0];
870
 
        INT16* L = &output[1];
871
 
        INT16* U = &output[2];
872
 
 
873
 
        *F = (dsp1_state.shared.MatrixA[0][0] * X >> 15) + (dsp1_state.shared.MatrixA[1][0] * Y >> 15) + (dsp1_state.shared.MatrixA[2][0] * Z >> 15);
874
 
        *L = (dsp1_state.shared.MatrixA[0][1] * X >> 15) + (dsp1_state.shared.MatrixA[1][1] * Y >> 15) + (dsp1_state.shared.MatrixA[2][1] * Z >> 15);
875
 
        *U = (dsp1_state.shared.MatrixA[0][2] * X >> 15) + (dsp1_state.shared.MatrixA[1][2] * Y >> 15) + (dsp1_state.shared.MatrixA[2][2] * Z >> 15);
876
 
}
877
 
 
878
 
//////////////////////////////////////////////////////////////////
879
 
 
880
 
// Same than 'objectiveA', but for the 'B' attitude
881
 
 
882
 
static void dsp1_objectiveB( INT16 *input, INT16 *output )
883
 
{
884
 
        INT16 X = input[0];
885
 
        INT16 Y = input[1];
886
 
        INT16 Z = input[2];
887
 
        INT16* F = &output[0];
888
 
        INT16* L = &output[1];
889
 
        INT16* U = &output[2];
890
 
 
891
 
        *F = (dsp1_state.shared.MatrixB[0][0] * X >> 15) + (dsp1_state.shared.MatrixB[1][0] * Y >> 15) + (dsp1_state.shared.MatrixB[2][0] * Z >> 15);
892
 
        *L = (dsp1_state.shared.MatrixB[0][1] * X >> 15) + (dsp1_state.shared.MatrixB[1][1] * Y >> 15) + (dsp1_state.shared.MatrixB[2][1] * Z >> 15);
893
 
        *U = (dsp1_state.shared.MatrixB[0][2] * X >> 15) + (dsp1_state.shared.MatrixB[1][2] * Y >> 15) + (dsp1_state.shared.MatrixB[2][2] * Z >> 15);
894
 
}
895
 
 
896
 
//////////////////////////////////////////////////////////////////
897
 
 
898
 
// Same than 'objectiveA', but for the 'C' attitude
899
 
 
900
 
static void dsp1_objectiveC( INT16 *input, INT16 *output )
901
 
{
902
 
        INT16 X = input[0];
903
 
        INT16 Y = input[1];
904
 
        INT16 Z = input[2];
905
 
        INT16* F = &output[0];
906
 
        INT16* L = &output[1];
907
 
        INT16* U = &output[2];
908
 
 
909
 
        *F = (dsp1_state.shared.MatrixC[0][0] * X >> 15) + (dsp1_state.shared.MatrixC[1][0] * Y >> 15) + (dsp1_state.shared.MatrixC[2][0] * Z >> 15);
910
 
        *L = (dsp1_state.shared.MatrixC[0][1] * X >> 15) + (dsp1_state.shared.MatrixC[1][1] * Y >> 15) + (dsp1_state.shared.MatrixC[2][1] * Z >> 15);
911
 
        *U = (dsp1_state.shared.MatrixC[0][2] * X >> 15) + (dsp1_state.shared.MatrixC[1][2] * Y >> 15) + (dsp1_state.shared.MatrixC[2][2] * Z >> 15);
912
 
}
913
 
 
914
 
//////////////////////////////////////////////////////////////////
915
 
 
916
 
// Convert object coordinates (F,L,U) to object coordinates (X,Y,Z)
917
 
// See the comment in "attitudeA" for a explanation about the calculation.
918
 
//
919
 
// input[0]: F ; input[1]: L ; input[2]: U
920
 
// &output[0]: X ; &output[1]: Y ; &output[2]: Z
921
 
 
922
 
static void dsp1_subjectiveA( INT16 *input, INT16 *output )
923
 
{
924
 
        INT16 F = input[0];
925
 
        INT16 L = input[1];
926
 
        INT16 U = input[2];
927
 
        INT16* X = &output[0];
928
 
        INT16* Y = &output[1];
929
 
        INT16* Z = &output[2];
930
 
 
931
 
        *X = (dsp1_state.shared.MatrixA[0][0] * F >> 15) + (dsp1_state.shared.MatrixA[0][1] * L >> 15) + (dsp1_state.shared.MatrixA[0][2] * U >> 15);
932
 
        *Y = (dsp1_state.shared.MatrixA[1][0] * F >> 15) + (dsp1_state.shared.MatrixA[1][1] * L >> 15) + (dsp1_state.shared.MatrixA[1][2] * U >> 15);
933
 
        *Z = (dsp1_state.shared.MatrixA[2][0] * F >> 15) + (dsp1_state.shared.MatrixA[2][1] * L >> 15) + (dsp1_state.shared.MatrixA[2][2] * U >> 15);
934
 
}
935
 
 
936
 
//////////////////////////////////////////////////////////////////
937
 
 
938
 
// Same than 'subjectiveA', but for the 'B' attitude
939
 
 
940
 
static void dsp1_subjectiveB( INT16 *input, INT16 *output )
941
 
{
942
 
        INT16 F = input[0];
943
 
        INT16 L = input[1];
944
 
        INT16 U = input[2];
945
 
        INT16* X = &output[0];
946
 
        INT16* Y = &output[1];
947
 
        INT16* Z = &output[2];
948
 
 
949
 
        *X = (dsp1_state.shared.MatrixB[0][0] * F >> 15) + (dsp1_state.shared.MatrixB[0][1] * L >> 15) + (dsp1_state.shared.MatrixB[0][2] * U >> 15);
950
 
        *Y = (dsp1_state.shared.MatrixB[1][0] * F >> 15) + (dsp1_state.shared.MatrixB[1][1] * L >> 15) + (dsp1_state.shared.MatrixB[1][2] * U >> 15);
951
 
        *Z = (dsp1_state.shared.MatrixB[2][0] * F >> 15) + (dsp1_state.shared.MatrixB[2][1] * L >> 15) + (dsp1_state.shared.MatrixB[2][2] * U >> 15);
952
 
}
953
 
 
954
 
//////////////////////////////////////////////////////////////////
955
 
 
956
 
// Same than 'subjectiveA', but for the 'C' attitude
957
 
 
958
 
static void dsp1_subjectiveC(INT16 *input, INT16 *output)
959
 
{
960
 
        INT16 F = input[0];
961
 
        INT16 L = input[1];
962
 
        INT16 U = input[2];
963
 
        INT16* X = &output[0];
964
 
        INT16* Y = &output[1];
965
 
        INT16* Z = &output[2];
966
 
 
967
 
        *X = (dsp1_state.shared.MatrixC[0][0] * F >> 15) + (dsp1_state.shared.MatrixC[0][1] * L >> 15) + (dsp1_state.shared.MatrixC[0][2] * U >> 15);
968
 
        *Y = (dsp1_state.shared.MatrixC[1][0] * F >> 15) + (dsp1_state.shared.MatrixC[1][1] * L >> 15) + (dsp1_state.shared.MatrixC[1][2] * U >> 15);
969
 
        *Z = (dsp1_state.shared.MatrixC[2][0] * F >> 15) + (dsp1_state.shared.MatrixC[2][1] * L >> 15) + (dsp1_state.shared.MatrixC[2][2] * U >> 15);
970
 
}
971
 
 
972
 
//////////////////////////////////////////////////////////////////
973
 
 
974
 
// This command calculates the inner product (S) of a vector (X,Y,Z) and
975
 
// the first column of MatrixA. It should be noted that that first column
976
 
// represent the global coordinates of an unity vector in the forward
977
 
// direction in the object coordinate system (coordinates (1,0,0) in the FLU
978
 
// axes system).
979
 
//
980
 
// input[0]: X ; input[1]: Y ; input[2]: Z
981
 
// &output[0]: S
982
 
 
983
 
static void dsp1_scalarA( INT16 *input, INT16 *output )
984
 
{
985
 
        INT16 X = input[0];
986
 
        INT16 Y = input[1];
987
 
        INT16 Z = input[2];
988
 
        INT16* S = &output[0];
989
 
 
990
 
        *S = (X * dsp1_state.shared.MatrixA[0][0] + Y * dsp1_state.shared.MatrixA[1][0] + Z * dsp1_state.shared.MatrixA[2][0]) >> 15;
991
 
}
992
 
 
993
 
//////////////////////////////////////////////////////////////////
994
 
 
995
 
// Same than 'scalarA', but for the 'B' attitude
996
 
 
997
 
static void dsp1_scalarB( INT16 *input, INT16 *output )
998
 
{
999
 
        INT16 X = input[0];
1000
 
        INT16 Y = input[1];
1001
 
        INT16 Z = input[2];
1002
 
        INT16* S = &output[0];
1003
 
 
1004
 
        *S = (X * dsp1_state.shared.MatrixB[0][0] + Y * dsp1_state.shared.MatrixB[1][0] + Z * dsp1_state.shared.MatrixB[2][0]) >> 15;
1005
 
}
1006
 
 
1007
 
//////////////////////////////////////////////////////////////////
1008
 
 
1009
 
// Same than 'scalarA', but for the 'C' attitude
1010
 
 
1011
 
static void dsp1_scalarC( INT16 *input, INT16 *output )
1012
 
{
1013
 
        INT16 X = input[0];
1014
 
        INT16 Y = input[1];
1015
 
        INT16 Z = input[2];
1016
 
        INT16* S = &output[0];
1017
 
 
1018
 
        *S = (X * dsp1_state.shared.MatrixC[0][0] + Y * dsp1_state.shared.MatrixC[1][0] + Z * dsp1_state.shared.MatrixC[2][0]) >> 15;
1019
 
}
1020
 
 
1021
 
//////////////////////////////////////////////////////////////////
1022
 
 
1023
 
// This command determines the final attitude angles after the body with attitude angles (Ax, Ay, Az) with
1024
 
// respect to the global coordinates is rotated by the minor angular displacements (DeltaF, DeltaL, DeltaU).
1025
 
// It means that the XYZ axes are rotated by (Ax, Ay, Az) to obtain the FLU axes and, then, these
1026
 
// are rotated by (DeltaF, DeltaL, DeltaU). The command calculates and return the new FLU angles respect to the
1027
 
// XYZ system (Rx, Ry, Rz)
1028
 
// The formulae are:
1029
 
// Rx = Ax + (DeltaU*sin(Ay)+DeltaF*cos(Ay))
1030
 
// Ry = Ay + DeltaL - tan(Ax)*(DeltaU*cos(Ay)+DeltaF*sin(Ay))
1031
 
// Rz = Az + sec(Ax)*(DeltaU*cos(Ay)-DeltaF*sin(Ay))
1032
 
//
1033
 
// Now the discussion: according to the official documentation, as described in various commands, you pass from
1034
 
// XYZ to FLU by doing the rotations in the order Y, X, Z. In this command, the formulae are coherent with the
1035
 
// fact that Y is the first axis to do a rotation around it. However, in the "attitude" command, while the official
1036
 
// document describe it that way, we have discovered, when reverse engineering the command, that the calculated
1037
 
// matrix do the rotation around Y in the second place. This incoherent behaviour of various commands is, in my
1038
 
// opinion, a pretty severe implementation error. However, if you only use small "minor displacements", the error term
1039
 
// introduced by that incoherence should be almost negligible.
1040
 
 
1041
 
static void dsp1_gyrate( INT16 *input, INT16 *output )
1042
 
{
1043
 
        INT16 Az = input[0];
1044
 
        INT16 Ax = input[1];
1045
 
        INT16 Ay = input[2];
1046
 
        INT16 U = input[3];
1047
 
        INT16 F = input[4];
1048
 
        INT16 L = input[5];
1049
 
        INT16* Rz = &output[0];
1050
 
        INT16* Rx = &output[1];
1051
 
        INT16* Ry = &output[2];
1052
 
 
1053
 
        INT16 CSec, ESec, CSin, C, E;
1054
 
        INT16 SinAy = dsp1_sin(Ay);
1055
 
        INT16 CosAy = dsp1_cos(Ay);
1056
 
 
1057
 
        inverse(dsp1_cos(Ax), 0, &CSec, &ESec);
1058
 
 
1059
 
        // Rotation Around Z
1060
 
        normalize_double(U * CosAy - F * SinAy, &C, &E);
1061
 
 
1062
 
        E = ESec - E;
1063
 
 
1064
 
        normalize(C * CSec >> 15, &C, &E);
1065
 
 
1066
 
        *Rz = Az + denormalize_and_clip(C, E);
1067
 
 
1068
 
        // Rotation Around X
1069
 
        *Rx = Ax + (U * SinAy >> 15) + (F * CosAy >> 15);
1070
 
 
1071
 
        // Rotation Around Y
1072
 
        normalize_double(U * CosAy + F * SinAy, &C, &E);
1073
 
 
1074
 
        E = ESec - E;
1075
 
 
1076
 
        normalize(dsp1_sin(Ax), &CSin, &E);
1077
 
 
1078
 
        normalize(-(C * (CSec * CSin >> 15) >> 15), &C, &E);
1079
 
 
1080
 
        *Ry = Ay + denormalize_and_clip(C, E) + L;
1081
 
}
1082
 
 
1083
 
//////////////////////////////////////////////////////////////////
1084
 
 
1085
 
 
1086
 
// Set-up the projection framework. Besides returning some values, it store in RAM some values that
1087
 
// will be used by the other three projection commands (raster, target an project)
1088
 
// Input:
1089
 
// (Fx, Fy, Fz)-> coordinates of base point (global coordinates)
1090
 
// Lfe-> distance between the base point and the viewpoint (center of projection)
1091
 
// Les-> distance between the base point and the screen
1092
 
// Aas-> azimuth angle (0 degrees is east; 90 degrees is north)
1093
 
// Azs-> zenith angle (0 degrees is zenith)
1094
 
// Output:
1095
 
// Vof-> raster line of imaginary center (whatever it means ;) )
1096
 
// Vva-> raster line representing the horizon line
1097
 
// (Cx, Cy)-> coordinates of the projection of the center of the screen over the ground (ground coordinates)
1098
 
 
1099
 
static void dsp1_parameter( INT16 *input, INT16 *output )
1100
 
{
1101
 
        INT16 Fx = input[0];
1102
 
        INT16 Fy = input[1];
1103
 
        INT16 Fz = input[2];
1104
 
        INT16 Lfe = input[3];
1105
 
        INT16 Les = input[4];
1106
 
        INT16 Aas = input[5];
1107
 
        INT16 Azs = input[6];
1108
 
        INT16* Vof = &output[0];
1109
 
        INT16* Vva = &output[1];
1110
 
        INT16* Cx = &output[2];
1111
 
        INT16* Cy = &output[3];
1112
 
 
1113
 
        INT16 CSec, C, E;
1114
 
        INT16 LfeNx, LfeNy, LfeNz;
1115
 
        INT16 LesNx, LesNy, LesNz;
1116
 
        INT16 AZS, MaxAZS;
1117
 
 
1118
 
        // Copy Zenith angle for clipping
1119
 
        AZS = Azs;
1120
 
 
1121
 
        // Store Les and his coefficient and exponent when normalized
1122
 
        dsp1_state.shared.Les = Les;
1123
 
        dsp1_state.shared.E_Les = 0;
1124
 
        normalize(Les, &dsp1_state.shared.C_Les, &dsp1_state.shared.E_Les);
1125
 
 
1126
 
        // Store Sine and Cosine of Azimuth and Zenith angle
1127
 
        dsp1_state.shared.SinAas = dsp1_sin(Aas);
1128
 
        dsp1_state.shared.CosAas = dsp1_cos(Aas);
1129
 
        dsp1_state.shared.SinAzs = dsp1_sin(Azs);
1130
 
        dsp1_state.shared.CosAzs = dsp1_cos(Azs);
1131
 
 
1132
 
        // normal vector to the screen (norm 1, points toward the center of projection)
1133
 
        dsp1_state.shared.Nx = dsp1_state.shared.SinAzs * -dsp1_state.shared.SinAas >> 15;
1134
 
        dsp1_state.shared.Ny = dsp1_state.shared.SinAzs * dsp1_state.shared.CosAas >> 15;
1135
 
        dsp1_state.shared.Nz = dsp1_state.shared.CosAzs * 0x7fff >> 15;
1136
 
 
1137
 
        // horizontal vector of the screen (Hz=0, norm 1, points toward the right of the screen)
1138
 
        dsp1_state.shared.Hx = dsp1_state.shared.CosAas*0x7fff>>15;
1139
 
        dsp1_state.shared.Hy = dsp1_state.shared.SinAas*0x7fff>>15;
1140
 
 
1141
 
        // vertical vector of the screen (norm 1, points toward the top of the screen)
1142
 
        dsp1_state.shared.Vx = dsp1_state.shared.CosAzs * -dsp1_state.shared.SinAas >> 15;
1143
 
        dsp1_state.shared.Vy = dsp1_state.shared.CosAzs * dsp1_state.shared.CosAas >> 15;
1144
 
        dsp1_state.shared.Vz = -dsp1_state.shared.SinAzs * 0x7fff>>15;
1145
 
 
1146
 
        LfeNx = Lfe * dsp1_state.shared.Nx >> 15;
1147
 
        LfeNy = Lfe * dsp1_state.shared.Ny >> 15;
1148
 
        LfeNz = Lfe * dsp1_state.shared.Nz >> 15;
1149
 
 
1150
 
        // Center of Projection
1151
 
        dsp1_state.shared.CentreX = Fx + LfeNx;
1152
 
        dsp1_state.shared.CentreY = Fy + LfeNy;
1153
 
        dsp1_state.shared.CentreZ = Fz + LfeNz;
1154
 
 
1155
 
        LesNx = Les * dsp1_state.shared.Nx >> 15;
1156
 
        LesNy = Les * dsp1_state.shared.Ny >> 15;
1157
 
        LesNz = Les * dsp1_state.shared.Nz >> 15;
1158
 
 
1159
 
        // center of the screen (global coordinates)
1160
 
        dsp1_state.shared.Gx = dsp1_state.shared.CentreX - LesNx;
1161
 
        dsp1_state.shared.Gy = dsp1_state.shared.CentreY - LesNy;
1162
 
        dsp1_state.shared.Gz = dsp1_state.shared.CentreZ - LesNz;
1163
 
 
1164
 
        E = 0;
1165
 
        normalize(dsp1_state.shared.CentreZ, &C, &E);
1166
 
 
1167
 
        dsp1_state.shared.CentreZ_C = C;
1168
 
        dsp1_state.shared.CentreZ_E = E;
1169
 
 
1170
 
        // Determine clip boundary and clip Zenith angle if necessary
1171
 
        // (Why to clip? Maybe to avoid the screen can only show sky with no ground? Only a guess...)
1172
 
        MaxAZS = dsp1_MaxAZS_Exp[-E];
1173
 
 
1174
 
        if (AZS < 0)
1175
 
        {
1176
 
                MaxAZS = -MaxAZS;
1177
 
                if (AZS < MaxAZS + 1)
1178
 
                        AZS = MaxAZS + 1;
1179
 
        }
1180
 
        else
1181
 
        {
1182
 
                if (AZS > MaxAZS)
1183
 
                        AZS = MaxAZS;
1184
 
        }
1185
 
 
1186
 
        // Store Sine and Cosine of clipped Zenith angle
1187
 
        dsp1_state.shared.SinAZS = dsp1_sin(AZS);
1188
 
        dsp1_state.shared.CosAZS = dsp1_cos(AZS);
1189
 
 
1190
 
        // calculate the separation of (cx, cy) from the projection of
1191
 
        // the 'centre of projection' over the ground... (CentreZ*tg(AZS))
1192
 
        inverse(dsp1_state.shared.CosAZS, 0, &dsp1_state.shared.SecAZS_C1, &dsp1_state.shared.SecAZS_E1);
1193
 
        normalize(C * dsp1_state.shared.SecAZS_C1 >> 15, &C, &E);
1194
 
        E += dsp1_state.shared.SecAZS_E1;
1195
 
        C = denormalize_and_clip(C, E) * dsp1_state.shared.SinAZS >> 15;
1196
 
 
1197
 
        // ... and then take into account the position of the centre of
1198
 
        // projection and the azimuth angle
1199
 
        dsp1_state.shared.CentreX += C * dsp1_state.shared.SinAas >> 15;
1200
 
        dsp1_state.shared.CentreY -= C * dsp1_state.shared.CosAas >> 15;
1201
 
 
1202
 
        *Cx = dsp1_state.shared.CentreX;
1203
 
        *Cy = dsp1_state.shared.CentreY;
1204
 
 
1205
 
        // Raster number of imaginary center and horizontal line
1206
 
        *Vof = 0;
1207
 
 
1208
 
        if ((Azs != AZS) || (Azs == MaxAZS))
1209
 
        {
1210
 
                INT16 Aux;
1211
 
 
1212
 
                // correct vof and vva when Azs is outside the 'non-clipping interval'
1213
 
                // we have only some few Taylor coefficients, so we cannot guess which ones
1214
 
                // are the approximated functions and, what is worse, we don't know why
1215
 
                // the own clipping stuff (and, particularly, this correction) is done
1216
 
                if (Azs == -32768)
1217
 
                Azs = -32767;
1218
 
 
1219
 
                C = Azs - MaxAZS;
1220
 
                if (C >= 0)
1221
 
                        C--;
1222
 
                Aux = ~(C << 2);
1223
 
 
1224
 
                // Vof += x+(1/3)*x^3, where x ranges from 0 to PI/4 when Azs-MaxAZS goes from 0 to 0x2000
1225
 
                C = Aux * dsp1_state.DataRom[0x0328] >> 15;
1226
 
                C = (C * Aux >> 15) + dsp1_state.DataRom[0x0327];
1227
 
                *Vof -= (C * Aux >> 15) * Les >> 15;
1228
 
 
1229
 
                // CosAZS *= 1+(1/2)*x^2+(5/24)*x^24, where x ranges from 0 to PI/4 when Azs-MaxAZS goes from 0 to 0x2000
1230
 
                C = Aux * Aux >> 15;
1231
 
                Aux = (C * dsp1_state.DataRom[0x0324] >> 15) + dsp1_state.DataRom[0x0325];
1232
 
                dsp1_state.shared.CosAZS += (C * Aux >> 15) * dsp1_state.shared.CosAZS >> 15;
1233
 
        }
1234
 
 
1235
 
        // vertical offset of the screen with regard to the horizontal plane
1236
 
        // containing the centre of projection
1237
 
        dsp1_state.shared.VOffset = Les * dsp1_state.shared.CosAZS >> 15;
1238
 
 
1239
 
        // The horizon line (the line in the screen that is crossed by the horizon plane
1240
 
        // -the horizontal plane containing the 'centre of projection'-),
1241
 
        // will be at distance Les*cotg(AZS) from the centre of the screen. This is difficult
1242
 
        // to explain but easily seen in a graph. To better see it, consider it in this way:
1243
 
        // Les*tg(AZS-90), draw some lines and apply basic trigonometry. ;)
1244
 
        inverse(dsp1_state.shared.SinAZS, 0, &CSec, &E);
1245
 
        normalize(dsp1_state.shared.VOffset, &C, &E);
1246
 
        normalize(C * CSec >> 15, &C, &E);
1247
 
 
1248
 
        if (C == -32768)
1249
 
        {
1250
 
                C >>= 1;
1251
 
                E++;
1252
 
        }
1253
 
 
1254
 
        *Vva = denormalize_and_clip(-C, E);
1255
 
 
1256
 
        // Store Secant of clipped Zenith angle
1257
 
        inverse(dsp1_state.shared.CosAZS, 0, &dsp1_state.shared.SecAZS_C2, &dsp1_state.shared.SecAZS_E2);
1258
 
}
1259
 
 
1260
 
//////////////////////////////////////////////////////////////////
1261
 
 
1262
 
// Calculates the matrix which transform an object situated on a raster line (Vs) into
1263
 
// his projection over the ground. The modified SecAZS is used here, so
1264
 
// i don't understand the fine details, but, basically, it's done
1265
 
// this way: The vertical offset between the point of projection and the
1266
 
// raster line is calculated (Vs*SinAzs>>15)+VOffset, then the height of
1267
 
// the center of projection is measured in that units (*CentreZ_C). If, now
1268
 
// you consider the "reference case" (center of projection at an unit of height),
1269
 
// the projection of a thin strip containing the raster line will have the same
1270
 
// width (as the raster line would be on the ground in this case, but will suffer a
1271
 
// change of scale in height (as the ground and the vertical axis would form an angle of 180-Azs degrees).
1272
 
// This scale factor, when the angle 'center of screen-center of projection-raster line' is small,
1273
 
// can be aproximated by the one of the center of the screen, 1/cos(Azs).(**) (Here is when it's used
1274
 
// SecAZS). By last, you have to consider the effect of the azimuth angle Aas, and you are done.
1275
 
//
1276
 
// Using matrix notation:
1277
 
//                    |A      B|     Centre_ZS    | cos(Aas)    -sin(Aas)|   | 1         0   |
1278
 
// ProjectionMatrix = |        | = ----------- *  |                      | * |               |
1279
 
//                    |C      D|    Vs*sin(Azs)   | sin(Aas)     cos(Aas)|   | 0     sec(Azs)|
1280
 
//
1281
 
// (**)
1282
 
// If Les=1, the vertical offset between the center
1283
 
// of projection and the center of the screen is Cos(Azs); then, if the vertical
1284
 
// offset is 1, the ratio of the projection over the ground respect to the
1285
 
// line on the screen is 1/cos(Azs).
1286
 
 
1287
 
static void dsp1_raster( INT16 *input, INT16 *output )
1288
 
{
1289
 
        INT16 Vs = input[0];
1290
 
        INT16* An = &output[0];
1291
 
        INT16* Bn = &output[1];
1292
 
        INT16* Cn = &output[2];
1293
 
        INT16* Dn = &output[3];
1294
 
 
1295
 
        INT16 C, E, C1, E1;
1296
 
 
1297
 
        inverse((Vs * dsp1_state.shared.SinAzs >> 15) + dsp1_state.shared.VOffset, 7, &C, &E);
1298
 
 
1299
 
        E += dsp1_state.shared.CentreZ_E;
1300
 
        C1 = C * dsp1_state.shared.CentreZ_C >> 15;
1301
 
 
1302
 
        E1 = E + dsp1_state.shared.SecAZS_E2;
1303
 
 
1304
 
        normalize(C1, &C, &E);
1305
 
        C = denormalize_and_clip(C, E);
1306
 
 
1307
 
        *An = C * dsp1_state.shared.CosAas >> 15;
1308
 
        *Cn = C * dsp1_state.shared.SinAas >> 15;
1309
 
 
1310
 
        normalize(C1 * dsp1_state.shared.SecAZS_C2 >> 15, &C, &E1);
1311
 
        C = denormalize_and_clip(C, E1);
1312
 
 
1313
 
        *Bn = C * -dsp1_state.shared.SinAas >> 15;
1314
 
        *Dn = C * dsp1_state.shared.CosAas >> 15;
1315
 
}
1316
 
 
1317
 
//////////////////////////////////////////////////////////////////
1318
 
 
1319
 
// Calculate the projection over the ground of a selected point of screen
1320
 
// It simply apply the projection matrix described in the "Raster" command
1321
 
// to the vector (H,V) transposed, and add the result to the position of
1322
 
// the centre of projection.
1323
 
// The only special point to take into account is the directions on the screen:
1324
 
// H is positive rightward, but V is positive downward; this is why
1325
 
// the signs take that configuration
1326
 
 
1327
 
static void dsp1_target( INT16 *input, INT16 *output )
1328
 
{
1329
 
        INT16 H = input[0];
1330
 
        INT16 V = input[1];
1331
 
        INT16* X = &output[0];
1332
 
        INT16* Y = &output[1];
1333
 
 
1334
 
        INT16 C, E, C1, E1;
1335
 
 
1336
 
        inverse((V * dsp1_state.shared.SinAzs >> 15) + dsp1_state.shared.VOffset, 8, &C, &E);
1337
 
 
1338
 
        E += dsp1_state.shared.CentreZ_E;
1339
 
        C1 = C * dsp1_state.shared.CentreZ_C >> 15;
1340
 
 
1341
 
        E1 = E + dsp1_state.shared.SecAZS_E1;
1342
 
 
1343
 
        H <<= 8;
1344
 
        normalize(C1, &C, &E);
1345
 
        C = denormalize_and_clip(C, E) * H >> 15;
1346
 
 
1347
 
        *X = dsp1_state.shared.CentreX + (C * dsp1_state.shared.CosAas >> 15);
1348
 
        *Y = dsp1_state.shared.CentreY - (C * dsp1_state.shared.SinAas >> 15);
1349
 
 
1350
 
        V <<= 8;
1351
 
        normalize(C1 * dsp1_state.shared.SecAZS_C1 >> 15, &C, &E1);
1352
 
        C = denormalize_and_clip(C, E1) * V >> 15;
1353
 
 
1354
 
        *X += C * -dsp1_state.shared.SinAas >> 15;
1355
 
        *Y += C * dsp1_state.shared.CosAas >> 15;
1356
 
}
1357
 
 
1358
 
//////////////////////////////////////////////////////////////////
1359
 
 
1360
 
// Calculation of the projection over the screen (H,V) of an object (X,Y,Z) and his
1361
 
// 'enlargement ratio' (M). The positive directions on the screen are as described
1362
 
// in the targe command. M is scaled down by 2^-7, that is, M==0x0100 means ratio 1:1
1363
 
 
1364
 
static void dsp1_project( INT16 *input, INT16 *output )
1365
 
{
1366
 
        INT16 X = input[0];
1367
 
        INT16 Y = input[1];
1368
 
        INT16 Z = input[2];
1369
 
        INT16* H = &output[0];
1370
 
        INT16* V = &output[1];
1371
 
        INT16* M = &output[2];
1372
 
 
1373
 
        INT32 aux, aux4;
1374
 
        INT16 E, E2, E3, E4, E5, refE, E6, E7;
1375
 
        INT16 C2, C4, C6, C8, C9, C10, C11, C12, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26;
1376
 
        INT16 Px, Py, Pz;
1377
 
 
1378
 
        E4 = E3 = E2 = E = E5 = 0;
1379
 
 
1380
 
        normalize_double((INT32)(X) - dsp1_state.shared.Gx, &Px, &E4);
1381
 
        normalize_double((INT32)(Y) - dsp1_state.shared.Gy, &Py, &E);
1382
 
        normalize_double((INT32)(Z) - dsp1_state.shared.Gz, &Pz, &E3);
1383
 
        Px >>= 1; E4--; // to avoid overflows when calculating the scalar products
1384
 
        Py >>= 1; E--;
1385
 
        Pz >>= 1; E3--;
1386
 
 
1387
 
        refE = (E < E3) ? E : E3;
1388
 
        refE = (refE < E4) ? refE : E4;
1389
 
 
1390
 
        Px = shiftR(Px, E4 - refE);      // normalize them to the same exponent
1391
 
        Py = shiftR(Py, E  - refE);
1392
 
        Pz = shiftR(Pz, E3 - refE);
1393
 
 
1394
 
        C11 = -(Px * dsp1_state.shared.Nx >> 15);
1395
 
        C8  = -(Py * dsp1_state.shared.Ny >> 15);
1396
 
        C9  = -(Pz * dsp1_state.shared.Nz >> 15);
1397
 
        C12 = C11 + C8 + C9;    // this cannot overflow!
1398
 
 
1399
 
        aux4 = C12;     // de-normalization with 32-bits arithmetic
1400
 
        refE = 16 - refE;        // refE can be up to 3
1401
 
        if (refE >= 0)
1402
 
                aux4 <<=  (refE);
1403
 
        else
1404
 
                aux4 >>= -(refE);
1405
 
        if (aux4 == -1)
1406
 
                aux4 = 0;               // why?
1407
 
        aux4 >>= 1;
1408
 
 
1409
 
        aux = (UINT16)(dsp1_state.shared.Les) + aux4;   // Les - the scalar product of P with the normal vector of the screen
1410
 
        normalize_double(aux, &C10, &E2);
1411
 
        E2 = 15 - E2;
1412
 
 
1413
 
        inverse(C10, 0, &C4, &E4);
1414
 
        C2 = C4 * dsp1_state.shared.C_Les >> 15;                                          // scale factor
1415
 
 
1416
 
 
1417
 
        // H
1418
 
        E7 = 0;
1419
 
        C16 = (Px * dsp1_state.shared.Hx >> 15);
1420
 
        C20 = (Py * dsp1_state.shared.Hy >> 15);
1421
 
        C17 = C16 + C20;        // scalar product of P with the normalized horizontal vector of the screen...
1422
 
 
1423
 
        C18 = C17 * C2 >> 15;    // ... multiplied by the scale factor
1424
 
        normalize(C18, &C19, &E7);
1425
 
        *H = denormalize_and_clip(C19, dsp1_state.shared.E_Les - E2 + refE + E7);
1426
 
 
1427
 
        // V
1428
 
        E6 = 0;
1429
 
        C21 = Px * dsp1_state.shared.Vx >> 15;
1430
 
        C22 = Py * dsp1_state.shared.Vy >> 15;
1431
 
        C23 = Pz * dsp1_state.shared.Vz >> 15;
1432
 
        C24 = C21 + C22 + C23;  // scalar product of P with the normalized vertical vector of the screen...
1433
 
 
1434
 
        C26 = C24 * C2 >> 15;    // ... multiplied by the scale factor
1435
 
        normalize(C26, &C25, &E6);
1436
 
        *V = denormalize_and_clip(C25, dsp1_state.shared.E_Les - E2 + refE + E6);
1437
 
 
1438
 
        // M
1439
 
        normalize(C2, &C6, &E4);
1440
 
        *M = denormalize_and_clip(C6, E4 + dsp1_state.shared.E_Les - E2 - 7); // M is the scale factor divided by 2^7
1441
 
}
1442
 
 
1443
 
//////////////////////////////////////////////////////////////////
1444
 
 
1445
 
// Calculate the sine of the input parameter
1446
 
// this is done by linear interpolation between
1447
 
// the points of a look-up table
1448
 
 
1449
 
static INT16 dsp1_sin( INT16 Angle )
1450
 
{
1451
 
        INT32 S;
1452
 
 
1453
 
        if (Angle < 0)
1454
 
        {
1455
 
                if (Angle == -32768)
1456
 
                        return 0;
1457
 
 
1458
 
                return -dsp1_sin(-Angle);
1459
 
        }
1460
 
        S = dsp1_sin_table[Angle >> 8] + (dsp1_mul_table[Angle & 0xff] * dsp1_sin_table[0x40 + (Angle >> 8)] >> 15);
1461
 
        if (S > 32767)
1462
 
                S = 32767;
1463
 
        return (INT16) S;
1464
 
}
1465
 
 
1466
 
//////////////////////////////////////////////////////////////////
1467
 
 
1468
 
// Calculate the cosine of the input parameter.
1469
 
// It's used the same method than in sin(INT16)
1470
 
 
1471
 
static INT16 dsp1_cos( INT16 Angle )
1472
 
{
1473
 
        INT32 S;
1474
 
 
1475
 
        if (Angle < 0)
1476
 
        {
1477
 
                if (Angle == -32768)
1478
 
                        return -32768;
1479
 
                Angle = -Angle;
1480
 
        }
1481
 
        S = dsp1_sin_table[0x40 + (Angle >> 8)] - (dsp1_mul_table[Angle & 0xff] * dsp1_sin_table[Angle >> 8] >> 15);
1482
 
        if (S < -32768)
1483
 
                S = -32767;
1484
 
        return (INT16) S;
1485
 
}
1486
 
 
1487
 
//////////////////////////////////////////////////////////////////
1488
 
 
1489
 
// Determines the inverse of a floating point decimal number
1490
 
// iCoefficient*2^iExponent = 1/(Coefficient*2^Exponent), with the output
1491
 
// normalized (iCoefficient represents a number whose absolute value is between 1/2 and 1)
1492
 
// To invert 'Coefficient' a first initial guess is taken from a look-up table
1493
 
// and, then, two iterations of the Newton method (applied to the function
1494
 
// f(x)=1/(2*x)-Coefficient) are done. This results in a close approximation (iCoefficient) to a number 'y'
1495
 
// that verify Coefficient*y=1/2. This is why you have to correct the exponent by one
1496
 
// unit at the end.
1497
 
 
1498
 
static void inverse( INT16 Coefficient, INT16 Exponent, INT16 *iCoefficient, INT16 *iExponent )
1499
 
{
1500
 
        // Step One: Division by Zero
1501
 
        if (Coefficient == 0x0000)
1502
 
        {
1503
 
                *iCoefficient = 0x7fff;
1504
 
                *iExponent = 0x002f;
1505
 
        }
1506
 
        else
1507
 
        {
1508
 
                INT16 Sign = 1;
1509
 
 
1510
 
                // Step Two: Remove Sign
1511
 
                if (Coefficient < 0)
1512
 
                {
1513
 
                        if (Coefficient < -32767)
1514
 
                                Coefficient = -32767;
1515
 
                        Coefficient = -Coefficient;
1516
 
                        Sign = -1;
1517
 
                }
1518
 
 
1519
 
                // Step Three: Normalize
1520
 
                while (Coefficient < 0x4000)
1521
 
                {
1522
 
                        Coefficient <<= 1;
1523
 
                        Exponent--;
1524
 
                }
1525
 
 
1526
 
                // Step Four: Special Case
1527
 
                if (Coefficient == 0x4000)
1528
 
                        if (Sign == 1) *iCoefficient = 0x7fff;
1529
 
                else  {
1530
 
                        *iCoefficient = -0x4000;
1531
 
                        Exponent--;
1532
 
                }
1533
 
                else {
1534
 
                        // Step Five: Initial Guess
1535
 
                        INT16 i = dsp1_state.DataRom[((Coefficient - 0x4000) >> 7) + 0x0065];
1536
 
 
1537
 
                        // Step Six: Iterate Newton's Method
1538
 
                        i = (i + (-i * (Coefficient * i >> 15) >> 15)) << 1;
1539
 
                        i = (i + (-i * (Coefficient * i >> 15) >> 15)) << 1;
1540
 
 
1541
 
                        *iCoefficient = i * Sign;
1542
 
                }
1543
 
 
1544
 
                *iExponent = 1 - Exponent;
1545
 
        }
1546
 
}
1547
 
 
1548
 
//////////////////////////////////////////////////////////////////
1549
 
 
1550
 
static INT16 denormalize_and_clip( INT16 C, INT16 E )
1551
 
{
1552
 
        if (E > 0)
1553
 
        {
1554
 
                if (C > 0)
1555
 
                        return 32767;
1556
 
                else if (C < 0)
1557
 
                        return -32767;
1558
 
        }
1559
 
        else
1560
 
        {
1561
 
                if (E < 0)
1562
 
                        return C * dsp1_state.DataRom[0x0031 + E] >> 15;
1563
 
        }
1564
 
        return C;
1565
 
}
1566
 
 
1567
 
//////////////////////////////////////////////////////////////////
1568
 
 
1569
 
// Normalize the input number (m), understood as ranging from -1 to 1,
1570
 
// to the form: Coefficient*2^Exponent,
1571
 
// where the absolute value of Coefficient is >= 1/2
1572
 
// (Coefficient>=0x4000 or Coefficient <= (INT16)0xc001)
1573
 
 
1574
 
static void normalize( INT16 m, INT16 *Coefficient, INT16 *Exponent )
1575
 
{
1576
 
        INT16 i = 0x4000;
1577
 
        INT16 e = 0;
1578
 
 
1579
 
        if (m < 0)
1580
 
                while ((m & i) && i)
1581
 
                {
1582
 
                        i >>= 1;
1583
 
                        e++;
1584
 
                }
1585
 
        else
1586
 
                while (!(m & i) && i)
1587
 
                {
1588
 
                        i >>= 1;
1589
 
                        e++;
1590
 
                }
1591
 
 
1592
 
        if (e > 0)
1593
 
                *Coefficient = m * dsp1_state.DataRom[0x21 + e] << 1;
1594
 
        else
1595
 
                *Coefficient = m;
1596
 
 
1597
 
        *Exponent -= e;
1598
 
}
1599
 
 
1600
 
//////////////////////////////////////////////////////////////////
1601
 
 
1602
 
// Same than 'normalize' but with an INT32 input
1603
 
 
1604
 
static void normalize_double( INT32 Product, INT16 *Coefficient, INT16 *Exponent )
1605
 
{
1606
 
        INT16 n = Product & 0x7fff;
1607
 
        INT16 m = Product >> 15;
1608
 
        INT16 i = 0x4000;
1609
 
        INT16 e = 0;
1610
 
 
1611
 
        if (m < 0)
1612
 
                while ((m & i) && i)
1613
 
                {
1614
 
                        i >>= 1;
1615
 
                        e++;
1616
 
                }
1617
 
        else
1618
 
                while (!(m & i) && i)
1619
 
                {
1620
 
                        i >>= 1;
1621
 
                        e++;
1622
 
                }
1623
 
 
1624
 
        if (e > 0)
1625
 
        {
1626
 
                *Coefficient = m * dsp1_state.DataRom[0x0021 + e] << 1;
1627
 
 
1628
 
                if (e < 15)
1629
 
                        *Coefficient += n * dsp1_state.DataRom[0x0040 - e] >> 15;
1630
 
                else
1631
 
                {
1632
 
                        i = 0x4000;
1633
 
 
1634
 
                        if (m < 0)
1635
 
                                while ((n & i) && i)
1636
 
                                {
1637
 
                                        i >>= 1;
1638
 
                                        e++;
1639
 
                                }
1640
 
                        else
1641
 
                                while (!(n & i) && i)
1642
 
                                {
1643
 
                                        i >>= 1;
1644
 
                                        e++;
1645
 
                                }
1646
 
 
1647
 
                        if (e > 15)
1648
 
                                *Coefficient = n * dsp1_state.DataRom[0x0012 + e] << 1;
1649
 
                        else
1650
 
                                *Coefficient += n;
1651
 
                }
1652
 
        }
1653
 
        else
1654
 
                *Coefficient = m;
1655
 
 
1656
 
        *Exponent = e;
1657
 
}
1658
 
 
1659
 
//////////////////////////////////////////////////////////////////
1660
 
 
1661
 
// Shift to the right
1662
 
 
1663
 
static INT16 shiftR( INT16 C, INT16 E )
1664
 
{
1665
 
        return (C * dsp1_state.DataRom[0x0031 + E] >> 15);
1666
 
}
1667
 
 
1668
 
//////////////////////////////////////////////////////////////////
1669
 
 
1670
 
// Save DSP1 variables
1671
 
 
1672
 
static void dsp1_register_save( running_machine *machine )
1673
 
{
1674
 
        state_save_register_global(machine, dsp1_state.Sr);
1675
 
        state_save_register_global(machine, dsp1_state.Dr);
1676
 
        state_save_register_global(machine, dsp1_state.SrLowByteAccess);
1677
 
        state_save_register_global(machine, dsp1_state.FsmMajorState);
1678
 
        state_save_register_global(machine, dsp1_state.Command);
1679
 
        state_save_register_global(machine, dsp1_state.DataCounter);
1680
 
        state_save_register_global(machine, dsp1_state.Freeze);
1681
 
        state_save_register_global_array(machine, dsp1_state.ReadBuffer);
1682
 
        state_save_register_global_array(machine, dsp1_state.WriteBuffer);
1683
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixA[0]);
1684
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixA[1]);
1685
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixA[2]);
1686
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixB[0]);
1687
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixB[1]);
1688
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixB[2]);
1689
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixC[0]);
1690
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixC[1]);
1691
 
        state_save_register_global_array(machine, dsp1_state.shared.MatrixC[2]);
1692
 
        state_save_register_global(machine, dsp1_state.shared.CentreX);
1693
 
        state_save_register_global(machine, dsp1_state.shared.CentreY);
1694
 
        state_save_register_global(machine, dsp1_state.shared.CentreZ);
1695
 
        state_save_register_global(machine, dsp1_state.shared.CentreZ_C);
1696
 
        state_save_register_global(machine, dsp1_state.shared.CentreZ_E);
1697
 
        state_save_register_global(machine, dsp1_state.shared.VOffset);
1698
 
        state_save_register_global(machine, dsp1_state.shared.Les);
1699
 
        state_save_register_global(machine, dsp1_state.shared.C_Les);
1700
 
        state_save_register_global(machine, dsp1_state.shared.E_Les);
1701
 
        state_save_register_global(machine, dsp1_state.shared.SinAas);
1702
 
        state_save_register_global(machine, dsp1_state.shared.CosAas);
1703
 
        state_save_register_global(machine, dsp1_state.shared.SinAzs);
1704
 
        state_save_register_global(machine, dsp1_state.shared.CosAzs);
1705
 
        state_save_register_global(machine, dsp1_state.shared.SinAZS);
1706
 
        state_save_register_global(machine, dsp1_state.shared.CosAZS);
1707
 
        state_save_register_global(machine, dsp1_state.shared.SecAZS_C1);
1708
 
        state_save_register_global(machine, dsp1_state.shared.SecAZS_E1);
1709
 
        state_save_register_global(machine, dsp1_state.shared.SecAZS_C2);
1710
 
        state_save_register_global(machine, dsp1_state.shared.SecAZS_E2);
1711
 
        state_save_register_global(machine, dsp1_state.shared.Nx);
1712
 
        state_save_register_global(machine, dsp1_state.shared.Ny);
1713
 
        state_save_register_global(machine, dsp1_state.shared.Nz);
1714
 
        state_save_register_global(machine, dsp1_state.shared.Gx);
1715
 
        state_save_register_global(machine, dsp1_state.shared.Gy);
1716
 
        state_save_register_global(machine, dsp1_state.shared.Gz);
1717
 
        state_save_register_global(machine, dsp1_state.shared.Hx);
1718
 
        state_save_register_global(machine, dsp1_state.shared.Hy);
1719
 
        state_save_register_global(machine, dsp1_state.shared.Vx);
1720
 
        state_save_register_global(machine, dsp1_state.shared.Vy);
1721
 
        state_save_register_global(machine, dsp1_state.shared.Vz);
1722
 
}