//Copyright (C) 1997-2001 ZSNES Team ( zsknight@zsnes.com / _demo_@zsnes.com )
//
//This program is free software; you can redistribute it and/or
//modify it under the terms of the GNU General Public License
//as published by the Free Software Foundation; either
//version 2 of the License, or (at your option) any later
//version.
//
//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.


#include <stdio.h>
#include <stdarg.h>
#include <math.h>

#include <string.h>
#include <stdlib.h>

#define __OPT__
#define __OPT01__
#define __OPT02__
#define __OPT04__ //was commented on original source (yoyo)
#define __OPT06__
#define __OPT0C__    // this optimisation may break pilotwings
#define __OPT11__
#define __OPT21__
#define __OPT1C__

/***************************************************************************\
*  Math tables                                                              *
\***************************************************************************/

double* CosTable2;
double* SinTable2;

int32*  CosTable2Fix;
int32*  SinTable2Fix;

#define INCR 2048
#define Angle(x) (((x)/(65536/INCR)) & (INCR-1))

#define AngleFix(x) (((x)>>5) & (INCR-1))

#define Cos(x) ((double) CosTable2[x])
#define Sin(x) ((double) SinTable2[x])

#define CosFix(x) (CosTable2Fix[x])
#define SinFix(x) (SinTable2Fix[x])

#define PI 3.14159265358979323846264338327

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__
   unsigned int i;
   CosTable2 = (double*) malloc(INCR * sizeof(double));
   SinTable2 = (double*) malloc(INCR * sizeof(double));

   CosTable2Fix = (int32*) malloc(INCR * sizeof(int32));
   SinTable2Fix = (int32*) malloc(INCR * sizeof(int32));
   for (i = 0; i < INCR; i++)
   {
      CosTable2[i] = (cos((double)(2 * PI * i / INCR)));
      SinTable2[i] = (sin((double)(2 * PI * i / INCR)));
      CosTable2Fix[i] = (65536 * cos((double)(2 * PI * i / INCR)));
      SinTable2Fix[i] = (65536 * sin((double)(2 * PI * i / INCR)));
   }
#endif
}


short Op00Multiplicand;
short Op00Multiplier;
short Op00Result;

void DSPOp00()
{
   dsp_opcode[0]++;

   Op00Result = Op00Multiplicand * Op00Multiplier / 32768;
}

signed short Op10Coefficient;
signed short Op10Exponent;
signed short Op10CoefficientR;
signed short Op10ExponentR;
float Op10Temp;

void DSPOp10()
{
   dsp_opcode[0x10]++;

   Op10ExponentR = -Op10Exponent;
   Op10Temp = Op10Coefficient / 32768.0;
   if (Op10Temp == 0)
      Op10CoefficientR = 0;
   else
      Op10Temp = 1 / Op10Temp;
   if (Op10Temp > 0)
      while (Op10Temp >= 1.0)
      {
         Op10Temp = Op10Temp / 2.0;
         Op10ExponentR++;
      }
   else
      while (Op10Temp < -1.0)
      {
         Op10Temp = Op10Temp / 2.0;
         Op10ExponentR++;
      }
   Op10CoefficientR = Op10Temp * 32768;
}


short Op04Angle;
unsigned short Op04Radius;
short Op04Sin;
short Op04Cos;

#ifdef __OPT04__

void DSPOp04()
{
   dsp_opcode[0x04]++;

   int32 angle;
   //angle = Angle(Op04Angle);
   angle = AngleFix(Op04Angle);

   //Op04Sin = Sin(angle) * Op04Radius;
   SMULT1616(Op04Sin, SinFix(angle), (int32)Op04Radius)
   //Op04Cos = Cos(angle) * Op04Radius;
   SMULT1616(Op04Cos, CosFix(angle), (int32)Op04Radius);

}
#else

void DSPOp04()
{
   double angle;
   
   angle = Op04Angle * 2 * PI / 65536.0;

   Op04Sin = sin(angle) * Op04Radius;
   Op04Cos = cos(angle) * Op04Radius;

}
#endif

unsigned short Op0CA;
short Op0CX1;
short Op0CY1;
short Op0CX2;
short Op0CY2;

#ifdef __OPT0C__
void DSPOp0C()
{
   dsp_opcode[0x0C]++;

   Op0CX2 = (Op0CX1 * Cos(Angle(Op0CA)) + Op0CY1 * Sin(Angle(Op0CA)));
   Op0CY2 = (Op0CX1 * -Sin(Angle(Op0CA)) + Op0CY1 * Cos(Angle(Op0CA)));
}
#else
void DSPOp0C()
{

   Op0CX2 = (Op0CX1 * cos(Op0CA * 2 * PI / 65536.0) + Op0CY1 * sin(Op0CA * 2 * PI / 65536.0));
   Op0CY2 = (Op0CX1 * -sin(Op0CA * 2 * PI / 65536.0) + Op0CY1 * cos(Op0CA * 2 * PI / 65536.0));
}

#endif

short Op02FX;
short Op02FY;
short Op02FZ;
short Op02LFE;
short Op02LES;
unsigned short Op02AAS;
unsigned short Op02AZS;
unsigned short Op02VOF;
unsigned short Op02VVA;

short Op02CX;
short Op02CY;
double Op02CXF;
double Op02CYF;
double ViewerX0;
double ViewerY0;
double ViewerZ0;
double ViewerX1;
double ViewerY1;
double ViewerZ1;
double ViewerX;
double ViewerY;
double ViewerZ;
int ViewerAX;
int ViewerAY;
int ViewerAZ;
double NumberOfSlope;
double ScreenX;
double ScreenY;
double ScreenZ;
double TopLeftScreenX;
double TopLeftScreenY;
double TopLeftScreenZ;
double BottomRightScreenX;
double BottomRightScreenY;
double BottomRightScreenZ;
double Ready;
double RasterLX;
double RasterLY;
double RasterLZ;
double ScreenLX1;
double ScreenLY1;
double ScreenLZ1;
int    ReversedLES;
short Op02LESb;
double NAzsB, NAasB;
double ViewerXc;
double ViewerYc;
double ViewerZc;
double CenterX, CenterY;
short Op02CYSup, Op02CXSup;
double CXdistance;

#define VofAngle 0x3880

short TValDebug, TValDebug2;
short ScrDispl;


#ifdef __OPT02__
void DSPOp02()
{
   dsp_opcode[0x02]++;

   ViewerZ1 = -Cos(Angle(Op02AZS));
   ViewerX1 = Sin(Angle(Op02AZS)) * Sin(Angle(Op02AAS));
   ViewerY1 = Sin(Angle(Op02AZS)) * Cos(Angle(Op02AAS));


   ViewerX = Op02FX - ViewerX1 * Op02LFE;
   ViewerY = Op02FY - ViewerY1 * Op02LFE;
   ViewerZ = Op02FZ - ViewerZ1 * Op02LFE;

   ScreenX = Op02FX + ViewerX1 * (Op02LES - Op02LFE);
   ScreenY = Op02FY + ViewerY1 * (Op02LES - Op02LFE);
   ScreenZ = Op02FZ + ViewerZ1 * (Op02LES - Op02LFE);

   if (ViewerZ1 == 0)ViewerZ1++;
   NumberOfSlope = ViewerZ / -ViewerZ1;

   Op02CX = (short)(Op02CXF = ViewerX + ViewerX1 * NumberOfSlope);
   Op02CY = (short)(Op02CYF = ViewerY + ViewerY1 * NumberOfSlope);

   Op02VOF = 0x0000;
   ReversedLES = 0;
   Op02LESb = Op02LES;
   if ((Op02LES >= VofAngle + 16384.0) && (Op02LES < VofAngle + 32768.0))
   {
      ReversedLES = 1;
      Op02LESb = VofAngle + 0x4000 - (Op02LES - (VofAngle + 0x4000));
   }
   Op02VVA = (short)(Op02LESb * tan((Op02AZS - 0x4000) * 6.2832 / 65536.0));
   if ((Op02LESb >= VofAngle) && (Op02LESb <= VofAngle + 0x4000))
   {
      Op02VOF = (short)(Op02LESb * tan((Op02AZS - 0x4000 - VofAngle) * 6.2832 / 65536.0));
      Op02VVA -= Op02VOF;
   }
   if (ReversedLES)
      Op02VOF = -Op02VOF;

   NAzsB = (Op02AZS - 0x4000) * 6.2832 / 65536.0;
   NAasB = Op02AAS * 6.2832 / 65536.0;

   if (tan(NAzsB) == 0) NAzsB = 0.1;

   ScrDispl = 0;
   if (NAzsB > -0.15)
   {
      NAzsB = -0.15;
      ScrDispl = Op02VVA - 0xFFDA;
   }

   CXdistance = 1 / tan(NAzsB);

   ViewerXc = Op02FX;
   ViewerYc = Op02FY;
   ViewerZc = Op02FZ;

   CenterX = (-sin(NAasB) * ViewerZc * CXdistance) + ViewerXc;
   CenterY = (cos(NAasB) * ViewerZc * CXdistance) + ViewerYc;
   Op02CX = (short)CenterX;
   Op02CY = (short)CenterY;

   ViewerXc = ViewerX; //-Op02FX);
   ViewerYc = ViewerY; //-Op02FY);
   ViewerZc = ViewerZ; //-Op02FZ);

   CenterX = (-sin(NAasB) * ViewerZc * CXdistance) + ViewerXc;
   if (CenterX < -32768) CenterX = -32768;
   if (CenterX > 32767) CenterX = 32767;
   CenterY = (cos(NAasB) * ViewerZc * CXdistance) + ViewerYc;
   if (CenterY < -32768) CenterY = -32768;
   if (CenterY > 32767) CenterY = 32767;

   TValDebug = (NAzsB * 65536 / 6.28);
   TValDebug2 = ScrDispl;

   //   if (Op02CY < 0) {Op02CYSup = Op02CY/256; Op02CY = 0;}
   //   if (Op02CX < 0) {Op02CXSup = Op02CX/256; Op02CX = 0;}

   //  [4/15/2001]   (ViewerX+ViewerX1*NumberOfSlope);
   //  [4/15/2001]   (ViewerY+ViewerY1*NumberOfSlope);

   //   if(Op02LFE==0x2200)Op02VVA=0xFECD;
   //   else Op02VVA=0xFFB2;



}
#else

void DSPOp02()
{
   ViewerZ1 = -cos(Op02AZS * 6.2832 / 65536.0);
   ViewerX1 = sin(Op02AZS * 6.2832 / 65536.0) * sin(Op02AAS * 6.2832 / 65536.0);
   ViewerY1 = sin(Op02AZS * 6.2832 / 65536.0) * cos(-Op02AAS * 6.2832 / 65536.0);

   ViewerX = Op02FX - ViewerX1 * Op02LFE;
   ViewerY = Op02FY - ViewerY1 * Op02LFE;
   ViewerZ = Op02FZ - ViewerZ1 * Op02LFE;

   ScreenX = Op02FX + ViewerX1 * (Op02LES - Op02LFE);
   ScreenY = Op02FY + ViewerY1 * (Op02LES - Op02LFE);
   ScreenZ = Op02FZ + ViewerZ1 * (Op02LES - Op02LFE);

   if (ViewerZ1 == 0)ViewerZ1++;
   NumberOfSlope = ViewerZ / -ViewerZ1;

   Op02CX = (short)(Op02CXF = ViewerX + ViewerX1 * NumberOfSlope);
   Op02CY = (short)(Op02CYF = ViewerY + ViewerY1 * NumberOfSlope);

   ViewerXc = ViewerX; //-Op02FX);
   ViewerYc = ViewerY; //-Op02FY);
   ViewerZc = ViewerZ; //-Op02FZ);

   Op02VOF = 0x0000;
   ReversedLES = 0;
   Op02LESb = Op02LES;
   if ((Op02LES >= VofAngle + 16384.0) && (Op02LES < VofAngle + 32768.0))
   {
      ReversedLES = 1;
      Op02LESb = VofAngle + 0x4000 - (Op02LES - (VofAngle + 0x4000));
   }
   Op02VVA = (short)(Op02LESb * tan((Op02AZS - 0x4000) * 6.2832 / 65536.0));
   if ((Op02LESb >= VofAngle) && (Op02LESb <= VofAngle + 0x4000))
   {
      Op02VOF = (short)(Op02LESb * tan((Op02AZS - 0x4000 - VofAngle) * 6.2832 / 65536.0));
      Op02VVA -= Op02VOF;
   }
   if (ReversedLES)
      Op02VOF = -Op02VOF;

   NAzsB = (Op02AZS - 0x4000) * 6.2832 / 65536.0;
   NAasB = Op02AAS * 6.2832 / 65536.0;

   if (tan(NAzsB) == 0) NAzsB = 0.1;

   ScrDispl = 0;
   if (NAzsB > -0.15)
   {
      NAzsB = -0.15;
      ScrDispl = Op02VVA - 0xFFDA;
   }

   CXdistance = 1 / tan(NAzsB);

   CenterX = (-sin(NAasB) * ViewerZc * CXdistance) + ViewerXc;
   if (CenterX < -32768) CenterX = -32768;
   if (CenterX > 32767) CenterX = 32767;
   Op02CX = (short)CenterX;
   CenterY = (cos(NAasB) * ViewerZc * CXdistance) + ViewerYc;
   if (CenterY < -32768) CenterY = -32768;
   if (CenterY > 32767) CenterY = 32767;
   Op02CY = (short)CenterY;

   TValDebug = (NAzsB * 65536 / 6.28);
   TValDebug2 = ScrDispl;

   //   if (Op02CY < 0) {Op02CYSup = Op02CY/256; Op02CY = 0;}
   //   if (Op02CX < 0) {Op02CXSup = Op02CX/256; Op02CX = 0;}

   //  [4/15/2001]   (ViewerX+ViewerX1*NumberOfSlope);
   //  [4/15/2001]   (ViewerY+ViewerY1*NumberOfSlope);

   //   if(Op02LFE==0x2200)Op02VVA=0xFECD;
   //   else Op02VVA=0xFFB2;



}
#endif

short Op0AVS;
short Op0AA;
short Op0AB;
short Op0AC;
short Op0AD;

double RasterRX;
double RasterRY;
double RasterRZ;
double RasterLSlopeX;
double RasterLSlopeY;
double RasterLSlopeZ;
double RasterRSlopeX;
double RasterRSlopeY;
double RasterRSlopeZ;
double GroundLX;
double GroundLY;
double GroundRX;
double GroundRY;
double Distance;

double NAzs, NAas;
double RVPos, RHPos, RXRes, RYRes;


void GetRXYPos()
{
   double scalar;

   if (Op02LES == 0) return;


   NAzs = NAzsB - Atan((RVPos) / (double)Op02LES);
   NAas = NAasB;// + Atan(RHPos) / (double)Op02LES);

   if (cos(NAzs) == 0) NAzs += 0.001;
   if (tan(NAzs) == 0) NAzs += 0.001;

   RXRes = (-sin(NAas) * ViewerZc / (tan(NAzs)) + ViewerXc);
   RYRes = (cos(NAas) * ViewerZc / (tan(NAzs)) + ViewerYc);
   scalar = ((ViewerZc / sin(NAzs)) / (double)Op02LES);
   RXRes += scalar * -sin(NAas + PI / 2) * RHPos;
   RYRes += scalar * cos(NAas + PI / 2) * RHPos;
}

void DSPOp0A()
{
   dsp_opcode[0x0A]++;

   double x2, y2, x3, y3, x4, y4, m, ypos;


   if (Op0AVS == 0)
   {
      Op0AVS++;
      return;
   }
   ypos = Op0AVS - ScrDispl;
   // CenterX,CenterX = Center (x1,y1)
   // Get (0,Vs) coords (x2,y2)
   RVPos = ypos;
   RHPos = 0;
   GetRXYPos();
   x2 = RXRes;
   y2 = RYRes;
   // Get (-128,Vs) coords (x3,y3)
   RVPos = ypos;
   RHPos = -128;
   GetRXYPos();
   x3 = RXRes;
   y3 = RYRes;
   // Get (127,Vs) coords (x4,y4)
   RVPos = ypos;
   RHPos = 127;
   GetRXYPos();
   x4 = RXRes;
   y4 = RYRes;

   // A = (x4-x3)/256
   m = (x4 - x3) / 256 * 256;
   if (m > 32767) m = 32767;
   if (m < -32768) m = -32768;
   Op0AA = (short)(m);
   // C = (y4-y3)/256
   m = (y4 - y3) / 256 * 256;
   if (m > 32767) m = 32767;
   if (m < -32768) m = -32768;
   Op0AC = (short)(m);
   if (ypos == 0)
   {
      Op0AB = 0;
      Op0AD = 0;
   }
   else
   {
      // B = (x2-x1)/Vs
      m = (x2 - CenterX) / ypos * 256;
      if (m > 32767) m = 32767;
      if (m < -32768) m = -32768;
      Op0AB = (short)(m);
      // D = (y2-y1)/Vs
      m = (y2 - CenterY) / ypos * 256;
      if (m > 32767) m = 32767;
      if (m < -32768) m = -32768;
      Op0AD = (short)(m);
   }

   Op0AVS += 1;
}

short Op06X;
short Op06Y;
short Op06Z;
short Op06H;
short Op06V;
unsigned short Op06S;

/*double ObjPX;
double ObjPY;
double ObjPZ;
double ObjPX1;
double ObjPY1;
double ObjPZ1;
double ObjPX2;
double ObjPY2;
double ObjPZ2;*/
int32 ObjPX;
int32 ObjPY;
int32 ObjPZ;
int32 ObjPX1;
int32 ObjPY1;
int32 ObjPZ1;
int32 ObjPX2;
int32 ObjPY2;
int32 ObjPZ2;
double DivideOp06;
int Temp;
int tanval2;

#ifdef __OPT06__
void DSPOp06()
{

   dsp_opcode[0x06]++;

   ObjPX = Op06X - Op02FX;
   ObjPY = Op06Y - Op02FY;
   ObjPZ = Op06Z - Op02FZ;



   // rotate around Z
   //tanval2 = Angle(-Op02AAS+32768);
   //   tanval2 = (-Op02AAS+32768)/(65536/INCR);
   //ObjPX1=(ObjPX*Cos(tanval2)+ObjPY*-Sin(tanval2));
   //ObjPY1=(ObjPX*Sin(tanval2)+ObjPY*Cos(tanval2));
   //ObjPZ1=ObjPZ;
   tanval2 = AngleFix(-Op02AAS + 32768);
   SADDMULT1616(ObjPX1, ObjPX, CosFix(tanval2), ObjPY, -SinFix(tanval2))
   SADDMULT1616(ObjPY1, ObjPX, SinFix(tanval2), ObjPY, CosFix(tanval2))
   ObjPZ1 = ObjPZ;


   // rotate around X
   //   tanval2 = (-Op02AZS/(65536/INCR)) & 1023;
   //tanval2 = Angle(-Op02AZS);
   //   tanval2 = (-Op02AZS)/256;
   /*ObjPX2=ObjPX1;
   ObjPY2=(ObjPY1*Cos(tanval2)+ObjPZ1*-Sin(tanval2));
   ObjPZ2=(ObjPY1*Sin(tanval2)+ObjPZ1*Cos(tanval2));*/
   tanval2 = AngleFix(-Op02AZS);
   ObjPX2 = ObjPX1;
   SADDMULT1616(ObjPY2, ObjPY1, CosFix(tanval2), ObjPZ1, -SinFix(tanval2))
   SADDMULT1616(ObjPZ2, ObjPY1, SinFix(tanval2), ObjPZ1, CosFix(tanval2))

   ObjPZ2 = ObjPZ2 - Op02LFE;

   if (ObjPZ2 < 0)
   {
      Op06H = (short)(-ObjPX2 * Op02LES / -(ObjPZ2)); //-ObjPX2*256/-ObjPZ2;
      Op06V = (short)(-ObjPY2 * Op02LES / -(ObjPZ2)); //-ObjPY2*256/-ObjPZ2;
      //Op06S=(unsigned short)(256*(double)Op02LES/-ObjPZ2);
      Op06S = (unsigned short)(256 * (int32)Op02LES / -ObjPZ2);
   }
   else
   {
      Op06H = 0;
      Op06V = 14 * 16;
      Op06S = 0xFFFF;
   }


}
#else

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 = (short)(-ObjPX2 * Op02LES / -(ObjPZ2)); //-ObjPX2*256/-ObjPZ2;
      Op06V = (short)(-ObjPY2 * Op02LES / -(ObjPZ2)); //-ObjPY2*256/-ObjPZ2;
      Op06S = (unsigned short)(256 * (double)Op02LES / -ObjPZ2);
   }
   else
   {
      Op06H = 0;
      Op06V = 14 * 16;
      Op06S = 0xFFFF;
   }


}
#endif



double matrixB[3][3];
double matrixB2[3][3];
double matrixB3[3][3];

double matrixA[3][3];
double matrixA2[3][3];
double matrixA3[3][3];

void MultMatrixB(double result[3][3], double mat1[3][3], double mat2[3][3])
{
   result[0][0] = (mat1[0][0] * mat2[0][0] + mat1[0][1] * mat2[1][0] + mat1[0][2] * mat2[2][0]);
   result[0][1] = (mat1[0][0] * mat2[0][1] + mat1[0][1] * mat2[1][1] + mat1[0][2] * mat2[2][1]);
   result[0][2] = (mat1[0][0] * mat2[0][2] + mat1[0][1] * mat2[1][2] + mat1[0][2] * mat2[2][2]);
   
   result[1][0] = (mat1[1][0] * mat2[0][0] + mat1[1][1] * mat2[1][0] + mat1[1][2] * mat2[2][0]);
   result[1][1] = (mat1[1][0] * mat2[0][1] + mat1[1][1] * mat2[1][1] + mat1[1][2] * mat2[2][1]);
   result[1][2] = (mat1[1][0] * mat2[0][2] + mat1[1][1] * mat2[1][2] + mat1[1][2] * mat2[2][2]);

   result[2][0] = (mat1[2][0] * mat2[0][0] + mat1[2][1] * mat2[1][0] + mat1[2][2] * mat2[2][0]);
   result[2][1] = (mat1[2][0] * mat2[0][1] + mat1[2][1] * mat2[1][1] + mat1[2][2] * mat2[2][1]);
   result[2][2] = (mat1[2][0] * mat2[0][2] + mat1[2][1] * mat2[1][2] + mat1[2][2] * mat2[2][2]);

}


short Op01m;
short Op01Zr;
short Op01Xr;
short Op01Yr;
short Op11m;
short Op11Zr;
short Op11Xr;
short Op11Yr;
short Op21m;
short Op21Zr;
short Op21Xr;
short Op21Yr;
double sc, sc2, sc3;



#ifdef __OPT01__
void DSPOp01()
{
   dsp_opcode[0x01]++;

   unsigned short zr, yr, xr;

   zr = Angle(Op01Zr);
   xr = Angle(Op01Yr);
   yr = Angle(Op01Xr);

   matrixB[0][0] = 1;
   matrixB[0][1] = 0;
   matrixB[0][2] = 0;
   matrixB[1][0] = 0;
   matrixB[1][1] = Cos(xr);
   matrixB[1][2] = -Sin(xr);
   matrixB[2][0] = 0;
   matrixB[2][1] = Sin(xr);
   matrixB[2][2] = Cos(xr);

   matrixB2[0][0] = Cos(yr);
   matrixB2[0][1] = 0;
   matrixB2[0][2] = Sin(yr);
   matrixB2[1][0] = 0;
   matrixB2[1][1] = 1;
   matrixB2[1][2] = 0;
   matrixB2[2][0] = -Sin(yr);
   matrixB2[2][1] = 0;
   matrixB2[2][2] = Cos(yr);

   MultMatrixB(matrixB3, matrixB, matrixB2);

   matrixB2[0][0] = Cos(zr);
   matrixB2[0][1] = -Sin(zr);
   matrixB2[0][2] = 0;
   matrixB2[1][0] = Sin(zr);
   matrixB2[1][1] = Cos(zr);
   matrixB2[1][2] = 0;
   matrixB2[2][0] = 0;
   matrixB2[2][1] = 0;
   matrixB2[2][2] = 1;

   MultMatrixB(matrixB, matrixB3, matrixB2);

   sc = ((double)Op01m) / 32768.0;

   matrixA[0][0] = matrixB[0][0];
   matrixA[0][1] = matrixB[0][1];
   matrixA[0][2] = matrixB[0][2];
   matrixA[1][0] = matrixB[1][0];
   matrixA[1][1] = matrixB[1][1];
   matrixA[1][2] = matrixB[1][2];
   matrixA[2][0] = matrixB[2][0];
   matrixA[2][1] = matrixB[2][1];
   matrixA[2][2] = matrixB[2][2];

}

#else

void DSPOp01()
{
   double zr, yr, xr;

   zr = ((double)Op01Zr) * 6.2832 / 65536;
   xr = ((double)Op01Yr) * 6.2832 / 65536;
   yr = ((double)Op01Xr) * 6.2832 / 65536;

   matrixB[0][0] = 1;
   matrixB[0][1] = 0;
   matrixB[0][2] = 0;
   matrixB[1][0] = 0;
   matrixB[1][1] = cos(xr);
   matrixB[1][2] = -sin(xr);
   matrixB[2][0] = 0;
   matrixB[2][1] = sin(xr);
   matrixB[2][2] = cos(xr);

   matrixB2[0][0] = cos(yr);
   matrixB2[0][1] = 0;
   matrixB2[0][2] = sin(yr);
   matrixB2[1][0] = 0;
   matrixB2[1][1] = 1;
   matrixB2[1][2] = 0;
   matrixB2[2][0] = -sin(yr);
   matrixB2[2][1] = 0;
   matrixB2[2][2] = cos(yr);

   MultMatrixB(matrixB3, matrixB, matrixB2);

   matrixB2[0][0] = cos(zr);
   matrixB2[0][1] = -sin(zr);
   matrixB2[0][2] = 0;
   matrixB2[1][0] = sin(zr);
   matrixB2[1][1] = cos(zr);
   matrixB2[1][2] = 0;
   matrixB2[2][0] = 0;
   matrixB2[2][1] = 0;
   matrixB2[2][2] = 1;

   MultMatrixB(matrixB, matrixB3, matrixB2);

   sc = ((double)Op01m) / 32768.0;

   matrixA[0][0] = matrixB[0][0];
   matrixA[0][1] = matrixB[0][1];
   matrixA[0][2] = matrixB[0][2];
   matrixA[1][0] = matrixB[1][0];
   matrixA[1][1] = matrixB[1][1];
   matrixA[1][2] = matrixB[1][2];
   matrixA[2][0] = matrixB[2][0];
   matrixA[2][1] = matrixB[2][1];
   matrixA[2][2] = matrixB[2][2];

}
#endif


#ifdef __OPT11__
void DSPOp11()
{
   dsp_opcode[0x11]++;

   short zr, yr, xr;

   zr = Angle(Op11Zr);
   xr = Angle(Op11Yr);
   yr = Angle(Op11Xr);

   matrixB[0][0] = 1;
   matrixB[0][1] = 0;
   matrixB[0][2] = 0;
   matrixB[1][0] = 0;
   matrixB[1][1] = Cos(xr);
   matrixB[1][2] = -Sin(xr);
   matrixB[2][0] = 0;
   matrixB[2][1] = Sin(xr);
   matrixB[2][2] = Cos(xr);

   matrixB2[0][0] = Cos(yr);
   matrixB2[0][1] = 0;
   matrixB2[0][2] = Sin(yr);
   matrixB2[1][0] = 0;
   matrixB2[1][1] = 1;
   matrixB2[1][2] = 0;
   matrixB2[2][0] = -Sin(yr);
   matrixB2[2][1] = 0;
   matrixB2[2][2] = Cos(yr);

   MultMatrixB(matrixB3, matrixB, matrixB2);

   matrixB2[0][0] = Cos(zr);
   matrixB2[0][1] = -Sin(zr);
   matrixB2[0][2] = 0;
   matrixB2[1][0] = Sin(zr);
   matrixB2[1][1] = Cos(zr);
   matrixB2[1][2] = 0;
   matrixB2[2][0] = 0;
   matrixB2[2][1] = 0;
   matrixB2[2][2] = 1;

   MultMatrixB(matrixB, matrixB3, matrixB2);

   sc2 = ((double)Op11m) / 32768.0;

   matrixA2[0][0] = matrixB[0][0];
   matrixA2[0][1] = matrixB[0][1];
   matrixA2[0][2] = matrixB[0][2];
   matrixA2[1][0] = matrixB[1][0];
   matrixA2[1][1] = matrixB[1][1];
   matrixA2[1][2] = matrixB[1][2];
   matrixA2[2][0] = matrixB[2][0];
   matrixA2[2][1] = matrixB[2][1];
   matrixA2[2][2] = matrixB[2][2];
}
#else

void DSPOp11()
{
   double zr, yr, xr;

   zr = ((double)Op11Zr) * 6.2832 / 65536;
   xr = ((double)Op11Yr) * 6.2832 / 65536;
   yr = ((double)Op11Xr) * 6.2832 / 65536;

   matrixB[0][0] = 1;
   matrixB[0][1] = 0;
   matrixB[0][2] = 0;
   matrixB[1][0] = 0;
   matrixB[1][1] = cos(xr);
   matrixB[1][2] = -sin(xr);
   matrixB[2][0] = 0;
   matrixB[2][1] = sin(xr);
   matrixB[2][2] = cos(xr);

   matrixB2[0][0] = cos(yr);
   matrixB2[0][1] = 0;
   matrixB2[0][2] = sin(yr);
   matrixB2[1][0] = 0;
   matrixB2[1][1] = 1;
   matrixB2[1][2] = 0;
   matrixB2[2][0] = -sin(yr);
   matrixB2[2][1] = 0;
   matrixB2[2][2] = cos(yr);

   MultMatrixB(matrixB3, matrixB, matrixB2);

   matrixB2[0][0] = cos(zr);
   matrixB2[0][1] = -sin(zr);
   matrixB2[0][2] = 0;
   matrixB2[1][0] = sin(zr);
   matrixB2[1][1] = cos(zr);
   matrixB2[1][2] = 0;
   matrixB2[2][0] = 0;
   matrixB2[2][1] = 0;
   matrixB2[2][2] = 1;

   MultMatrixB(matrixB, matrixB3, matrixB2);

   sc2 = ((double)Op11m) / 32768.0;

   matrixA2[0][0] = matrixB[0][0];
   matrixA2[0][1] = matrixB[0][1];
   matrixA2[0][2] = matrixB[0][2];
   matrixA2[1][0] = matrixB[1][0];
   matrixA2[1][1] = matrixB[1][1];
   matrixA2[1][2] = matrixB[1][2];
   matrixA2[2][0] = matrixB[2][0];
   matrixA2[2][1] = matrixB[2][1];
   matrixA2[2][2] = matrixB[2][2];
}
#endif


#ifdef __OPT21__
void DSPOp21()
{
   dsp_opcode[0x21]++;

   short zr, yr, xr;

   zr = Angle(Op21Zr);
   xr = Angle(Op21Yr);
   yr = Angle(Op21Xr);


   matrixB[0][0] = 1;
   matrixB[0][1] = 0;
   matrixB[0][2] = 0;
   matrixB[1][0] = 0;
   matrixB[1][1] = Cos(xr);
   matrixB[1][2] = -Sin(xr);
   matrixB[2][0] = 0;
   matrixB[2][1] = Sin(xr);
   matrixB[2][2] = Cos(xr);

   matrixB2[0][0] = Cos(yr);
   matrixB2[0][1] = 0;
   matrixB2[0][2] = Sin(yr);
   matrixB2[1][0] = 0;
   matrixB2[1][1] = 1;
   matrixB2[1][2] = 0;
   matrixB2[2][0] = -Sin(yr);
   matrixB2[2][1] = 0;
   matrixB2[2][2] = Cos(yr);

   MultMatrixB(matrixB3, matrixB, matrixB2);

   matrixB2[0][0] = Cos(zr);
   matrixB2[0][1] = -Sin(zr);
   matrixB2[0][2] = 0;
   matrixB2[1][0] = Sin(zr);
   matrixB2[1][1] = Cos(zr);
   matrixB2[1][2] = 0;
   matrixB2[2][0] = 0;
   matrixB2[2][1] = 0;
   matrixB2[2][2] = 1;

   MultMatrixB(matrixB, matrixB3, matrixB2);

   sc3 = ((double)Op21m) / 32768.0;

   matrixA3[0][0] = matrixB[0][0];
   matrixA3[0][1] = matrixB[0][1];
   matrixA3[0][2] = matrixB[0][2];
   matrixA3[1][0] = matrixB[1][0];
   matrixA3[1][1] = matrixB[1][1];
   matrixA3[1][2] = matrixB[1][2];
   matrixA3[2][0] = matrixB[2][0];
   matrixA3[2][1] = matrixB[2][1];
   matrixA3[2][2] = matrixB[2][2];
}
#else

void DSPOp21()
{
   double zr, yr, xr;

   zr = ((double)Op21Zr) * 6.2832 / 65536;
   xr = ((double)Op21Yr) * 6.2832 / 65536;
   yr = ((double)Op21Xr) * 6.2832 / 65536;

   matrixB[0][0] = 1;
   matrixB[0][1] = 0;
   matrixB[0][2] = 0;
   matrixB[1][0] = 0;
   matrixB[1][1] = cos(xr);
   matrixB[1][2] = -sin(xr);
   matrixB[2][0] = 0;
   matrixB[2][1] = sin(xr);
   matrixB[2][2] = cos(xr);

   matrixB2[0][0] = cos(yr);
   matrixB2[0][1] = 0;
   matrixB2[0][2] = sin(yr);
   matrixB2[1][0] = 0;
   matrixB2[1][1] = 1;
   matrixB2[1][2] = 0;
   matrixB2[2][0] = -sin(yr);
   matrixB2[2][1] = 0;
   matrixB2[2][2] = cos(yr);

   MultMatrixB(matrixB3, matrixB, matrixB2);

   matrixB2[0][0] = cos(zr);
   matrixB2[0][1] = -sin(zr);
   matrixB2[0][2] = 0;
   matrixB2[1][0] = sin(zr);
   matrixB2[1][1] = cos(zr);
   matrixB2[1][2] = 0;
   matrixB2[2][0] = 0;
   matrixB2[2][1] = 0;
   matrixB2[2][2] = 1;

   MultMatrixB(matrixB, matrixB3, matrixB2);

   sc3 = ((double)Op21m) / 32768.0;

   matrixA3[0][0] = matrixB[0][0];
   matrixA3[0][1] = matrixB[0][1];
   matrixA3[0][2] = matrixB[0][2];
   matrixA3[1][0] = matrixB[1][0];
   matrixA3[1][1] = matrixB[1][1];
   matrixA3[1][2] = matrixB[1][2];
   matrixA3[2][0] = matrixB[2][0];
   matrixA3[2][1] = matrixB[2][1];
   matrixA3[2][2] = matrixB[2][2];
}
#endif

short Op0DX;
short Op0DY;
short Op0DZ;
short Op0DF;
short Op0DL;
short Op0DU;
short Op1DX;
short Op1DY;
short Op1DZ;
short Op1DF;
short Op1DL;
short Op1DU;
short Op2DX;
short Op2DY;
short Op2DZ;
short Op2DF;
short Op2DL;
short Op2DU;

#define swap(a,b) temp=a;a=b;b=temp;

void DSPOp0D()
{
   dsp_opcode[0x0D]++;

   double a, b, c, d, e, f, g, h, i, det, temp;
   double a2, b2, c2, d2, e2, f2, g2, h2, i2, x, y, z;

   a = matrixA[0][0];
   b = matrixA[0][1];
   c = matrixA[0][2];
   d = matrixA[1][0];
   e = matrixA[1][1];
   f = matrixA[1][2];
   g = matrixA[2][0];
   h = matrixA[2][1];
   i = matrixA[2][2];
   //abc
   //def
   //ghi
   det = a * e * i + b * f * g + c * d * h - g * e * c - h * f * a - i * d * b;
   if (det == 0)
   {
      Op0DF = Op0DX;
      Op0DL = Op0DY;
      Op0DU = Op0DZ;
      return;
   }
   swap(d, b);
   swap(g, c);
   swap(h, f);
   b = -b;
   d = -d;
   f = -f;
   h = -h;
   a2 = (e * i - h * f) / det;
   b2 = (d * i - g * f) / det;
   c2 = (d * h - g * e) / det;
   d2 = (b * i - h * c) / det;
   e2 = (a * i - g * c) / det;
   f2 = (a * h - g * b) / det;
   g2 = (b * f - e * c) / det;
   h2 = (a * f - d * c) / det;
   i2 = (a * e - d * b) / det;
   x = Op0DX;
   y = Op0DY;
   z = Op0DZ;
   Op0DF = (short)((x * a2 + y * d2 + z * g2) / 2 * sc);
   Op0DL = (short)((x * b2 + y * e2 + z * h2) / 2 * sc);
   Op0DU = (short)((x * c2 + y * f2 + z * i2) / 2 * sc);

}

void DSPOp1D()
{
   dsp_opcode[0x1D]++;

   double a, b, c, d, e, f, g, h, i, det, temp;
   double a2, b2, c2, d2, e2, f2, g2, h2, i2, x, y, z;
   a = matrixA2[0][0];
   b = matrixA2[0][1];
   c = matrixA2[0][2];
   d = matrixA2[1][0];
   e = matrixA2[1][1];
   f = matrixA2[1][2];
   g = matrixA2[2][0];
   h = matrixA2[2][1];
   i = matrixA2[2][2];
   //abc
   //def
   //ghi
   det = a * e * i + b * f * g + c * d * h - g * e * c - h * f * a - i * d * b;
   if (det == 0)
   {
      Op1DF = 0;
      Op1DL = 0;
      Op1DU = 0;
      return;
   }
   swap(d, b);
   swap(g, c);
   swap(h, f);
   b = -b;
   d = -d;
   f = -f;
   h = -h;
   a2 = (e * i - h * f) / det;
   b2 = (d * i - g * f) / det;
   c2 = (d * h - g * e) / det;
   d2 = (b * i - h * c) / det;
   e2 = (a * i - g * c) / det;
   f2 = (a * h - g * b) / det;
   g2 = (b * f - e * c) / det;
   h2 = (a * f - d * c) / det;
   i2 = (a * e - d * b) / det;
   x = Op1DX;
   y = Op1DY;
   z = Op1DZ;
   Op1DF = (short)((x * a2 + y * d2 + z * g2) / 2 * sc2);
   Op1DL = (short)((x * b2 + y * e2 + z * h2) / 2 * sc2);
   Op1DU = (short)((x * c2 + y * f2 + z * i2) / 2 * sc2);
}

void DSPOp2D()
{
   dsp_opcode[0x2D]++;

   double a, b, c, d, e, f, g, h, i, det, temp;
   double a2, b2, c2, d2, e2, f2, g2, h2, i2, x, y, z;
   a = matrixA3[0][0];
   b = matrixA3[0][1];
   c = matrixA3[0][2];
   d = matrixA3[1][0];
   e = matrixA3[1][1];
   f = matrixA3[1][2];
   g = matrixA3[2][0];
   h = matrixA3[2][1];
   i = matrixA3[2][2];
   //abc
   //def
   //ghi
   det = a * e * i + b * f * g + c * d * h - g * e * c - h * f * a - i * d * b;
   if (det == 0)
   {
      Op2DF = 0;
      Op2DL = 0;
      Op2DU = 0;
      return;
   }
   swap(d, b);
   swap(g, c);
   swap(h, f);
   b = -b;
   d = -d;
   f = -f;
   h = -h;
   a2 = (e * i - h * f) / det;
   b2 = (d * i - g * f) / det;
   c2 = (d * h - g * e) / det;
   d2 = (b * i - h * c) / det;
   e2 = (a * i - g * c) / det;
   f2 = (a * h - g * b) / det;
   g2 = (b * f - e * c) / det;
   h2 = (a * f - d * c) / det;
   i2 = (a * e - d * b) / det;
   x = Op2DX;
   y = Op2DY;
   z = Op2DZ;
   Op2DF = (short)((x * a2 + y * d2 + z * g2) / 2 * sc3);
   Op2DL = (short)((x * b2 + y * e2 + z * h2) / 2 * sc3);
   Op2DU = (short)((x * c2 + y * f2 + z * i2) / 2 * sc3);
}

short Op03F;
short Op03L;
short Op03U;
short Op03X;
short Op03Y;
short Op03Z;
short Op13F;
short Op13L;
short Op13U;
short Op13X;
short Op13Y;
short Op13Z;
short Op23F;
short Op23L;
short Op23U;
short Op23X;
short Op23Y;
short Op23Z;

void DSPOp03()
{
   dsp_opcode[0x03]++;

   double F, L, U;

   F = Op03F;
   L = Op03L;
   U = Op03U;
   Op03X = (short)((F * matrixA[0][0] + L * matrixA[1][0] + U * matrixA[2][0]) / 2 * sc);
   Op03Y = (short)((F * matrixA[0][1] + L * matrixA[1][1] + U * matrixA[2][1]) / 2 * sc);
   Op03Z = (short)((F * matrixA[0][2] + L * matrixA[1][2] + U * matrixA[2][2]) / 2 * sc);

}

void DSPOp13()
{
   dsp_opcode[0x13]++;

   double F, L, U;
   F = Op13F;
   L = Op13L;
   U = Op13U;
   Op13X = (short)((F * matrixA2[0][0] + L * matrixA2[1][0] + U * matrixA2[2][0]) / 2 * sc2);
   Op13Y = (short)((F * matrixA2[0][1] + L * matrixA2[1][1] + U * matrixA2[2][1]) / 2 * sc2);
   Op13Z = (short)((F * matrixA2[0][2] + L * matrixA2[1][2] + U * matrixA2[2][2]) / 2 * sc2);
}

void DSPOp23()
{
   dsp_opcode[0x23]++;

   double F, L, U;
   F = Op23F;
   L = Op23L;
   U = Op23U;
   Op23X = (short)((F * matrixA3[0][0] + L * matrixA3[1][0] + U * matrixA3[2][0]) / 2 * sc3);
   Op23Y = (short)((F * matrixA3[0][1] + L * matrixA3[1][1] + U * matrixA3[2][1]) / 2 * sc3);
   Op23Z = (short)((F * matrixA3[0][2] + L * matrixA3[1][2] + U * matrixA3[2][2]) / 2 * sc3);
}

short Op14Zr;
short Op14Xr;
short Op14Yr;
short Op14U;
short Op14F;
short Op14L;
short Op14Zrr;
short Op14Xrr;
short Op14Yrr;

double Op14Temp;
void DSPOp14()
{
   dsp_opcode[0x14]++;

   Op14Temp = (Op14Zr * 6.2832 / 65536.0) + (1 / cos(Op14Xr * 6.2832 / 65536.0)) * ((Op14U * 6.2832 / 65536.0) * cos(
                 Op14Yr * 6.2832 / 65536.0) - (Op14F * 6.2832 / 65536.0) * sin(Op14Yr * 6.2832 / 65536.0));
   Op14Zrr = (short)(Op14Temp * 65536.0 / 6.2832);
   Op14Temp = (Op14Xr * 6.2832 / 65536.0) + ((Op14U * 6.2832 / 65536.0) * sin(Op14Yr * 6.2832 / 65536.0) +
              (Op14F * 6.2832 / 65536.0) * cos(Op14Yr * 6.2832 / 65536.0));
   Op14Xrr = (short)(Op14Temp * 65536.0 / 6.2832);
   Op14Temp = (Op14Yr * 6.2832 / 65536.0) - tan(Op14Xr * 6.2832 / 65536.0) * ((Op14U * 6.2832 / 65536.0) * cos(
                 Op14Yr * 6.2832 / 65536.0) + (Op14F * 6.2832 / 65536.0) * sin(Op14Yr * 6.2832 / 65536.0)) + (Op14L * 6.2832 / 65536.0);
   Op14Yrr = (short)(Op14Temp * 65536.0 / 6.2832);
}

short Op0EH;
short Op0EV;
short Op0EX;
short Op0EY;

void DSPOp0E()
{
   dsp_opcode[0x0E]++;

   // screen Directions UP
   RVPos = Op0EV;
   RHPos = Op0EH;
   GetRXYPos();
   Op0EX = RXRes;
   Op0EY = RYRes;

}

short Op0BX;
short Op0BY;
short Op0BZ;
short Op0BS;
short Op1BX;
short Op1BY;
short Op1BZ;
short Op1BS;
short Op2BX;
short Op2BY;
short Op2BZ;
short Op2BS;

void DSPOp0B()
{
   dsp_opcode[0x0B]++;

   Op0BS = (Op0BX * matrixA[0][0] + Op0BY * matrixA2[0][1] + Op0BZ * matrixA2[0][2]);
}

void DSPOp1B()
{
   dsp_opcode[0x1B]++;

   Op1BS = (Op1BX * matrixA2[0][0] + Op1BY * matrixA2[0][1] + Op1BZ * matrixA2[0][2]);

}

void DSPOp2B()
{
   dsp_opcode[0x2B]++;

   Op2BS = (Op2BX * matrixA3[0][0] + Op2BY * matrixA3[0][1] + Op2BZ * matrixA3[0][2]);
}

short Op08X, Op08Y, Op08Z, Op08Ll, Op08Lh;
long Op08Size;

void DSPOp08()
{
   dsp_opcode[0x08]++;

   Op08Size = (Op08X * Op08X + Op08Y * Op08Y + Op08Z * Op08Z) * 2;
   Op08Ll = Op08Size & 0xFFFF;
   Op08Lh = (Op08Size >> 16) & 0xFFFF;
}

short Op18X, Op18Y, Op18Z, Op18R, Op18D;

void DSPOp18()
{
   dsp_opcode[0x18]++;

   double x, y, z, r;
   x = Op18X;
   y = Op18Y;
   z = Op18Z;
   r = Op18R;
   r = (x * x + y * y + z * z - r * r);
   if (r > 32767) r = 32767;
   if (r < -32768) r = -32768;
   Op18D = (short)r;
}

short Op28X;
short Op28Y;
short Op28Z;
short Op28R;

int32 fixed_sqrt(int32 r)
{
   int32 t, b, c = 0;

   for (b = 0x10000000; b != 0; b >>= 2)
   {
      t = c + b;
      c >>= 1;
      if (t <= r)
      {
         r -= t;
         c += b;
      }
   }
   return (c);
}


void DSPOp28()
{
   dsp_opcode[0x28]++;

   //   Op28R=(short)sqrt((double)(Op28X*Op28X+Op28Y*Op28Y+Op28Z*Op28Z));
   Op28R = (short)fixed_sqrt((int32)(Op28X * Op28X + Op28Y * Op28Y + Op28Z * Op28Z));
}

short Op1CAZ;
unsigned short Op1CX, Op1CY, Op1CZ;
short Op1CXBR, Op1CYBR, Op1CZBR, Op1CXAR, Op1CYAR, Op1CZAR;
short Op1CX1;
short Op1CY1;
short Op1CZ1;
short Op1CX2;
short Op1CY2;
short Op1CZ2;

#ifdef __OPT1C__
void DSPOp1C()
{
   dsp_opcode[0x1C]++;

   short ya, xa, za;
   ya = Angle(Op1CX);
   xa = Angle(Op1CY);
   za = Angle(Op1CZ);

   // rotate around Z
   Op1CX1 = (Op1CXBR * Cos(za) + Op1CYBR * Sin(za));
   Op1CY1 = (Op1CXBR * -Sin(za) + Op1CYBR * Cos(za));
   Op1CZ1 = Op1CZBR;
   // rotate around Y
   Op1CX2 = (Op1CX1 * Cos(ya) + Op1CZ1 * -Sin(ya));
   Op1CY2 = Op1CY1;
   Op1CZ2 = (Op1CX1 * Sin(ya) + Op1CZ1 * Cos(ya));
   // rotate around X
   Op1CXAR = Op1CX2;
   Op1CYAR = (Op1CY2 * Cos(xa) + Op1CZ2 * Sin(xa));
   Op1CZAR = (Op1CY2 * -Sin(xa) + Op1CZ2 * Cos(xa));

}
#else
void DSPOp1C()
{
   double ya, xa, za;
   ya = Op1CX / 65536.0 * PI * 2;
   xa = Op1CY / 65536.0 * PI * 2;
   za = Op1CZ / 65536.0 * PI * 2;
   // rotate around Z
   Op1CX1 = (Op1CXBR * cos(za) + Op1CYBR * sin(za));
   Op1CY1 = (Op1CXBR * -sin(za) + Op1CYBR * cos(za));
   Op1CZ1 = Op1CZBR;
   // rotate around Y
   Op1CX2 = (Op1CX1 * cos(ya) + Op1CZ1 * -sin(ya));
   Op1CY2 = Op1CY1;
   Op1CZ2 = (Op1CX1 * sin(ya) + Op1CZ1 * cos(ya));
   // rotate around X
   Op1CXAR = Op1CX2;
   Op1CYAR = (Op1CY2 * cos(xa) + Op1CZ2 * sin(xa));
   Op1CZAR = (Op1CY2 * -sin(xa) + Op1CZ2 * cos(xa));

}

#endif
