dsp1.cpp

著名SFC模拟器Snes9x的源代码。

				int32 x = (int16) DSP1.parameters [0];
				int32 y = (int16) DSP1.parameters [1];
				int32 z = (int16) DSP1.parameters [2];
				DSP1.output [0] = (uint16) sqrt (double(x * x + y * y + z * z));
				DSP1.out_count = 1;
				break;
			    }

			    case 0x0c:	// Rotate (2D rotate)
			    {
				DSP1_Rotate rotate ((int16) DSP1.parameters [0],
						    (int16) DSP1.parameters [1],
						    (int16) DSP1.parameters [2]);
				DSP1.out_count = 2;
				DSP1.output [0] = (uint16) rotate.x2;
				DSP1.output [1] = (uint16) rotate.y2;
				break;
			    }

			    case 0x1c:	// Polar (3D rotate)
			    {
				Op1CAZ = DSP1.parameters [0];
				Op1CAY = DSP1.parameters [1];
				Op1CAX = DSP1.parameters [2];
				Op1CX1 = DSP1.parameters [3];
				Op1CY1 = DSP1.parameters [4];
				Op1CZ1 = DSP1.parameters [5];
				DSPOp1C ();
				DSP1.out_count = 3;
				DSP1.output [0] = (uint16) Op1CX3;
				DSP1.output [1] = (uint16) Op1CY3;
				DSP1.output [2] = (uint16) Op1CZ3;
				break;
			    }

			    case 0x02:	// Parameter (Projection)
				    Op02FX = DSP1.parameters [0];
				    Op02FY = DSP1.parameters [1];
				    Op02FZ = DSP1.parameters [2];
				    Op02LFE = DSP1.parameters [3];
				    Op02LES = DSP1.parameters [4];
				    Op02AAS = DSP1.parameters [5];
				    Op02AZS = DSP1.parameters [6];

				    DSPOp02 ();
				    DSP1.out_count = 4;
				    DSP1.output [0] = Op02VOF;
				    DSP1.output [1] = Op02VVA;
				    DSP1.output [2] = Op02CX;
				    DSP1.output [3] = Op02CY;
				    break;

			    case 0x1a:	// Raster mode 7 matrix data
			    case 0x0a:
				    Op0AVS = DSP1.parameters [0];
				    DSPOp0A ();
				    DSP1.out_count = 4;
				    DSP1.output [0] = Op0AA;
				    DSP1.output [1] = Op0AB;
				    DSP1.output [2] = Op0AC;
				    DSP1.output [3] = Op0AD;
				    break;

			    case 0x06:	// Project object
				{
				    DSP1_Project Object ((int16) DSP1.parameters [0],
							 (int16) DSP1.parameters [1],
							 (int16) DSP1.parameters [2]);
			
				    DSP1.out_count = 3;
				    DSP1.output [0] = Object.H;
				    DSP1.output [1] = Object.V;
				    DSP1.output [2] = Object.M;
				    break;
				}
			    
			    case 0x0e:	// Target
				// XXX:
				DSP1.output [0] = 0;
				DSP1.output [1] = 0;
				DSP1.out_count = 2;
				break;

			    // Extra commands used by Pilot Wings
			    case 0x01: // Set attitude matrix A
				Op01m = (int16) DSP1.parameters [0];
				Op01Zr = (int16) DSP1.parameters [1];
				Op01Xr = (int16) DSP1.parameters [2];
				Op01Yr = (int16) DSP1.parameters [3];
				DSPOp01 ();
				break;

			    case 0x11:	// Set attitude matrix B
				Op01m = (int16) DSP1.parameters [0];
				Op01Zr = (int16) DSP1.parameters [1];
				Op01Xr = (int16) DSP1.parameters [2];
				Op01Yr = (int16) DSP1.parameters [3];
				DSPOp11 ();
				break;

			    case 0x21:	// Set attitude matrix C
				Op01m = (int16) DSP1.parameters [0];
				Op01Zr = (int16) DSP1.parameters [1];
				Op01Xr = (int16) DSP1.parameters [2];
				Op01Yr = (int16) DSP1.parameters [3];
				DSPOp21 ();
				break;

			    case 0x0d:	// Objective matrix A
				Op0DX = (int16) DSP1.parameters [0];
				Op0DY = (int16) DSP1.parameters [1];
				Op0DZ = (int16) DSP1.parameters [2];
				DSPOp0D ();
				DSP1.output [0] = (uint16) Op0DF;
				DSP1.output [1] = (uint16) Op0DL;
				DSP1.output [2] = (uint16) Op0DU;
				DSP1.out_count = 3;
				break;

			    case 0x1d:	// Objective matrix B
				Op0DX = (int16) DSP1.parameters [0];
				Op0DY = (int16) DSP1.parameters [1];
				Op0DZ = (int16) DSP1.parameters [2];
				DSPOp1D ();
				DSP1.output [0] = (uint16) Op0DF;
				DSP1.output [1] = (uint16) Op0DL;
				DSP1.output [2] = (uint16) Op0DU;
				DSP1.out_count = 3;
				break;

			    case 0x2d:	// Objective matrix C
				Op0DX = (int16) DSP1.parameters [0];
				Op0DY = (int16) DSP1.parameters [1];
				Op0DZ = (int16) DSP1.parameters [2];
				DSPOp2D ();
				DSP1.output [0] = (uint16) Op0DF;
				DSP1.output [1] = (uint16) Op0DL;
				DSP1.output [2] = (uint16) Op0DU;
				DSP1.out_count = 3;
				break;

			    case 0x03:	// Subjective matrix A
				Op03F = ((int16) DSP1.parameters [0] * 2);
				Op03L = ((int16) DSP1.parameters [1] * 2);
				Op03U = ((int16) DSP1.parameters [2] * 2);
				DSPOp03 ();
				DSP1.output [0] = (uint16) Op03X;
				DSP1.output [1] = (uint16) Op03Y;
				DSP1.output [2] = (uint16) Op03Z;
				DSP1.out_count = 3;
				break;

			    case 0x13:	// Subjective matrix B
				Op03F = ((int16) DSP1.parameters [0] * 2);
				Op03L = ((int16) DSP1.parameters [1] * 2);
				Op03U = ((int16) DSP1.parameters [2] * 2);
				DSPOp13 ();
				DSP1.output [0] = (uint16) Op03X;
				DSP1.output [1] = (uint16) Op03Y;
				DSP1.output [2] = (uint16) Op03Z;
				DSP1.out_count = 3;
				break;

			    case 0x23:	// Subjective matrix C
				Op03F = ((int16) DSP1.parameters [0] * 2);
				Op03L = ((int16) DSP1.parameters [1] * 2);
				Op03U = ((int16) DSP1.parameters [2] * 2);
				DSPOp23 ();
				DSP1.output [0] = (uint16) Op03X;
				DSP1.output [1] = (uint16) Op03Y;
				DSP1.output [2] = (uint16) Op03Z;
				DSP1.out_count = 3;
				break;

			    case 0x0b:
				// XXX
			    case 0x1b:
			    case 0x2b:
				DSP1.out_count = 1;
				break;

			    case 0x14:	// Gyrate
#if 0
				Op14Zr = (int16) DSP1.parameters [0];
				Op14Xr = (int16) DSP1.parameters [1];
				Op14Yr = (int16) DSP1.parameters [2];
				Op14U = (int16) DSP1.parameters [3];
				Op14F = (int16) DSP1.parameters [4];
				Op14L = (int16) DSP1.parameters [5];
				DSPOp14 ();
				DSP1.output [0] = (uint16) Op14Zrr;
				DSP1.output [1] = (uint16) Op14Xrr;
				DSP1.output [2] = (uint16) Op14Yrr;
				DSP1.out_count = 3;
				break;
#endif
#if 1
{
#define AXES_3D 3
    double		Angle[AXES_3D];
    double		Delta[AXES_3D];
    double		Result;
    int			j;

    for( j = 0; j < AXES_3D; j++ )
    {
	    Angle[j] = M_PI * (((double)(int16) DSP1.parameters [j]) / 32768.0);
    }
    for( j = 0; j < AXES_3D; j++ )
    {
	    Delta[j] = M_PI * (((double)(int16) DSP1.parameters [j + 3]) / 32768.0);
    }
    Result = Angle[0];
    Result += (1.0 / cos(Angle[1])) *
	    ((Delta[0] * cos(Angle[2])) - (Delta[1] * sin(Angle[2])));
    DSP1.output [0] = (uint16) (Result * M_PI / 32768.0);
    Result = Angle[1];
    Result += (Delta[0] * sin(Angle[2])) + (Delta[1] * cos(Angle[2]));
    DSP1.output [1] = (uint16) (Result * M_PI / 32768.0);
    Result = Angle[2];
    Result -= tan(Angle[1]) *
	    ((Delta[0] * cos(Angle[2])) + (Delta[1] * sin(Angle[2])));
    DSP1.output [2] = (uint16) (Result * M_PI / 32768.0);
    DSP1.out_count = 3;
    break;
}
#endif
#if 0
			    {
				DSP1_Gyrate gyrate (DSP1.parameters [0] >> 8,
						    DSP1.parameters [1] >> 8,
						    DSP1.parameters [2] >> 8,
						    DSP1.parameters [3] >> 8,
						    DSP1.parameters [4] >> 8,
						    DSP1.parameters [5] >> 8);
				DSP1.output [0] = gyrate.Z0 << 8;
				DSP1.output [1] = gyrate.X0 << 8;
				DSP1.output [2] = gyrate.Y0 << 8;
				DSP1.out_count = 3;
				break;
			    }
#endif

			    default:
			        break;
			}
		    }
		    else
			DSP1.in_index++;
		}
	    }
	}
    }
    SHOW_FUNC_END();
}

///////////////////////// DSP1 Calculation code /////////////////////////

// Convert DSP1 type angle to a radian
#define ANGLE_TO_RAD(angle) (double(angle) * M_PI / 128.0)

// Create a unit matrix
void UnitMatrix(MATRIX &m, double scale = 1.0)
{
    SHOW_FUNC_START();
    m[0][0] = scale; m[0][1] =   0.0; m[0][2] =   0.0;
    m[1][0] =   0.0; m[1][1] = scale; m[1][2] =   0.0;
    m[2][0] =   0.0; m[2][1] =   0.0; m[2][2] = scale;
    SHOW_FUNC_END();
}

// Rotate matrix around X axis with 'a' radians
// anti-clockwise, in a right handed coordinate system
void RotateX(MATRIX &m, double a)
{
    SHOW_FUNC_START();
#ifdef REVERSE_MATRIX_ROTATION
    double cx = double(cos(-a));
    double sx = double(sin(-a));
#else
    double cx = double(cos(a));
    double sx = double(sin(a));
#endif
    double m01 = m[0][1];
    double m11 = m[1][1];
    double m21 = m[2][1];
    m[0][1] =	    m01 * cx	+ m[0][2] * sx;
    m[0][2] =	m[0][2] * cx	-     m01 * sx;
    m[1][1] =	    m11 * cx	+ m[1][2] * sx;
    m[1][2] =	m[1][2] * cx	-     m11 * sx;
    m[2][1] =	    m21 * cx	+ m[2][2] * sx;
    m[2][2] =	m[2][2] * cx	-     m21 * sx;
    SHOW_FUNC_END();
}

// Rotate matrix around Y axis with 'a' radians
// anti-clockwise, in a right handed coordinate system
void RotateY(MATRIX &m, double a)
{
    SHOW_FUNC_START();
#ifdef REVERSE_MATRIX_ROTATION
    double cy = double(cos(-a));
    double sy = double(sin(-a));
#else
    double cy = double(cos(a));
    double sy = double(sin(a));
#endif
    double m00 = m[0][0];
    double m10 = m[1][0];
    double m20 = m[2][0];
    m[0][0] =	m00 * cy	- m[0][2] * sy;
    m[0][2] =	m00 * sy	+ m[0][2] * cy;
    m[1][0] =	m10 * cy	- m[1][2] * sy;
    m[1][2] =	m10 * sy	+ m[1][2] * cy;
    m[2][0] =	m20 * cy	- m[2][2] * sy;
    m[2][2] =	m20 * sy	+ m[2][2] * cy;
    SHOW_FUNC_END();
}

// Rotate matrix around Y axis with 'a' radians
// anti-clockwise, in a right handed coordinate system
void RotateZ(MATRIX &m, double a)
{
    SHOW_FUNC_START();
#ifdef REVERSE_MATRIX_ROTATION
    double cz = double(cos(-a));
    double sz = double(sin(-a));
#else
    double cz = double(cos(a));
    double sz = double(sin(a));
#endif
    double m00 = m[0][0];
    double m10 = m[1][0];
    double m20 = m[2][0];
    m[0][0] =	    m00 * cz	+ m[0][1] * sz;
    m[0][1] =	m[0][1] * cz	-     m00 * sz;
    m[1][0] =	    m10 * cz	+ m[1][1] * sz;
    m[1][1] =	m[1][1] * cz	-     m10 * sz;
    m[2][0] =	    m20 * cz	+ m[2][1] * sz;
    m[2][1] =	m[2][1] * cz	-     m20 * sz;
    SHOW_FUNC_END();
}

void RotateVector(VECTOR &v, MATRIX &m)
{
    SHOW_FUNC_START();
    double x = v[0];
    double y = v[1];
    double z = v[2];
    v[0] = x * m[0][0] + y * m[0][1] + z * m[0][2];
    v[1] = x * m[1][0] + y * m[1][1] + z * m[1][2];
    v[2] = x * m[2][0] + y * m[2][1] + z * m[2][2];
    SHOW_FUNC_END();
}

void TransposeRotateVector(VECTOR &v, MATRIX &m)
{
    SHOW_FUNC_START();
    double x = v[0];
    double y = v[1];
    double z = v[2];
    v[0] = x * m[0][0] + y * m[1][0] + z * m[2][0];
    v[1] = x * m[0][1] + y * m[1][1] + z * m[2][1];
    v[2] = x * m[0][2] + y * m[1][2] + z * m[2][2];
    SHOW_FUNC_END();
}

void Normalize(VECTOR &v)
{
    SHOW_FUNC_START();
    double l = Length(v);
    if(l != 0.0)
    {
	v[0] /= l;
	v[1] /= l;
	v[2] /= l;
    }
    SHOW_FUNC_END();
}

////////////////////////////////////////////////////////////////////////////////////

// This will not work if the screen coordinate specified is above the horizon
void SDSP1::ScreenToGround(VECTOR &v, double X2d, double Y2d)
{
    SHOW_FUNC_START();
    // Calculate screen ray vector
    VECTOR vRay;
    vRay[0] = X2d;
    vRay[1] = Y2d;
    vRay[2] = vFov;
    Normalize(vRay);

    // Calculate the distance from the view point to the ground at specified screen coordinate
    // (If dist value is negative, screen coordinate is above the horizon)
    double div = vRay[0]*vM[0][2] + vRay[1]*vM[1][2] + vRay[2]*vM[2][2];
    double dist = (div != 0.0) ? (vPlaneD / div) : 0.0;
    
    // Calculate ground coordinate
    v[0] = vM[2][0] * dist + vT[0];
    v[1] = vM[2][1] * dist + vT[1];
    v[2] = vM[2][2] * dist + vT[2];
    SHOW_FUNC_END();
}

MATRIX &SDSP1::GetMatrix( AttitudeMatrix Matrix )
{
    SHOW_FUNC_START();
    switch(Matrix)
    {
	default:
	case MatrixA:
	    SHOW_FUNC_END();
	    return vMa;
	case MatrixB:
	    SHOW_FUNC_END();
	    return vMb;
	case MatrixC:
	    SHOW_FUNC_END();
	    return vMc;
    }
    SHOW_FUNC_END();
}

////////////////////////////////////////////////////////////////////

DSP1_Parameter::DSP1_Parameter( int16 Fx, int16 Fy, int16 Fz,
				uint16 Lfe, uint16 Les,
				int8 Aas, int8 Azs )
{
    SHOW_FUNC_START();
    MATRIX &vM = DSP1.vM;
    VECTOR &vT = DSP1.vT;

    // Set focal distance
    DSP1.vFov = double(Les);
    
    // Create matrix.
    // (This can be done a lot faster,
    // but I just want it to work first)
    UnitMatrix(vM);