seta10.c

linux下的任天堂模拟器代码。供大家参考。

/*
Copyright (C) 1997-2007 ZSNES Team ( zsKnight, _Demo_, pagefault, Nach )

http://www.zsnes.com
http://sourceforge.net/projects/zsnes
https://zsnes.bountysource.com

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
version 2 as published by the Free Software Foundation.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

//http://users.tpg.com.au/trauma/dsp/st010.html

#ifdef __UNIXSDL__
#include "gblhdr.h"
#else
#include <math.h> //sqrt()
#include <stdlib.h> //abs()
#endif

#define SRAM setaramdata

extern unsigned char *setaramdata;
void ST010DoCommand(void);

//C++ style code in C
#define bool unsigned char
#define true 1
#define false 0

typedef signed char     int8;
typedef unsigned char   uint8;
typedef short           int16;
typedef unsigned short  uint16;
typedef int             int32;
typedef unsigned int    uint32;

// Mode 7 scaling constants for all raster lines
const int16 ST010_M7Scale[176] = {
	0x0380,  0x0325,  0x02da,  0x029c,  0x0268,  0x023b,  0x0215,  0x01f3,
	0x01d5,  0x01bb,  0x01a3,  0x018e,  0x017b,  0x016a,  0x015a,  0x014b,
	0x013e,  0x0132,  0x0126,  0x011c,  0x0112,  0x0109,  0x0100,  0x00f8,
	0x00f0,  0x00e9,  0x00e3,  0x00dc,  0x00d6,  0x00d1,  0x00cb,  0x00c6,
	0x00c1,  0x00bd,  0x00b8,  0x00b4,  0x00b0,  0x00ac,  0x00a8,  0x00a5,
	0x00a2,  0x009e,  0x009b,  0x0098,  0x0095,  0x0093,  0x0090,  0x008d,
	0x008b,  0x0088,  0x0086,  0x0084,  0x0082,  0x0080,  0x007e,  0x007c,
	0x007a,  0x0078,  0x0076,  0x0074,  0x0073,  0x0071,  0x006f,  0x006e,
	0x006c,  0x006b,  0x0069,  0x0068,  0x0067,  0x0065,  0x0064,  0x0063,
	0x0062,  0x0060,  0x005f,  0x005e,  0x005d,  0x005c,  0x005b,  0x005a,
	0x0059,  0x0058,  0x0057,  0x0056,  0x0055,  0x0054,  0x0053,  0x0052,
	0x0051,  0x0051,  0x0050,  0x004f,  0x004e,  0x004d,  0x004d,  0x004c,
	0x004b,  0x004b,  0x004a,  0x0049,  0x0048,  0x0048,  0x0047,  0x0047,
	0x0046,  0x0045,  0x0045,  0x0044,  0x0044,  0x0043,  0x0042,  0x0042,
	0x0041,  0x0041,  0x0040,  0x0040,  0x003f,  0x003f,  0x003e,  0x003e,
	0x003d,  0x003d,  0x003c,  0x003c,  0x003b,  0x003b,  0x003a,  0x003a,
	0x003a,  0x0039,  0x0039,  0x0038,  0x0038,  0x0038,  0x0037,  0x0037,
	0x0036,  0x0036,  0x0036,  0x0035,  0x0035,  0x0035,  0x0034,  0x0034,
	0x0034,  0x0033,  0x0033,  0x0033,  0x0032,  0x0032,  0x0032,  0x0031,
	0x0031,  0x0031,  0x0030,  0x0030,  0x0030,  0x0030,  0x002f,  0x002f,
	0x002f,  0x002e,  0x002e,  0x002e,  0x002e,  0x002d,  0x002d,  0x002d,
	0x002d,  0x002c,  0x002c,  0x002c,  0x002c,  0x002b,  0x002b,  0x002b
};

const int16 ST010_SinTable[256] = {
	 0x0000,  0x0324,  0x0648,  0x096a,  0x0c8c,  0x0fab,  0x12c8,  0x15e2,
	 0x18f9,  0x1c0b,  0x1f1a,  0x2223,  0x2528,  0x2826,  0x2b1f,  0x2e11,
	 0x30fb,  0x33df,  0x36ba,  0x398c,  0x3c56,  0x3f17,  0x41ce,  0x447a,
	 0x471c,  0x49b4,  0x4c3f,  0x4ebf,  0x5133,  0x539b,  0x55f5,  0x5842,
	 0x5a82,  0x5cb3,  0x5ed7,  0x60eb,  0x62f1,  0x64e8,  0x66cf,  0x68a6,
	 0x6a6d,  0x6c23,  0x6dc9,  0x6f5e,  0x70e2,  0x7254,  0x73b5,  0x7504,
	 0x7641,  0x776b,  0x7884,  0x7989,  0x7a7c,  0x7b5c,  0x7c29,  0x7ce3,
	 0x7d89,  0x7e1d,  0x7e9c,  0x7f09,  0x7f61,  0x7fa6,  0x7fd8,  0x7ff5,
	 0x7fff,  0x7ff5,  0x7fd8,  0x7fa6,  0x7f61,  0x7f09,  0x7e9c,  0x7e1d,
	 0x7d89,  0x7ce3,  0x7c29,  0x7b5c,  0x7a7c,  0x7989,  0x7884,  0x776b,
	 0x7641,  0x7504,  0x73b5,  0x7254,  0x70e2,  0x6f5e,  0x6dc9,  0x6c23,
	 0x6a6d,  0x68a6,  0x66cf,  0x64e8,  0x62f1,  0x60eb,  0x5ed7,  0x5cb3,
	 0x5a82,  0x5842,  0x55f5,  0x539b,  0x5133,  0x4ebf,  0x4c3f,  0x49b4,
	 0x471c,  0x447a,  0x41ce,  0x3f17,  0x3c56,  0x398c,  0x36ba,  0x33df,
	 0x30fb,  0x2e11,  0x2b1f,  0x2826,  0x2528,  0x2223,  0x1f1a,  0x1c0b,
	 0x18f8,  0x15e2,  0x12c8,  0x0fab,  0x0c8c,  0x096a,  0x0648,  0x0324,
	 0x0000, -0x0324, -0x0648, -0x096b, -0x0c8c, -0x0fab, -0x12c8, -0x15e2,
	-0x18f9, -0x1c0b, -0x1f1a, -0x2223, -0x2528, -0x2826, -0x2b1f, -0x2e11,
	-0x30fb, -0x33df, -0x36ba, -0x398d, -0x3c56, -0x3f17, -0x41ce, -0x447a,
	-0x471c, -0x49b4, -0x4c3f, -0x4ebf, -0x5133, -0x539b, -0x55f5, -0x5842,
	-0x5a82, -0x5cb3, -0x5ed7, -0x60ec, -0x62f1, -0x64e8, -0x66cf, -0x68a6,
	-0x6a6d, -0x6c23, -0x6dc9, -0x6f5e, -0x70e2, -0x7254, -0x73b5, -0x7504,
	-0x7641, -0x776b, -0x7884, -0x7989, -0x7a7c, -0x7b5c, -0x7c29, -0x7ce3,
	-0x7d89, -0x7e1d, -0x7e9c, -0x7f09, -0x7f61, -0x7fa6, -0x7fd8, -0x7ff5,
	-0x7fff, -0x7ff5, -0x7fd8, -0x7fa6, -0x7f61, -0x7f09, -0x7e9c, -0x7e1d,
	-0x7d89, -0x7ce3, -0x7c29, -0x7b5c, -0x7a7c, -0x7989, -0x7883, -0x776b,
	-0x7641, -0x7504, -0x73b5, -0x7254, -0x70e2, -0x6f5e, -0x6dc9, -0x6c23,
	-0x6a6d, -0x68a6, -0x66cf, -0x64e8, -0x62f1, -0x60eb, -0x5ed7, -0x5cb3,
	-0x5a82, -0x5842, -0x55f5, -0x539a, -0x5133, -0x4ebf, -0x4c3f, -0x49b3,
	-0x471c, -0x447a, -0x41cd, -0x3f17, -0x3c56, -0x398c, -0x36b9, -0x33de,
	-0x30fb, -0x2e10, -0x2b1f, -0x2826, -0x2527, -0x2223, -0x1f19, -0x1c0b,
	-0x18f8, -0x15e2, -0x12c8, -0x0fab, -0x0c8b, -0x096a, -0x0647, -0x0324};

const unsigned char ST010_ArcTan[32][32] = {
	{ 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,
	  0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80},
	{ 0x80, 0xa0, 0xad, 0xb3, 0xb6, 0xb8, 0xb9, 0xba, 0xbb, 0xbb, 0xbc, 0xbc, 0xbd, 0xbd, 0xbd, 0xbd,
	  0xbd, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbe, 0xbf, 0xbf, 0xbf, 0xbf},
	{ 0x80, 0x93, 0xa0, 0xa8, 0xad, 0xb0, 0xb3, 0xb5, 0xb6, 0xb7, 0xb8, 0xb9, 0xb9, 0xba, 0xba, 0xbb,
	  0xbb, 0xbb, 0xbb, 0xbc, 0xbc, 0xbc, 0xbc, 0xbc, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd, 0xbd},
	{ 0x80, 0x8d, 0x98, 0xa0, 0xa6, 0xaa, 0xad, 0xb0, 0xb1, 0xb3, 0xb4, 0xb5, 0xb6, 0xb7, 0xb7, 0xb8,
	  0xb8, 0xb9, 0xb9, 0xba, 0xba, 0xba, 0xba, 0xbb, 0xbb, 0xbb, 0xbb, 0xbb, 0xbc, 0xbc, 0xbc, 0xbc},
	{ 0x80, 0x8a, 0x93, 0x9a, 0xa0, 0xa5, 0xa8, 0xab, 0xad, 0xaf, 0xb0, 0xb2, 0xb3, 0xb4, 0xb5, 0xb5,
	  0xb6, 0xb7, 0xb7, 0xb8, 0xb8, 0xb8, 0xb9, 0xb9, 0xb9, 0xba, 0xba, 0xba, 0xba, 0xba, 0xbb, 0xbb},
	{ 0x80, 0x88, 0x90, 0x96, 0x9b, 0xa0, 0xa4, 0xa7, 0xa9, 0xab, 0xad, 0xaf, 0xb0, 0xb1, 0xb2, 0xb3,
	  0xb4, 0xb4, 0xb5, 0xb6, 0xb6, 0xb6, 0xb7, 0xb7, 0xb8, 0xb8, 0xb8, 0xb9, 0xb9, 0xb9, 0xb9, 0xb9},
	{ 0x80, 0x87, 0x8d, 0x93, 0x98, 0x9c, 0xa0, 0xa3, 0xa6, 0xa8, 0xaa, 0xac, 0xad, 0xae, 0xb0, 0xb0,
	  0xb1, 0xb2, 0xb3, 0xb4, 0xb4, 0xb5, 0xb5, 0xb6, 0xb6, 0xb6, 0xb7, 0xb7, 0xb7, 0xb8, 0xb8, 0xb8},
	{ 0x80, 0x86, 0x8b, 0x90, 0x95, 0x99, 0x9d, 0xa0, 0xa3, 0xa5, 0xa7, 0xa9, 0xaa, 0xac, 0xad, 0xae,
	  0xaf, 0xb0, 0xb1, 0xb2, 0xb2, 0xb3, 0xb3, 0xb4, 0xb4, 0xb5, 0xb5, 0xb6, 0xb6, 0xb6, 0xb7, 0xb7},
	{ 0x80, 0x85, 0x8a, 0x8f, 0x93, 0x97, 0x9a, 0x9d, 0xa0, 0xa2, 0xa5, 0xa6, 0xa8, 0xaa, 0xab, 0xac,
	  0xad, 0xae, 0xaf, 0xb0, 0xb0, 0xb1, 0xb2, 0xb2, 0xb3, 0xb3, 0xb4, 0xb4, 0xb5, 0xb5, 0xb5, 0xb5},
	{ 0x80, 0x85, 0x89, 0x8d, 0x91, 0x95, 0x98, 0x9b, 0x9e, 0xa0, 0xa0, 0xa4, 0xa6, 0xa7, 0xa9, 0xaa,
	  0xab, 0xac, 0xad, 0xae, 0xaf, 0xb0, 0xb0, 0xb1, 0xb1, 0xb2, 0xb2, 0xb3, 0xb3, 0xb4, 0xb4, 0xb4},
	{ 0x80, 0x84, 0x88, 0x8c, 0x90, 0x93, 0x96, 0x99, 0x9b, 0x9e, 0xa0, 0xa2, 0xa4, 0xa5, 0xa7, 0xa8,
	  0xa9, 0xaa, 0xab, 0xac, 0xad, 0xae, 0xaf, 0xaf, 0xb0, 0xb0, 0xb1, 0xb2, 0xb2, 0xb2, 0xb3, 0xb3},
	{ 0x80, 0x84, 0x87, 0x8b, 0x8e, 0x91, 0x94, 0x97, 0x9a, 0x9c, 0x9e, 0xa0, 0xa2, 0xa3, 0xa5, 0xa6,
	  0xa7, 0xa9, 0xaa, 0xab, 0xac, 0xac, 0xad, 0xae, 0xae, 0xaf, 0xb0, 0xb0, 0xb1, 0xb1, 0xb2, 0xb2},
	{ 0x80, 0x83, 0x87, 0x8a, 0x8d, 0x90, 0x93, 0x96, 0x98, 0x9a, 0x9c, 0x9e, 0xa0, 0xa2, 0xa3, 0xa5,
	  0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xab, 0xac, 0xac, 0xad, 0xae, 0xae, 0xaf, 0xb0, 0xb0, 0xb0, 0xb1},
	{ 0x80, 0x83, 0x86, 0x89, 0x8c, 0x8f, 0x92, 0x94, 0x96, 0x99, 0x9b, 0x9d, 0x9e, 0xa0, 0xa2, 0xa3,
	  0xa4, 0xa5, 0xa7, 0xa8, 0xa9, 0xa9, 0xaa, 0xab, 0xac, 0xac, 0xad, 0xae, 0xae, 0xaf, 0xaf, 0xb0},
	{ 0x80, 0x83, 0x86, 0x89, 0x8b, 0x8e, 0x90, 0x93, 0x95, 0x97, 0x99, 0x9b, 0x9d, 0x9e, 0xa0, 0xa1,
	  0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xaa, 0xab, 0xac, 0xad, 0xad, 0xae, 0xae, 0xaf},
	{ 0x80, 0x83, 0x85, 0x88, 0x8b, 0x8d, 0x90, 0x92, 0x94, 0x96, 0x98, 0x9a, 0x9b, 0x9d, 0x9f, 0xa0,
	  0xa1, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa8, 0xa9, 0xaa, 0xab, 0xab, 0xac, 0xad, 0xad, 0xae},
	{ 0x80, 0x83, 0x85, 0x88, 0x8a, 0x8c, 0x8f, 0x91, 0x93, 0x95, 0x97, 0x99, 0x9a, 0x9c, 0x9d, 0x9f,
	  0xa0, 0xa1, 0xa2, 0xa3, 0xa5, 0xa5, 0xa6, 0xa7, 0xa8, 0xa9, 0xaa, 0xaa, 0xab, 0xab, 0xac, 0xad},
	{ 0x80, 0x82, 0x85, 0x87, 0x89, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x96, 0x97, 0x99, 0x9b, 0x9c, 0x9d,
	  0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa8, 0xa8, 0xa9, 0xaa, 0xaa, 0xab, 0xac},
	{ 0x80, 0x82, 0x85, 0x87, 0x89, 0x8b, 0x8d, 0x8f, 0x91, 0x93, 0x95, 0x96, 0x98, 0x99, 0x9b, 0x9c,
	  0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7, 0xa7, 0xa8, 0xa9, 0xa9, 0xaa, 0xab},
	{ 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x92, 0x94, 0x95, 0x97, 0x98, 0x9a, 0x9b,
	  0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa6, 0xa7, 0xa8, 0xa8, 0xa9, 0xaa},
	{ 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8c, 0x8e, 0x90, 0x91, 0x93, 0x94, 0x96, 0x97, 0x99, 0x9a,
	  0x9b, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa5, 0xa5, 0xa6, 0xa7, 0xa7, 0xa8, 0xa9},
	{ 0x80, 0x82, 0x84, 0x86, 0x88, 0x8a, 0x8b, 0x8d, 0x8f, 0x90, 0x92, 0x94, 0x95, 0x97, 0x98, 0x99,
	  0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa4, 0xa4, 0xa5, 0xa6, 0xa6, 0xa7, 0xa8},
	{ 0x80, 0x82, 0x84, 0x86, 0x87, 0x89, 0x8b, 0x8d, 0x8e, 0x90, 0x91, 0x93, 0x94, 0x96, 0x97, 0x98,
	  0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa3, 0xa3, 0xa4, 0xa5, 0xa6, 0xa6, 0xa7},
	{ 0x80, 0x82, 0x84, 0x85, 0x87, 0x89, 0x8a, 0x8c, 0x8e, 0x8f, 0x91, 0x92, 0x94, 0x95, 0x96, 0x98,
	  0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4, 0xa5, 0xa5, 0xa6},
	{ 0x80, 0x82, 0x83, 0x85, 0x87, 0x88, 0x8a, 0x8c, 0x8d, 0x8f, 0x90, 0x92, 0x93, 0x94, 0x96, 0x97,
	  0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4, 0xa5, 0xa5},
	{ 0x80, 0x82, 0x83, 0x85, 0x86, 0x88, 0x8a, 0x8b, 0x8d, 0x8e, 0x90, 0x91, 0x92, 0x94, 0x95, 0x96,
	  0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4, 0xa4},
	{ 0x80, 0x82, 0x83, 0x85, 0x86, 0x88, 0x89, 0x8b, 0x8c, 0x8e, 0x8f, 0x90, 0x92, 0x93, 0x94, 0x95,
	  0x96, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9e, 0x9f, 0xa0, 0xa1, 0xa2, 0xa2, 0xa3, 0xa4},
	{ 0x80, 0x82, 0x83, 0x85, 0x86, 0x87, 0x89, 0x8a, 0x8c, 0x8d, 0x8e, 0x90, 0x91, 0x92, 0x93, 0x95,
	  0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9e, 0x9f, 0xa0, 0xa1, 0xa1, 0xa2, 0xa3},
	{ 0x80, 0x81, 0x83, 0x84, 0x86, 0x87, 0x89, 0x8a, 0x8b, 0x8d, 0x8e, 0x8f, 0x90, 0x92, 0x93, 0x94,
	  0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9e, 0x9f, 0xa0, 0xa1, 0xa1, 0xa2},
	{ 0x80, 0x81, 0x83, 0x84, 0x86, 0x87, 0x88, 0x8a, 0x8b, 0x8c, 0x8e, 0x8f, 0x90, 0x91, 0x92, 0x93,
	  0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9a, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0x9f, 0xa0, 0xa1, 0xa1},
	{ 0x80, 0x81, 0x83, 0x84, 0x85, 0x87, 0x88, 0x89, 0x8b, 0x8c, 0x8d, 0x8e, 0x90, 0x91, 0x92, 0x93,
	  0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9b, 0x9c, 0x9d, 0x9e, 0x9f, 0x9f, 0xa0, 0xa1},
	{ 0x80, 0x81, 0x83, 0x84, 0x85, 0x87, 0x88, 0x89, 0x8a, 0x8c, 0x8d, 0x8e, 0x8f, 0x90, 0x91, 0x92,
	  0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9a, 0x9b, 0x9c, 0x9c, 0x9d, 0x9e, 0x9f, 0x9f, 0xa0}};

short ST010_Sin(short Theta)
{
  return ST010_SinTable[(Theta >> 8) & 0xff];
}

short ST010_Cos(short Theta)
{
  return ST010_SinTable[((Theta + 0x4000) >> 8) & 0xff];
}

void ST010_OP01(short x0, short y0, short *x1, short *y1, short *Quadrant, short *Theta)
{
  if ((x0 < 0) && (y0 < 0))
  {
    *x1 = -x0;
    *y1 = -y0;
    *Quadrant = -0x8000;
  }
  else if (x0 < 0)
  {
    *x1 = y0;
    *y1 = -x0;
    *Quadrant = -0x4000;
  }
  else if (y0 < 0)
  {
    *x1 = -y0;
    *y1 = x0;
    *Quadrant = 0x4000;
  }
  else
  {
    *x1 = x0;
    *y1 = y0;
    *Quadrant = 0x0000;
  }

  while ((*x1 > 0x1f) || (*y1 > 0x1f))
  {
    if (*x1 > 1) *x1 >>= 1;
    if (*y1 > 1) *y1 >>= 1;
  }

  if (*y1 == 0) *Quadrant += 0x4000;

  *Theta = (ST010_ArcTan[*y1][*x1] << 8) ^ *Quadrant;
}

void ST010_Scale(short Multiplier, short X0, short Y0, int *X1, int *Y1)
{
  *X1 = X0 * Multiplier << 1;
  *Y1 = Y0 * Multiplier << 1;
}

void ST010_Multiply(short Multiplicand, short Multiplier, int *Product)
{
  *Product = Multiplicand * Multiplier << 1;
}

void ST010_Rotate(short Theta, short X0, short Y0, short *X1, short *Y1)
{
  *X1 = (Y0 * ST010_Sin(Theta) >> 15) + (X0 * ST010_Cos(Theta) >> 15);
  *Y1 = (Y0 * ST010_Cos(Theta) >> 15) - (X0 * ST010_Sin(Theta) >> 15);
}

void ST010_SortDrivers(uint16 Positions, uint16 Places[32], uint16 Drivers[32])
{
  bool Sorted;
  uint16 Temp;

  if (Positions > 1)
  {
    do
    {
      int i;
      Sorted = true;
      for (i = 0; i < Positions - 1; i++)
      {
        if (Places[i] < Places[i + 1])
        {
          Temp = Places[i + 1];
          Places[i + 1] = Places[i];
          Places[i] = Temp;

          Temp = Drivers[i + 1];
          Drivers[i + 1] = Drivers[i];
          Drivers[i] = Temp;

          Sorted = false;
        }
      }
      Positions--;
    } while (!Sorted);
  }
}

#define ST010_WORD(offset) (*((short *)(SRAM+offset)))
//#define ST010_WORD(offset) (SRAM[offset + 1] << 8) | SRAM[offset]

void ST010DoCommand(void)
{
  switch(SRAM[0x20])
  {
    /*
    Calculate track data based on direction coords

    Input
      0x0000-0x0001 : DX (signed)
      0x0002-0x0003 : DY (signed)
    Output
      0x0010-0x0011 : Angle (signed)