aboutsummaryrefslogtreecommitdiff
path: root/source/dsp1emu.c
diff options
context:
space:
mode:
Diffstat (limited to 'source/dsp1emu.c')
-rw-r--r--source/dsp1emu.c453
1 files changed, 181 insertions, 272 deletions
diff --git a/source/dsp1emu.c b/source/dsp1emu.c
index a2767b0..3e34795 100644
--- a/source/dsp1emu.c
+++ b/source/dsp1emu.c
@@ -6,9 +6,6 @@
#include <string.h>
#include <stdlib.h>
-#define __OPT__
-#define __OPT06__
-
const uint16_t DSP1ROM[1024] =
{
0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000, 0x0000,
@@ -142,46 +139,9 @@ const uint16_t DSP1ROM[1024] =
};
/***************************************************************************\
-* Math tables *
-\***************************************************************************/
-
-#define INCR 2048
-#define Angle(x) (((x)/(65536/INCR)) & (INCR-1))
-#define Cos(x) ((double) CosTable2[x])
-#define Sin(x) ((double) SinTable2[x])
-#ifdef PI
-#undef PI
-#endif
-#define PI 3.1415926535897932384626433832795
-double CosTable2[INCR];
-double SinTable2[INCR];
-
-
-double Atan(double x)
-{
- if ((x >= 1) || (x <= 1))
- return (x / (1 + 0.28 * x * x));
- else
- return (PI / 2 - Atan(1 / x));
-}
-
-/***************************************************************************\
* DSP1 code *
\***************************************************************************/
-void InitDSP(void)
-{
-#ifdef __OPT__
- uint32_t i;
- for (i = 0; i < INCR; i++)
- {
- CosTable2[i] = (cos((double)(2 * PI * i / INCR)));
- SinTable2[i] = (sin((double)(2 * PI * i / INCR)));
- }
-#endif
-}
-
-
int16_t Op00Multiplicand;
int16_t Op00Multiplier;
int16_t Op00Result;
@@ -189,7 +149,6 @@ int16_t Op00Result;
void DSPOp00()
{
Op00Result = Op00Multiplicand * Op00Multiplier >> 15;
-
}
int16_t Op20Multiplicand;
@@ -200,7 +159,6 @@ void DSPOp20()
{
Op20Result = Op20Multiplicand * Op20Multiplier >> 15;
Op20Result++;
-
}
int16_t Op10Coefficient;
@@ -208,8 +166,7 @@ int16_t Op10Exponent;
int16_t Op10CoefficientR;
int16_t Op10ExponentR;
-void DSP1_Inverse(int16_t Coefficient, int16_t Exponent, int16_t* iCoefficient,
- int16_t* iExponent)
+void DSP1_Inverse(int16_t Coefficient, int16_t Exponent, int16_t* iCoefficient, int16_t* iExponent)
{
// Step One: Division by Zero
if (Coefficient == 0x0000)
@@ -308,22 +265,22 @@ const int16_t DSP1_MulTable[256] =
const int16_t DSP1_SinTable[256] =
{
- 0x0000, 0x0324, 0x0647, 0x096a, 0x0c8b, 0x0fab, 0x12c8, 0x15e2,
- 0x18f8, 0x1c0b, 0x1f19, 0x2223, 0x2528, 0x2826, 0x2b1f, 0x2e11,
- 0x30fb, 0x33de, 0x36ba, 0x398c, 0x3c56, 0x3f17, 0x41ce, 0x447a,
- 0x471c, 0x49b4, 0x4c3f, 0x4ebf, 0x5133, 0x539b, 0x55f5, 0x5842,
- 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e8, 0x66cf, 0x68a6,
- 0x6a6d, 0x6c24, 0x6dca, 0x6f5f, 0x70e2, 0x7255, 0x73b5, 0x7504,
- 0x7641, 0x776c, 0x7884, 0x798a, 0x7a7d, 0x7b5d, 0x7c29, 0x7ce3,
- 0x7d8a, 0x7e1d, 0x7e9d, 0x7f09, 0x7f62, 0x7fa7, 0x7fd8, 0x7ff6,
- 0x7fff, 0x7ff6, 0x7fd8, 0x7fa7, 0x7f62, 0x7f09, 0x7e9d, 0x7e1d,
- 0x7d8a, 0x7ce3, 0x7c29, 0x7b5d, 0x7a7d, 0x798a, 0x7884, 0x776c,
- 0x7641, 0x7504, 0x73b5, 0x7255, 0x70e2, 0x6f5f, 0x6dca, 0x6c24,
- 0x6a6d, 0x68a6, 0x66cf, 0x64e8, 0x62f2, 0x60ec, 0x5ed7, 0x5cb4,
- 0x5a82, 0x5842, 0x55f5, 0x539b, 0x5133, 0x4ebf, 0x4c3f, 0x49b4,
- 0x471c, 0x447a, 0x41ce, 0x3f17, 0x3c56, 0x398c, 0x36ba, 0x33de,
- 0x30fb, 0x2e11, 0x2b1f, 0x2826, 0x2528, 0x2223, 0x1f19, 0x1c0b,
- 0x18f8, 0x15e2, 0x12c8, 0x0fab, 0x0c8b, 0x096a, 0x0647, 0x0324,
+ 0x0000, 0x0324, 0x0647, 0x096a, 0x0c8b, 0x0fab, 0x12c8, 0x15e2,
+ 0x18f8, 0x1c0b, 0x1f19, 0x2223, 0x2528, 0x2826, 0x2b1f, 0x2e11,
+ 0x30fb, 0x33de, 0x36ba, 0x398c, 0x3c56, 0x3f17, 0x41ce, 0x447a,
+ 0x471c, 0x49b4, 0x4c3f, 0x4ebf, 0x5133, 0x539b, 0x55f5, 0x5842,
+ 0x5a82, 0x5cb4, 0x5ed7, 0x60ec, 0x62f2, 0x64e8, 0x66cf, 0x68a6,
+ 0x6a6d, 0x6c24, 0x6dca, 0x6f5f, 0x70e2, 0x7255, 0x73b5, 0x7504,
+ 0x7641, 0x776c, 0x7884, 0x798a, 0x7a7d, 0x7b5d, 0x7c29, 0x7ce3,
+ 0x7d8a, 0x7e1d, 0x7e9d, 0x7f09, 0x7f62, 0x7fa7, 0x7fd8, 0x7ff6,
+ 0x7fff, 0x7ff6, 0x7fd8, 0x7fa7, 0x7f62, 0x7f09, 0x7e9d, 0x7e1d,
+ 0x7d8a, 0x7ce3, 0x7c29, 0x7b5d, 0x7a7d, 0x798a, 0x7884, 0x776c,
+ 0x7641, 0x7504, 0x73b5, 0x7255, 0x70e2, 0x6f5f, 0x6dca, 0x6c24,
+ 0x6a6d, 0x68a6, 0x66cf, 0x64e8, 0x62f2, 0x60ec, 0x5ed7, 0x5cb4,
+ 0x5a82, 0x5842, 0x55f5, 0x539b, 0x5133, 0x4ebf, 0x4c3f, 0x49b4,
+ 0x471c, 0x447a, 0x41ce, 0x3f17, 0x3c56, 0x398c, 0x36ba, 0x33de,
+ 0x30fb, 0x2e11, 0x2b1f, 0x2826, 0x2528, 0x2223, 0x1f19, 0x1c0b,
+ 0x18f8, 0x15e2, 0x12c8, 0x0fab, 0x0c8b, 0x096a, 0x0647, 0x0324,
-0x0000, -0x0324, -0x0647, -0x096a, -0x0c8b, -0x0fab, -0x12c8, -0x15e2,
-0x18f8, -0x1c0b, -0x1f19, -0x2223, -0x2528, -0x2826, -0x2b1f, -0x2e11,
-0x30fb, -0x33de, -0x36ba, -0x398c, -0x3c56, -0x3f17, -0x41ce, -0x447a,
@@ -349,8 +306,7 @@ int16_t DSP1_Sin(int16_t Angle)
if (Angle == -32768) return 0;
return -DSP1_Sin(-Angle);
}
- int32_t S = DSP1_SinTable[Angle >> 8] + (DSP1_MulTable[Angle & 0xff] *
- DSP1_SinTable[0x40 + (Angle >> 8)] >> 15);
+ int32_t S = DSP1_SinTable[Angle >> 8] + (DSP1_MulTable[Angle & 0xff] * DSP1_SinTable[0x40 + (Angle >> 8)] >> 15);
if (S > 32767) S = 32767;
return (int16_t) S;
}
@@ -362,8 +318,7 @@ int16_t DSP1_Cos(int16_t Angle)
if (Angle == -32768) return -32768;
Angle = -Angle;
}
- int32_t S = DSP1_SinTable[0x40 + (Angle >> 8)] - (DSP1_MulTable[Angle & 0xff] *
- DSP1_SinTable[Angle >> 8] >> 15);
+ int32_t S = DSP1_SinTable[0x40 + (Angle >> 8)] - (DSP1_MulTable[Angle & 0xff] * DSP1_SinTable[Angle >> 8] >> 15);
if (S < -32768) S = -32767;
return (int16_t) S;
}
@@ -502,50 +457,75 @@ int16_t SecAZS_E1;
int16_t SecAZS_C2;
int16_t SecAZS_E2;
+int16_t Nx, Ny, Nz;
+int16_t Gx, Gy, Gz;
+int16_t C_Les, E_Les, G_Les;
+
const int16_t MaxAZS_Exp[16] =
{
0x38b4, 0x38b7, 0x38ba, 0x38be, 0x38c0, 0x38c4, 0x38c7, 0x38ca,
0x38ce, 0x38d0, 0x38d4, 0x38d7, 0x38da, 0x38dd, 0x38e0, 0x38e4
};
-void DSP1_Parameter(int16_t Fx, int16_t Fy, int16_t Fz, int16_t Lfe, int16_t Les,
- int16_t Aas, int16_t Azs, int16_t* Vof, int16_t* Vva, int16_t* Cx, int16_t* Cy)
+void DSP1_Parameter(int16_t Fx, int16_t Fy, int16_t Fz, int16_t Lfe, int16_t Les, int16_t Aas, int16_t Azs, int16_t* Vof, int16_t* Vva, int16_t* Cx, int16_t* Cy)
{
- int16_t CSec, C, E;
+ int16_t CSec, C, E, MaxAZS, Aux;
+ int16_t LfeNx, LfeNy, LfeNz;
+ int16_t LesNx, LesNy, LesNz;
+ int16_t CentreZ;
// Copy Zenith angle for clipping
int16_t AZS = Azs;
- // Store Sin and Cos of Azimuth and Zenith angles
+ // Store Sine and Cosine of Azimuth and Zenith angle
SinAas = DSP1_Sin(Aas);
CosAas = DSP1_Cos(Aas);
SinAzs = DSP1_Sin(Azs);
CosAzs = DSP1_Cos(Azs);
+ Nx = SinAzs * -SinAas >> 15;
+ Ny = SinAzs * CosAas >> 15;
+ Nz = CosAzs * 0x7fff >> 15;
+
+ LfeNx = Lfe * Nx >> 15;
+ LfeNy = Lfe * Ny >> 15;
+ LfeNz = Lfe * Nz >> 15;
+
// Center of Projection
- CentreX = Fx + (Lfe * (SinAzs * -SinAas >> 15) >> 15);
- CentreY = Fy + (Lfe * (SinAzs * CosAas >> 15) >> 15);
+ CentreX = Fx + LfeNx;
+ CentreY = Fy + LfeNy;
+ CentreZ = Fz + LfeNz;
+
+ LesNx = Les * Nx >> 15;
+ LesNy = Les * Ny >> 15;
+ LesNz = Les * Nz >> 15;
+
+ Gx = CentreX - LesNx;
+ Gy = CentreY - LesNy;
+ Gz = CentreZ - LesNz;
+
+ E_Les=0;
+ DSP1_Normalize(Les, &C_Les, &E_Les);
+ G_Les = Les;
E = 0;
- DSP1_Normalize(Fz + (Lfe * (CosAzs * 0x7fff >> 15) >> 15), &C, &E);
+ DSP1_Normalize(CentreZ, &C, &E);
VPlane_C = C;
VPlane_E = E;
// Determine clip boundary and clip Zenith angle if necessary
- int16_t MaxAZS = MaxAZS_Exp[-E];
+ MaxAZS = MaxAZS_Exp[-E];
if (AZS < 0)
{
MaxAZS = -MaxAZS;
if (AZS < MaxAZS + 1) AZS = MaxAZS + 1;
}
- else
- {
- if (AZS > MaxAZS) AZS = MaxAZS;
- }
+ else if (AZS > MaxAZS)
+ AZS = MaxAZS;
- // Store Sin and Cos of clipped Zenith angle
+ // Store Sine and Cosine of clipped Zenith angle
SinAZS = DSP1_Sin(AZS);
CosAZS = DSP1_Cos(AZS);
@@ -570,7 +550,7 @@ void DSP1_Parameter(int16_t Fx, int16_t Fy, int16_t Fz, int16_t Lfe, int16_t Les
C = Azs - MaxAZS;
if (C >= 0) C--;
- int16_t Aux = ~(C << 2);
+ Aux = ~(C << 2);
C = Aux * DSP1ROM[0x0328] >> 15;
C = (C * Aux >> 15) + DSP1ROM[0x0327];
@@ -595,7 +575,7 @@ void DSP1_Parameter(int16_t Fx, int16_t Fy, int16_t Fz, int16_t Lfe, int16_t Les
*Vva = DSP1_Truncate(-C, E);
- // Store Sec of clipped Zenith angle
+ // Store Secant of clipped Zenith angle
DSP1_Inverse(CosAZS, 0, &SecAZS_C2, &SecAZS_E2);
}
@@ -638,8 +618,7 @@ int16_t Op02CY;
void DSPOp02()
{
- DSP1_Parameter(Op02FX, Op02FY, Op02FZ, Op02LFE, Op02LES, Op02AAS, Op02AZS,
- &Op02VOF, &Op02VVA, &Op02CX, &Op02CY);
+ DSP1_Parameter(Op02FX, Op02FY, Op02FZ, Op02LFE, Op02LES, Op02AAS, Op02AZS, &Op02VOF, &Op02VVA, &Op02CX, &Op02CY);
}
int16_t Op0AVS;
@@ -654,115 +633,96 @@ void DSPOp0A()
Op0AVS++;
}
+int16_t DSP1_ShiftR(int16_t C, int16_t E)
+{
+ return (C * DSP1ROM[0x0031 + E] >> 15);
+}
+
+void DSP1_Project(int16_t X, int16_t Y, int16_t Z, int16_t *H, int16_t *V, int16_t *M)
+{
+ int32_t aux, aux4;
+ int16_t E, E2, E3, E4, E5, refE, E6, E7;
+ int16_t C2, C4, C6, C8, C9, C10, C11, C12, C16, C17, C18, C19, C20, C21, C22, C23, C24, C25, C26;
+ int16_t Px, Py, Pz;
+
+ E4 = E3 = E2 = E = E5 = 0;
+
+ DSP1_NormalizeDouble((int32_t) X - Gx, &Px, &E4);
+ DSP1_NormalizeDouble((int32_t) Y - Gy, &Py, &E);
+ DSP1_NormalizeDouble((int32_t) Z - Gz, &Pz, &E3);
+ Px>>=1;
+ E4--; // to avoid overflows when calculating the scalar products
+ Py>>=1;
+ E--;
+ Pz>>=1;
+ E3--;
+
+ refE = MIN(E, E3);
+ refE = MIN(refE, E4);
+
+ Px = DSP1_ShiftR(Px, E4 - refE); // normalize them to the same exponent
+ Py = DSP1_ShiftR(Py, E - refE);
+ Pz = DSP1_ShiftR(Pz, E3 - refE);
+
+ C11 = -(Px * Nx >> 15);
+ C8 = -(Py * Ny >> 15);
+ C9 = -(Pz * Nz >> 15);
+ C12 = C11 + C8 + C9; // this cannot overflow!
+
+ aux4 = C12; // de-normalization with 32-bit arithmetic
+ refE = 16 - refE; // refE can be up to 3
+ if (refE >= 0)
+ aux4 <<= refE;
+ else
+ aux4 >>= -refE;
+ if (aux4 == -1)
+ aux4 = 0; // why?
+ aux4>>=1;
+
+ aux = ((int16_t) G_Les) + aux4; // Les - the scalar product of P with the normal vector of the screen
+ DSP1_NormalizeDouble(aux, &C10, &E2);
+ E2 = 15 - E2;
+
+ DSP1_Inverse(C10, 0, &C4, &E4);
+ C2 = C4 * C_Les >> 15; // scale factor
+
+ // H
+ E7 = 0;
+ C16 = (Px * (CosAas * 0x7fff >> 15) >> 15);
+ C20 = (Py * (SinAas * 0x7fff >> 15) >> 15);
+ C17 = C16 + C20; // scalar product of P with the normalized horizontal vector of the screen...
+
+ C18 = C17 * C2 >> 15; // ... multiplied by the scale factor
+ DSP1_Normalize(C18, &C19, &E7);
+ *H = DSP1_Truncate(C19, E_Les - E2 + refE + E7);
+
+ // V
+ E6 = 0;
+ C21 = Px * (CosAzs * -SinAas >> 15) >> 15;
+ C22 = Py * (CosAzs * CosAas >> 15) >> 15;
+ C23 = Pz * (-SinAzs * 0x7fff >> 15) >> 15;
+ C24 = C21 + C22 + C23; // scalar product of P with the normalized vertical vector of the screen...
+
+ C26 = C24 * C2 >> 15; // ... multiplied by the scale factor
+ DSP1_Normalize(C26, &C25, &E6);
+ *V = DSP1_Truncate(C25, E_Les - E2 + refE + E6);
+
+ // M
+ DSP1_Normalize(C2, &C6, &E4);
+ *M = DSP1_Truncate(C6, E4 + E_Les - E2 - 7); // M is the scale factor divided by 2^7
+}
+
int16_t Op06X;
int16_t Op06Y;
int16_t Op06Z;
int16_t Op06H;
int16_t Op06V;
-uint16_t Op06S;
-
-double ObjPX;
-double ObjPY;
-double ObjPZ;
-double ObjPX1;
-double ObjPY1;
-double ObjPZ1;
-double ObjPX2;
-double ObjPY2;
-double ObjPZ2;
-double DivideOp06;
-int32_t Temp;
-int32_t tanval2;
-
-#ifdef __OPT06__
-void DSPOp06()
-{
- ObjPX = Op06X - Op02FX;
- ObjPY = Op06Y - Op02FY;
- ObjPZ = Op06Z - Op02FZ;
-
- // rotate around Z
- tanval2 = Angle(-Op02AAS + 32768);
- ObjPX1 = (ObjPX * Cos(tanval2) + ObjPY * -Sin(tanval2));
- ObjPY1 = (ObjPX * Sin(tanval2) + ObjPY * Cos(tanval2));
- ObjPZ1 = ObjPZ;
-
- // rotate around X
- tanval2 = Angle(-Op02AZS);
- ObjPX2 = ObjPX1;
- ObjPY2 = (ObjPY1 * Cos(tanval2) + ObjPZ1 * -Sin(tanval2));
- ObjPZ2 = (ObjPY1 * Sin(tanval2) + ObjPZ1 * Cos(tanval2));
-
- ObjPZ2 = ObjPZ2 - Op02LFE;
-
- if (ObjPZ2 < 0)
- {
- double d;
- Op06H = (int16_t)(-ObjPX2 * Op02LES / -(ObjPZ2));
- Op06V = (int16_t)(-ObjPY2 * Op02LES / -(ObjPZ2));
- d = (double)Op02LES;
- d *= 256.0;
- d /= (-ObjPZ2);
- if (d > 65535.0)
- d = 65535.0;
- else if (d < 0.0)
- d = 0.0;
- Op06S = (uint16_t)d;
- }
- else
- {
- Op06H = 0;
- Op06V = 14 * 16;
- Op06S = 0xFFFF;
- }
-
-
-}
-#else
+int16_t Op06M;
void DSPOp06()
{
- ObjPX = Op06X - Op02FX;
- ObjPY = Op06Y - Op02FY;
- ObjPZ = Op06Z - Op02FZ;
-
- // rotate around Z
- tanval = (-Op02AAS + 32768) / 65536.0 * 6.2832;
- ObjPX1 = (ObjPX * cos(tanval) + ObjPY * -sin(tanval));
- ObjPY1 = (ObjPX * sin(tanval) + ObjPY * cos(tanval));
- ObjPZ1 = ObjPZ;
-
- // rotate around X
- tanval = (-Op02AZS) / 65536.0 * 6.2832;
- ObjPX2 = ObjPX1;
- ObjPY2 = (ObjPY1 * cos(tanval) + ObjPZ1 * -sin(tanval));
- ObjPZ2 = (ObjPY1 * sin(tanval) + ObjPZ1 * cos(tanval));
-
- ObjPZ2 = ObjPZ2 - Op02LFE;
-
- if (ObjPZ2 < 0)
- {
- Op06H = (int16_t)(-ObjPX2 * Op02LES / -(ObjPZ2));
- Op06V = (int16_t)(-ObjPY2 * Op02LES / -(ObjPZ2));
- double d = (double)Op02LES;
- d *= 256.0;
- d /= (-ObjPZ2);
- if (d > 65535.0)
- d = 65535.0;
- else if (d < 0.0)
- d = 0.0;
- Op06S = (uint16_t)d;
- }
- else
- {
- Op06H = 0;
- Op06V = 14 * 16;
- Op06S = 0xFFFF;
- }
-
+ DSP1_Project(Op06X, Op06Y, Op06Z, &Op06H, &Op06V, &Op06M);
}
-#endif
-
int16_t matrixC[3][3];
int16_t matrixB[3][3];
@@ -796,16 +756,12 @@ void DSPOp01()
matrixA[0][1] = -((Op01m * SinAz >> 15) * CosAy >> 15);
matrixA[0][2] = Op01m * SinAy >> 15;
- matrixA[1][0] = ((Op01m * SinAz >> 15) * CosAx >> 15) + (((
- Op01m * CosAz >> 15) * SinAx >> 15) * SinAy >> 15);
- matrixA[1][1] = ((Op01m * CosAz >> 15) * CosAx >> 15) - (((
- Op01m * SinAz >> 15) * SinAx >> 15) * SinAy >> 15);
+ matrixA[1][0] = ((Op01m * SinAz >> 15) * CosAx >> 15) + (((Op01m * CosAz >> 15) * SinAx >> 15) * SinAy >> 15);
+ matrixA[1][1] = ((Op01m * CosAz >> 15) * CosAx >> 15) - (((Op01m * SinAz >> 15) * SinAx >> 15) * SinAy >> 15);
matrixA[1][2] = -((Op01m * SinAx >> 15) * CosAy >> 15);
- matrixA[2][0] = ((Op01m * SinAz >> 15) * SinAx >> 15) - (((
- Op01m * CosAz >> 15) * CosAx >> 15) * SinAy >> 15);
- matrixA[2][1] = ((Op01m * CosAz >> 15) * SinAx >> 15) + (((
- Op01m * SinAz >> 15) * CosAx >> 15) * SinAy >> 15);
+ matrixA[2][0] = ((Op01m * SinAz >> 15) * SinAx >> 15) - (((Op01m * CosAz >> 15) * CosAx >> 15) * SinAy >> 15);
+ matrixA[2][1] = ((Op01m * CosAz >> 15) * SinAx >> 15) + (((Op01m * SinAz >> 15) * CosAx >> 15) * SinAy >> 15);
matrixA[2][2] = (Op01m * CosAx >> 15) * CosAy >> 15;
}
@@ -824,16 +780,12 @@ void DSPOp11()
matrixB[0][1] = -((Op11m * SinAz >> 15) * CosAy >> 15);
matrixB[0][2] = Op11m * SinAy >> 15;
- matrixB[1][0] = ((Op11m * SinAz >> 15) * CosAx >> 15) + (((
- Op11m * CosAz >> 15) * SinAx >> 15) * SinAy >> 15);
- matrixB[1][1] = ((Op11m * CosAz >> 15) * CosAx >> 15) - (((
- Op11m * SinAz >> 15) * SinAx >> 15) * SinAy >> 15);
+ matrixB[1][0] = ((Op11m * SinAz >> 15) * CosAx >> 15) + (((Op11m * CosAz >> 15) * SinAx >> 15) * SinAy >> 15);
+ matrixB[1][1] = ((Op11m * CosAz >> 15) * CosAx >> 15) - (((Op11m * SinAz >> 15) * SinAx >> 15) * SinAy >> 15);
matrixB[1][2] = -((Op11m * SinAx >> 15) * CosAy >> 15);
- matrixB[2][0] = ((Op11m * SinAz >> 15) * SinAx >> 15) - (((
- Op11m * CosAz >> 15) * CosAx >> 15) * SinAy >> 15);
- matrixB[2][1] = ((Op11m * CosAz >> 15) * SinAx >> 15) + (((
- Op11m * SinAz >> 15) * CosAx >> 15) * SinAy >> 15);
+ matrixB[2][0] = ((Op11m * SinAz >> 15) * SinAx >> 15) - (((Op11m * CosAz >> 15) * CosAx >> 15) * SinAy >> 15);
+ matrixB[2][1] = ((Op11m * CosAz >> 15) * SinAx >> 15) + (((Op11m * SinAz >> 15) * CosAx >> 15) * SinAy >> 15);
matrixB[2][2] = (Op11m * CosAx >> 15) * CosAy >> 15;
}
@@ -852,16 +804,12 @@ void DSPOp21()
matrixC[0][1] = -((Op21m * SinAz >> 15) * CosAy >> 15);
matrixC[0][2] = Op21m * SinAy >> 15;
- matrixC[1][0] = ((Op21m * SinAz >> 15) * CosAx >> 15) + (((
- Op21m * CosAz >> 15) * SinAx >> 15) * SinAy >> 15);
- matrixC[1][1] = ((Op21m * CosAz >> 15) * CosAx >> 15) - (((
- Op21m * SinAz >> 15) * SinAx >> 15) * SinAy >> 15);
+ matrixC[1][0] = ((Op21m * SinAz >> 15) * CosAx >> 15) + (((Op21m * CosAz >> 15) * SinAx >> 15) * SinAy >> 15);
+ matrixC[1][1] = ((Op21m * CosAz >> 15) * CosAx >> 15) - (((Op21m * SinAz >> 15) * SinAx >> 15) * SinAy >> 15);
matrixC[1][2] = -((Op21m * SinAx >> 15) * CosAy >> 15);
- matrixC[2][0] = ((Op21m * SinAz >> 15) * SinAx >> 15) - (((
- Op21m * CosAz >> 15) * CosAx >> 15) * SinAy >> 15);
- matrixC[2][1] = ((Op21m * CosAz >> 15) * SinAx >> 15) + (((
- Op21m * SinAz >> 15) * CosAx >> 15) * SinAy >> 15);
+ matrixC[2][0] = ((Op21m * SinAz >> 15) * SinAx >> 15) - (((Op21m * CosAz >> 15) * CosAx >> 15) * SinAy >> 15);
+ matrixC[2][1] = ((Op21m * CosAz >> 15) * SinAx >> 15) + (((Op21m * SinAz >> 15) * CosAx >> 15) * SinAy >> 15);
matrixC[2][2] = (Op21m * CosAx >> 15) * CosAy >> 15;
}
@@ -886,35 +834,23 @@ int16_t Op2DU;
void DSPOp0D()
{
- Op0DF = (Op0DX * matrixA[0][0] >> 15) + (Op0DY * matrixA[0][1] >> 15) +
- (Op0DZ * matrixA[0][2] >> 15);
- Op0DL = (Op0DX * matrixA[1][0] >> 15) + (Op0DY * matrixA[1][1] >> 15) +
- (Op0DZ * matrixA[1][2] >> 15);
- Op0DU = (Op0DX * matrixA[2][0] >> 15) + (Op0DY * matrixA[2][1] >> 15) +
- (Op0DZ * matrixA[2][2] >> 15);
-
+ Op0DF = (Op0DX * matrixA[0][0] >> 15) + (Op0DY * matrixA[0][1] >> 15) + (Op0DZ * matrixA[0][2] >> 15);
+ Op0DL = (Op0DX * matrixA[1][0] >> 15) + (Op0DY * matrixA[1][1] >> 15) + (Op0DZ * matrixA[1][2] >> 15);
+ Op0DU = (Op0DX * matrixA[2][0] >> 15) + (Op0DY * matrixA[2][1] >> 15) + (Op0DZ * matrixA[2][2] >> 15);
}
void DSPOp1D()
{
- Op1DF = (Op1DX * matrixB[0][0] >> 15) + (Op1DY * matrixB[0][1] >> 15) +
- (Op1DZ * matrixB[0][2] >> 15);
- Op1DL = (Op1DX * matrixB[1][0] >> 15) + (Op1DY * matrixB[1][1] >> 15) +
- (Op1DZ * matrixB[1][2] >> 15);
- Op1DU = (Op1DX * matrixB[2][0] >> 15) + (Op1DY * matrixB[2][1] >> 15) +
- (Op1DZ * matrixB[2][2] >> 15);
-
+ Op1DF = (Op1DX * matrixB[0][0] >> 15) + (Op1DY * matrixB[0][1] >> 15) + (Op1DZ * matrixB[0][2] >> 15);
+ Op1DL = (Op1DX * matrixB[1][0] >> 15) + (Op1DY * matrixB[1][1] >> 15) + (Op1DZ * matrixB[1][2] >> 15);
+ Op1DU = (Op1DX * matrixB[2][0] >> 15) + (Op1DY * matrixB[2][1] >> 15) + (Op1DZ * matrixB[2][2] >> 15);
}
void DSPOp2D()
{
- Op2DF = (Op2DX * matrixC[0][0] >> 15) + (Op2DY * matrixC[0][1] >> 15) +
- (Op2DZ * matrixC[0][2] >> 15);
- Op2DL = (Op2DX * matrixC[1][0] >> 15) + (Op2DY * matrixC[1][1] >> 15) +
- (Op2DZ * matrixC[1][2] >> 15);
- Op2DU = (Op2DX * matrixC[2][0] >> 15) + (Op2DY * matrixC[2][1] >> 15) +
- (Op2DZ * matrixC[2][2] >> 15);
-
+ Op2DF = (Op2DX * matrixC[0][0] >> 15) + (Op2DY * matrixC[0][1] >> 15) + (Op2DZ * matrixC[0][2] >> 15);
+ Op2DL = (Op2DX * matrixC[1][0] >> 15) + (Op2DY * matrixC[1][1] >> 15) + (Op2DZ * matrixC[1][2] >> 15);
+ Op2DU = (Op2DX * matrixC[2][0] >> 15) + (Op2DY * matrixC[2][1] >> 15) + (Op2DZ * matrixC[2][2] >> 15);
}
int16_t Op03F;
@@ -938,35 +874,23 @@ int16_t Op23Z;
void DSPOp03()
{
- Op03X = (Op03F * matrixA[0][0] >> 15) + (Op03L * matrixA[1][0] >> 15) +
- (Op03U * matrixA[2][0] >> 15);
- Op03Y = (Op03F * matrixA[0][1] >> 15) + (Op03L * matrixA[1][1] >> 15) +
- (Op03U * matrixA[2][1] >> 15);
- Op03Z = (Op03F * matrixA[0][2] >> 15) + (Op03L * matrixA[1][2] >> 15) +
- (Op03U * matrixA[2][2] >> 15);
-
+ Op03X = (Op03F * matrixA[0][0] >> 15) + (Op03L * matrixA[1][0] >> 15) + (Op03U * matrixA[2][0] >> 15);
+ Op03Y = (Op03F * matrixA[0][1] >> 15) + (Op03L * matrixA[1][1] >> 15) + (Op03U * matrixA[2][1] >> 15);
+ Op03Z = (Op03F * matrixA[0][2] >> 15) + (Op03L * matrixA[1][2] >> 15) + (Op03U * matrixA[2][2] >> 15);
}
void DSPOp13()
{
- Op13X = (Op13F * matrixB[0][0] >> 15) + (Op13L * matrixB[1][0] >> 15) +
- (Op13U * matrixB[2][0] >> 15);
- Op13Y = (Op13F * matrixB[0][1] >> 15) + (Op13L * matrixB[1][1] >> 15) +
- (Op13U * matrixB[2][1] >> 15);
- Op13Z = (Op13F * matrixB[0][2] >> 15) + (Op13L * matrixB[1][2] >> 15) +
- (Op13U * matrixB[2][2] >> 15);
-
+ Op13X = (Op13F * matrixB[0][0] >> 15) + (Op13L * matrixB[1][0] >> 15) + (Op13U * matrixB[2][0] >> 15);
+ Op13Y = (Op13F * matrixB[0][1] >> 15) + (Op13L * matrixB[1][1] >> 15) + (Op13U * matrixB[2][1] >> 15);
+ Op13Z = (Op13F * matrixB[0][2] >> 15) + (Op13L * matrixB[1][2] >> 15) + (Op13U * matrixB[2][2] >> 15);
}
void DSPOp23()
{
- Op23X = (Op23F * matrixC[0][0] >> 15) + (Op23L * matrixC[1][0] >> 15) +
- (Op23U * matrixC[2][0] >> 15);
- Op23Y = (Op23F * matrixC[0][1] >> 15) + (Op23L * matrixC[1][1] >> 15) +
- (Op23U * matrixC[2][1] >> 15);
- Op23Z = (Op23F * matrixC[0][2] >> 15) + (Op23L * matrixC[1][2] >> 15) +
- (Op23U * matrixC[2][2] >> 15);
-
+ Op23X = (Op23F * matrixC[0][0] >> 15) + (Op23L * matrixC[1][0] >> 15) + (Op23U * matrixC[2][0] >> 15);
+ Op23Y = (Op23F * matrixC[0][1] >> 15) + (Op23L * matrixC[1][1] >> 15) + (Op23U * matrixC[2][1] >> 15);
+ Op23Z = (Op23F * matrixC[0][2] >> 15) + (Op23L * matrixC[1][2] >> 15) + (Op23U * matrixC[2][2] >> 15);
}
int16_t Op14Zr;
@@ -986,8 +910,7 @@ void DSPOp14()
DSP1_Inverse(DSP1_Cos(Op14Xr), 0, &CSec, &ESec);
// Rotation Around Z
- DSP1_NormalizeDouble(Op14U * DSP1_Cos(Op14Yr) - Op14F * DSP1_Sin(Op14Yr), &C,
- &E);
+ DSP1_NormalizeDouble(Op14U * DSP1_Cos(Op14Yr) - Op14F * DSP1_Sin(Op14Yr), &C, &E);
E = ESec - E;
@@ -996,12 +919,10 @@ void DSPOp14()
Op14Zrr = Op14Zr + DSP1_Truncate(C, E);
// Rotation Around X
- Op14Xrr = Op14Xr + (Op14U * DSP1_Sin(Op14Yr) >> 15) + (Op14F * DSP1_Cos(
- Op14Yr) >> 15);
+ Op14Xrr = Op14Xr + (Op14U * DSP1_Sin(Op14Yr) >> 15) + (Op14F * DSP1_Cos(Op14Yr) >> 15);
// Rotation Around Y
- DSP1_NormalizeDouble(Op14U * DSP1_Cos(Op14Yr) + Op14F * DSP1_Sin(Op14Yr), &C,
- &E);
+ DSP1_NormalizeDouble(Op14U * DSP1_Cos(Op14Yr) + Op14F * DSP1_Sin(Op14Yr), &C, &E);
E = ESec - E;
@@ -1068,23 +989,17 @@ int16_t Op2BS;
void DSPOp0B()
{
- Op0BS = (Op0BX * matrixA[0][0] + Op0BY * matrixA[0][1] + Op0BZ * matrixA[0][2])
- >> 15;
-
+ Op0BS = (Op0BX * matrixA[0][0] + Op0BY * matrixA[0][1] + Op0BZ * matrixA[0][2]) >> 15;
}
void DSPOp1B()
{
- Op1BS = (Op1BX * matrixB[0][0] + Op1BY * matrixB[0][1] + Op1BZ * matrixB[0][2])
- >> 15;
-
+ Op1BS = (Op1BX * matrixB[0][0] + Op1BY * matrixB[0][1] + Op1BZ * matrixB[0][2]) >> 15;
}
void DSPOp2B()
{
- Op2BS = (Op2BX * matrixC[0][0] + Op2BY * matrixC[0][1] + Op2BZ * matrixC[0][2])
- >> 15;
-
+ Op2BS = (Op2BX * matrixC[0][0] + Op2BY * matrixC[0][1] + Op2BZ * matrixC[0][2]) >> 15;
}
int16_t Op08X, Op08Y, Op08Z, Op08Ll, Op08Lh;
@@ -1094,7 +1009,6 @@ void DSPOp08()
int32_t Op08Size = (Op08X * Op08X + Op08Y * Op08Y + Op08Z * Op08Z) << 1;
Op08Ll = Op08Size & 0xffff;
Op08Lh = (Op08Size >> 16) & 0xffff;
-
}
int16_t Op18X, Op18Y, Op18Z, Op18R, Op18D;
@@ -1102,7 +1016,6 @@ int16_t Op18X, Op18Y, Op18Z, Op18R, Op18D;
void DSPOp18()
{
Op18D = (Op18X * Op18X + Op18Y * Op18Y + Op18Z * Op18Z - Op18R * Op18R) >> 15;
-
}
int16_t Op38X, Op38Y, Op38Z, Op38R, Op38D;
@@ -1111,7 +1024,6 @@ void DSPOp38()
{
Op38D = (Op38X * Op38X + Op38Y * Op38Y + Op38Z * Op38Z - Op38R * Op38R) >> 15;
Op38D++;
-
}
int16_t Op28X;
@@ -1126,19 +1038,18 @@ void DSPOp28()
if (Radius == 0) Op28R = 0;
else
{
- int16_t C, E;
+ int16_t C, E, Pos, Node1, Node2;
DSP1_NormalizeDouble(Radius, &C, &E);
if (E & 1) C = C * 0x4000 >> 15;
- int16_t Pos = C * 0x0040 >> 15;
+ Pos = C * 0x0040 >> 15;
- int16_t Node1 = DSP1ROM[0x00d5 + Pos];
- int16_t Node2 = DSP1ROM[0x00d6 + Pos];
+ Node1 = DSP1ROM[0x00d5 + Pos];
+ Node2 = DSP1ROM[0x00d6 + Pos];
Op28R = ((Node2 - Node1) * (C & 0x1ff) >> 9) + Node1;
Op28R >>= (E >> 1);
}
-
}
int16_t Op1CX, Op1CY, Op1CZ;
@@ -1169,7 +1080,6 @@ void DSPOp1C()
Op1CZ1 = (Op1CZBR * DSP1_Cos(Op1CX) >> 15) - (Op1CYBR * DSP1_Sin(Op1CX) >> 15);
Op1CYAR = Op1CY1;
Op1CZAR = Op1CZ1;
-
}
uint16_t Op0FRamsize;
@@ -1178,7 +1088,6 @@ uint16_t Op0FPass;
void DSPOp0F()
{
Op0FPass = 0x0000;
-
}
int16_t Op2FUnknown;