//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 funcSADDMULT1616(a,b,c,d) (((int64)a*(int64)b+(int64)c*(int64)d)>>16);

#define SADDMULT1616(res,a,b,c,d) {\
   res=funcSADDMULT1616(a,b,c,d);\
   }
#define SMULT1616(res,a,b) {\
   res=funcSADDMULT1616(a,b,0,0);\
   }

#define __OPT__
#define __OPT01__
#define __OPT02__
#define __OPT04__
#define __OPT06__
#define __OPT0C__    // this optimisation may break pilotwings
#define __OPT11__
#define __OPT21__
#define __OPT1C__

const unsigned short DSP1ROM[1024] =
{
   0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x0001,  0x0002,  0x0004,  0x0008,  0x0010,  0x0020,
   0x0040,  0x0080,  0x0100,  0x0200,  0x0400,  0x0800,  0x1000,  0x2000,
   0x4000,  0x7fff,  0x4000,  0x2000,  0x1000,  0x0800,  0x0400,  0x0200,
   0x0100,  0x0080,  0x0040,  0x0020,  0x0001,  0x0008,  0x0004,  0x0002,
   0x0001,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,  0x0000,
   0x0000,  0x0000,  0x8000,  0xffe5,  0x0100,  0x7fff,  0x7f02,  0x7e08,
   0x7d12,  0x7c1f,  0x7b30,  0x7a45,  0x795d,  0x7878,  0x7797,  0x76ba,
   0x75df,  0x7507,  0x7433,  0x7361,  0x7293,  0x71c7,  0x70fe,  0x7038,
   0x6f75,  0x6eb4,  0x6df6,  0x6d3a,  0x6c81,  0x6bca,  0x6b16,  0x6a64,
   0x69b4,  0x6907,  0x685b,  0x67b2,  0x670b,  0x6666,  0x65c4,  0x6523,
   0x6484,  0x63e7,  0x634c,  0x62b3,  0x621c,  0x6186,  0x60f2,  0x6060,
   0x5fd0,  0x5f41,  0x5eb5,  0x5e29,  0x5d9f,  0x5d17,  0x5c91,  0x5c0c,
   0x5b88,  0x5b06,  0x5a85,  0x5a06,  0x5988,  0x590b,  0x5890,  0x5816,
   0x579d,  0x5726,  0x56b0,  0x563b,  0x55c8,  0x5555,  0x54e4,  0x5474,
   0x5405,  0x5398,  0x532b,  0x52bf,  0x5255,  0x51ec,  0x5183,  0x511c,
   0x50b6,  0x5050,  0x4fec,  0x4f89,  0x4f26,  0x4ec5,  0x4e64,  0x4e05,
   0x4da6,  0x4d48,  0x4cec,  0x4c90,  0x4c34,  0x4bda,  0x4b81,  0x4b28,
   0x4ad0,  0x4a79,  0x4a23,  0x49cd,  0x4979,  0x4925,  0x48d1,  0x487f,
   0x482d,  0x47dc,  0x478c,  0x473c,  0x46ed,  0x469f,  0x4651,  0x4604,
   0x45b8,  0x456c,  0x4521,  0x44d7,  0x448d,  0x4444,  0x43fc,  0x43b4,
   0x436d,  0x4326,  0x42e0,  0x429a,  0x4255,  0x4211,  0x41cd,  0x4189,
   0x4146,  0x4104,  0x40c2,  0x4081,  0x4040,  0x3fff,  0x41f7,  0x43e1,
   0x45bd,  0x478d,  0x4951,  0x4b0b,  0x4cbb,  0x4e61,  0x4fff,  0x5194,
   0x5322,  0x54a9,  0x5628,  0x57a2,  0x5914,  0x5a81,  0x5be9,  0x5d4a,
   0x5ea7,  0x5fff,  0x6152,  0x62a0,  0x63ea,  0x6530,  0x6672,  0x67b0,
   0x68ea,  0x6a20,  0x6b53,  0x6c83,  0x6daf,  0x6ed9,  0x6fff,  0x7122,
   0x7242,  0x735f,  0x747a,  0x7592,  0x76a7,  0x77ba,  0x78cb,  0x79d9,
   0x7ae5,  0x7bee,  0x7cf5,  0x7dfa,  0x7efe,  0x7fff,  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,  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,  0xfcdc,
   0xf9b9,  0xf696,  0xf375,  0xf055,  0xed38,  0xea1e,  0xe708,  0xe3f5,
   0xe0e7,  0xdddd,  0xdad8,  0xd7da,  0xd4e1,  0xd1ef,  0xcf05,  0xcc22,
   0xc946,  0xc674,  0xc3aa,  0xc0e9,  0xbe32,  0xbb86,  0xb8e4,  0xb64c,
   0xb3c1,  0xb141,  0xaecd,  0xac65,  0xaa0b,  0xa7be,  0xa57e,  0xa34c,
   0xa129,  0x9f14,  0x9d0e,  0x9b18,  0x9931,  0x975a,  0x9593,  0x93dc,
   0x9236,  0x90a1,  0x8f1e,  0x8dab,  0x8c4b,  0x8afc,  0x89bf,  0x8894,
   0x877c,  0x8676,  0x8583,  0x84a3,  0x83d7,  0x831d,  0x8276,  0x81e3,
   0x8163,  0x80f7,  0x809e,  0x8059,  0x8028,  0x800a,  0x6488,  0x0080,
   0x03ff,  0x0116,  0x0002,  0x0080,  0x4000,  0x3fd7,  0x3faf,  0x3f86,
   0x3f5d,  0x3f34,  0x3f0c,  0x3ee3,  0x3eba,  0x3e91,  0x3e68,  0x3e40,
   0x3e17,  0x3dee,  0x3dc5,  0x3d9c,  0x3d74,  0x3d4b,  0x3d22,  0x3cf9,
   0x3cd0,  0x3ca7,  0x3c7f,  0x3c56,  0x3c2d,  0x3c04,  0x3bdb,  0x3bb2,
   0x3b89,  0x3b60,  0x3b37,  0x3b0e,  0x3ae5,  0x3abc,  0x3a93,  0x3a69,
   0x3a40,  0x3a17,  0x39ee,  0x39c5,  0x399c,  0x3972,  0x3949,  0x3920,
   0x38f6,  0x38cd,  0x38a4,  0x387a,  0x3851,  0x3827,  0x37fe,  0x37d4,
   0x37aa,  0x3781,  0x3757,  0x372d,  0x3704,  0x36da,  0x36b0,  0x3686,
   0x365c,  0x3632,  0x3609,  0x35df,  0x35b4,  0x358a,  0x3560,  0x3536,
   0x350c,  0x34e1,  0x34b7,  0x348d,  0x3462,  0x3438,  0x340d,  0x33e3,
   0x33b8,  0x338d,  0x3363,  0x3338,  0x330d,  0x32e2,  0x32b7,  0x328c,
   0x3261,  0x3236,  0x320b,  0x31df,  0x31b4,  0x3188,  0x315d,  0x3131,
   0x3106,  0x30da,  0x30ae,  0x3083,  0x3057,  0x302b,  0x2fff,  0x2fd2,
   0x2fa6,  0x2f7a,  0x2f4d,  0x2f21,  0x2ef4,  0x2ec8,  0x2e9b,  0x2e6e,
   0x2e41,  0x2e14,  0x2de7,  0x2dba,  0x2d8d,  0x2d60,  0x2d32,  0x2d05,
   0x2cd7,  0x2ca9,  0x2c7b,  0x2c4d,  0x2c1f,  0x2bf1,  0x2bc3,  0x2b94,
   0x2b66,  0x2b37,  0x2b09,  0x2ada,  0x2aab,  0x2a7c,  0x2a4c,  0x2a1d,
   0x29ed,  0x29be,  0x298e,  0x295e,  0x292e,  0x28fe,  0x28ce,  0x289d,
   0x286d,  0x283c,  0x280b,  0x27da,  0x27a9,  0x2777,  0x2746,  0x2714,
   0x26e2,  0x26b0,  0x267e,  0x264c,  0x2619,  0x25e7,  0x25b4,  0x2581,
   0x254d,  0x251a,  0x24e6,  0x24b2,  0x247e,  0x244a,  0x2415,  0x23e1,
   0x23ac,  0x2376,  0x2341,  0x230b,  0x22d6,  0x229f,  0x2269,  0x2232,
   0x21fc,  0x21c4,  0x218d,  0x2155,  0x211d,  0x20e5,  0x20ad,  0x2074,
   0x203b,  0x2001,  0x1fc7,  0x1f8d,  0x1f53,  0x1f18,  0x1edd,  0x1ea1,
   0x1e66,  0x1e29,  0x1ded,  0x1db0,  0x1d72,  0x1d35,  0x1cf6,  0x1cb8,
   0x1c79,  0x1c39,  0x1bf9,  0x1bb8,  0x1b77,  0x1b36,  0x1af4,  0x1ab1,
   0x1a6e,  0x1a2a,  0x19e6,  0x19a1,  0x195c,  0x1915,  0x18ce,  0x1887,
   0x183f,  0x17f5,  0x17ac,  0x1761,  0x1715,  0x16c9,  0x167c,  0x162e,
   0x15df,  0x158e,  0x153d,  0x14eb,  0x1497,  0x1442,  0x13ec,  0x1395,
   0x133c,  0x12e2,  0x1286,  0x1228,  0x11c9,  0x1167,  0x1104,  0x109e,
   0x1036,  0x0fcc,  0x0f5f,  0x0eef,  0x0e7b,  0x0e04,  0x0d89,  0x0d0a,
   0x0c86,  0x0bfd,  0x0b6d,  0x0ad6,  0x0a36,  0x098d,  0x08d7,  0x0811,
   0x0736,  0x063e,  0x0519,  0x039a,  0x0000,  0x7fff,  0x0100,  0x0080,
   0x021d,  0x00c8,  0x00ce,  0x0048,  0x0a26,  0x277a,  0x00ce,  0x6488,
   0x14ac,  0x0001,  0x00f9,  0x00fc,  0x00ff,  0x00fc,  0x00f9,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,
   0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff,  0xffff
};



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

#define INCR 2048  //<<11
#define INCR_SHIFT 5 //16-11 coz (>>INCR_SHIFT) = (*INCR/65536)
#define _FIX_SHIFT_ 16
//double *CosTable2;
int32 CosTable2[INCR];
//double *SinTable2;
int32 SinTable2[INCR];

//#define Angle(x) (((x)/(65536/INCR)) & (INCR-1))
#define Angle(x) (((int32)(x)>>INCR_SHIFT) & (INCR-1) )
//#define Cos(x) ((double) CosTable2[x])
#define Cos(x) ((int32) CosTable2[x])
//#define Sin(x) ((double) SinTable2[x])
#define Sin(x) ((int32) SinTable2[x])
//#define PI 3.14159265358979323846264338327
#define PI_float 3.14159265358979323846264338327f
#define PI 205887 //3.14159265358979323846264338327 << _FIX_SHIFT_

//double Atan(double x)
int32 Atan(int32 x)
{
   // if ((x>=1) || (x<=1))    //stupid ?
   if ((x >= (1 << _FIX_SHIFT_)) || (x <= (1 << _FIX_SHIFT_))) //stupid ?
      //return (x/(1+0.28*x*x));
   {
      int32 t;
      SMULT1616(t, x, x)
      return (((int64)x << _FIX_SHIFT_)  / (1 + 0.28f * t));
   }
   else
      //return ( PI/2 - Atan(1/x));
      return (PI / 2 - Atan(((int64)1 << (_FIX_SHIFT_ * 2)) / x));
}

/***************************************************************************\
*  DSP1 code                                                                *
\***************************************************************************/


void InitDSP(void)
{
#ifdef __OPT__
   unsigned int i;
   //CosTable2 = (double *) malloc(INCR*sizeof(double));
   //CosTable2 = (int32 *) malloc(INCR*sizeof(int32));
   //SinTable2 = (double *) malloc(INCR*sizeof(double));
   //SinTable2 = (int32 *) malloc(INCR*sizeof(int32));
   for (i = 0; i < INCR; i++)
   {
      //CosTable2[i] = (cos((double)(2*PI*i/INCR)));
      CosTable2[i] = (int32)((cos((double)(2 * PI_float * i / INCR))) * (1 << _FIX_SHIFT_));
      //SinTable2[i] = (sin((double)(2*PI*i/INCR)));
      SinTable2[i] = (int32)((sin((double)(2 * PI_float * i / INCR))) * (1 << _FIX_SHIFT_));
   }
#endif
}


short Op00Multiplicand;
short Op00Multiplier;
short Op00Result;

void DSPOp00()
{
   //Op00Result=Op00Multiplicand*Op00Multiplier/32768;
   Op00Result = Op00Multiplicand * Op00Multiplier >> 15;
}

short Op20Multiplicand;
short Op20Multiplier;
short Op20Result;

void DSPOp20()
{
   Op20Result = Op20Multiplicand * Op20Multiplier >> 15;
   Op20Result++;
}


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

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


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

#ifdef __OPT04__

void DSPOp04()
{
   int angle;
   
   angle = Angle(Op04Angle);

   //Op04Sin = Sin(angle) * Op04Radius;
   //Op04Cos = Cos(angle) * Op04Radius;
   SMULT1616(Op04Sin, Sin(angle), Op04Radius)
   SMULT1616(Op04Cos, Cos(angle), 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()
{
   //Op0CX2=Op0CX1*Cos(Angle(Op0CA))+Op0CY1*Sin(Angle(Op0CA));
   //Op0CY2=Op0CX1*-Sin(Angle(Op0CA))+Op0CY1*Cos(Angle(Op0CA));
   Op0CX2 = ((int32)Op0CX1 * Cos(Angle(Op0CA)) + (int32)Op0CY1 * Sin(Angle(Op0CA))) >> _FIX_SHIFT_;
   Op0CY2 = ((int32)Op0CX1 * -Sin(Angle(Op0CA)) + (int32)Op0CY1 * Cos(Angle(Op0CA))) >> _FIX_SHIFT_;
}
#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;*/
int32 Op02CXF;
int32 Op02CYF;
int32 ViewerX0;
int32 ViewerY0;
int32 ViewerZ0;
int32 ViewerX1;
int32 ViewerY1;
int32 ViewerZ1;
int32 ViewerX;
int32 ViewerY;
int32 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;*/
int32 NumberOfSlope;
int32 ScreenX;
int32 ScreenY;
int32 ScreenZ;
int32 TopLeftScreenX;
int32 TopLeftScreenY;
int32 TopLeftScreenZ;
int32 BottomRightScreenX;
int32 BottomRightScreenY;
int32 BottomRightScreenZ;
int32 Ready;
int32 RasterLX;
int32 RasterLY;
int32 RasterLZ;
int32 ScreenLX1;
int32 ScreenLY1;
int32 ScreenLZ1;
int    ReversedLES;
short Op02LESb;
/*double NAzsB,NAasB;
double ViewerXc;
double ViewerYc;
double ViewerZc;
double CenterX,CenterY;*/
int32 NAzsB, NAasB;
int32 ViewerXc;
int32 ViewerYc;
int32 ViewerZc;
int32 CenterX, CenterY;
short Op02CYSup, Op02CXSup;
//double CXdistance;
int32 CXdistance;

#define VofAngle 0x3880

short TValDebug, TValDebug2;
short ScrDispl;


#ifdef __OPT02__
void DSPOp02()
{
   ViewerZ1 = -Cos(Angle(Op02AZS));
   /*  ViewerX1=Sin(Angle(Op02AZS))*Sin(Angle(Op02AAS));
      ViewerY1=Sin(Angle(Op02AZS))*Cos(Angle(Op02AAS));*/
   SMULT1616(ViewerX1, Sin(Angle(Op02AZS)), Sin(Angle(Op02AAS)))
   SMULT1616(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);*/
   ViewerX = ((int32)Op02FX << _FIX_SHIFT_) - ViewerX1 * (int32)Op02LFE;
   ViewerY = ((int32)Op02FY << _FIX_SHIFT_) - ViewerY1 * (int32)Op02LFE;
   ViewerZ = ((int32)Op02FZ << _FIX_SHIFT_) - ViewerZ1 * (int32)Op02LFE;

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

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

   //Op02CX=(short)(Op02CXF=ViewerX+ViewerX1*NumberOfSlope);
   //Op02CY=(short)(Op02CYF=ViewerY+ViewerY1*NumberOfSlope);
   int32 t;
   SMULT1616(t, ViewerX1, NumberOfSlope)
   Op02CX = (short)(Op02CXF = (ViewerX + t) >> _FIX_SHIFT_);
   SMULT1616(t, ViewerY1, NumberOfSlope)
   Op02CY = (short)(Op02CYF = (ViewerY + t) >> _FIX_SHIFT_);

   Op02VOF = 0x0000;
   ReversedLES = 0;
   Op02LESb = Op02LES;
   //if ((Op02LES>=VofAngle+16384.0) && (Op02LES<VofAngle+32768.0)) {
   if ((Op02LES >= VofAngle + 16384) && (Op02LES < VofAngle + 32768))
   {
      ReversedLES = 1;
      Op02LESb = VofAngle + 0x4000 - (Op02LES - (VofAngle + 0x4000));
   }
   //to be optimized here : tan
   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;
   NAzsB = (int32)(Op02AZS - 0x4000);
   //NAasB = Op02AAS*6.2832/65536.0;
   NAasB = (int32)(Op02AAS);

   //if (tan(NAzsB)==0) NAzsB=0.1;
   if (Sin(Angle(NAzsB)) == 0) NAzsB = 1043; //0.1*65536/(2*pi)

   ScrDispl = 0;
   //if (NAzsB>-0.15) {NAzsB=-0.15;ScrDispl=Op02VVA-0xFFDA;}
   if (NAzsB > -1565 /*0.15*65536/2/pi*/)
   {
      NAzsB = -1565;
      ScrDispl = Op02VVA - 0xFFDA;
   }

   //CXdistance=1/tan(NAzsB);
   CXdistance = ((int64)Cos(Angle(NAzsB)) << _FIX_SHIFT_) / Sin(Angle((NAzsB)));


   ViewerXc = (int32)Op02FX << _FIX_SHIFT_;
   ViewerYc = (int32)Op02FY << _FIX_SHIFT_;
   ViewerZc = (int32)Op02FZ << _FIX_SHIFT_;

   //CenterX = (-sin(NAasB)*ViewerZc*CXdistance)+ViewerXc;
   //CenterY = (cos(NAasB)*ViewerZc*CXdistance)+ViewerYc;
   //Op02CX = (short)CenterX;
   //Op02CY = (short)CenterY;
   SMULT1616(t, -Sin(Angle(NAasB)), ViewerZc)
   SMULT1616(t, t, CXdistance)
   CenterX = t + ViewerXc;
   SMULT1616(t, Cos(Angle(NAasB)), ViewerZc)
   SMULT1616(t, t, CXdistance)
   CenterY = t + ViewerYc;
   Op02CX = CenterX >> _FIX_SHIFT_;
   Op02CY = CenterY >> _FIX_SHIFT_;

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

   //CenterX = (-sin(NAasB)*ViewerZc*CXdistance)+ViewerXc;
   SMULT1616(t, -Sin(Angle(NAasB)), ViewerZc)
   SMULT1616(t, t, CXdistance)
   CenterX = t + 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;*/
   
   //BUG en puissance : overflow
   if (CenterX < (-32768 << _FIX_SHIFT_)) CenterX = (-32768 << _FIX_SHIFT_);
   if (CenterX > (32767 << _FIX_SHIFT_)) CenterX = (32767 << _FIX_SHIFT_);
   SMULT1616(t, Cos(Angle(NAasB)), ViewerZc)
   SMULT1616(t, t, CXdistance)
   CenterY = t + ViewerYc;
   if (CenterY < (-32768 << _FIX_SHIFT_)) CenterY = (-32768 << _FIX_SHIFT_);
   if (CenterY > (32767 << _FIX_SHIFT_)) CenterY = (32767 << _FIX_SHIFT_);

   //   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;*/

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

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



void GetRXYPos()
{
   int32 scalar;

   if (Op02LES == 0) return;


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

   /* if (cos(NAzs)==0) NAzs+=0.001;
    if (tan(NAzs)==0) NAzs+=0.001;*/
   if (Cos(Angle(NAzs)) == 0) NAzs += 10;
   if (Sin(Angle(NAzs)) == 0) NAzs += 10;

   /*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_float/2)*RHPos;
   RYRes += scalar*cos(NAas+PI_float/2)*RHPos;*/
   RXRes = ((int64) - Sin(Angle(NAas)) * (int64)ViewerZc / ((int64)(Sin(Angle(NAzs)) << _FIX_SHIFT_) / (int64)Cos(Angle(
               NAzs))) + ViewerXc);
   RYRes = ((int64)Cos(Angle(NAas)) * (int64)ViewerZc / ((int64)(Sin(Angle(NAzs)) << _FIX_SHIFT_) / (int64)Cos(Angle(
               NAzs))) + ViewerYc);
   scalar = ((ViewerZc / Sin(Angle(NAzs))) / (int32)Op02LES);
   int32 t;
   SMULT1616(t, -Sin(Angle(NAas + PI / 2)), RHPos)
   RXRes += scalar * t;
   SMULT1616(t, Cos(Angle(NAas + PI / 2)), RHPos)
   RYRes += scalar * t;
}

void DSPOp0A()
{
   //double x2,y2,x3,y3,x4,y4,m,ypos;
   int32 x2, y2, x3, y3, x4, y4, m, ypos;


   if (Op0AVS == 0)
   {
      Op0AVS++;
      return;
   }
   ypos = (int32)(Op0AVS - ScrDispl) << _FIX_SHIFT_;
   // 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 << _FIX_SHIFT_;
   GetRXYPos();
   x3 = RXRes;
   y3 = RYRes;
   // Get (127,Vs) coords (x4,y4)
   RVPos = ypos;
   RHPos = 127 << _FIX_SHIFT_;
   GetRXYPos();
   x4 = RXRes;
   y4 = RYRes;

   // A = (x4-x3)/256
   //m = (x4-x3)/256*256; if (m>32767) m=32767; if (m<-32768) m=-32768;
   m = (x4 - x3) >> 16;
   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;
   m = (y4 - y3) >> 16;
   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;
double DivideOp06;*/
int32 ObjPX;
int32 ObjPY;
int32 ObjPZ;
int32 ObjPX1;
int32 ObjPY1;
int32 ObjPZ1;
int32 ObjPX2;
int32 ObjPY2;
int32 ObjPZ2;
int32 DivideOp06;
int Temp;
int tanval2;

#ifdef __OPT06__
void DSPOp06()
{

   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));
   SADDMULT1616(ObjPX1, ObjPX, Cos(tanval2), ObjPY, -Sin(tanval2))
   //ObjPY1=(ObjPX*Sin(tanval2)+ObjPY*Cos(tanval2));
   SADDMULT1616(ObjPY1, ObjPX, Sin(tanval2), ObjPY, Cos(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));
   SADDMULT1616(ObjPY2, ObjPY1, Cos(tanval2), ObjPZ1, -Sin(tanval2))
   //ObjPZ2=(ObjPY1*Sin(tanval2)+ObjPZ1*Cos(tanval2));
   SADDMULT1616(ObjPZ2, ObjPY1, Sin(tanval2), ObjPZ1, Cos(tanval2))

   ObjPZ2 = ObjPZ2 - Op02LFE;

   if (ObjPZ2 < 0)
   {
      Op06H = (short)(-(int64)ObjPX2 * (int64)Op02LES / -(ObjPZ2)); //-ObjPX2*256/-ObjPZ2;
      Op06V = (short)(-(int64)ObjPY2 * (int64)Op02LES / -(ObjPZ2)); //-ObjPY2*256/-ObjPZ2;
      Op06S = (unsigned short)(256 * (int64)(Op02LES << _FIX_SHIFT_) / -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];*/
int32 matrixB[3][3];
int32 matrixB2[3][3];
int32 matrixB3[3][3];

int32 matrixA[3][3];
int32 matrixA2[3][3];
int32 matrixA3[3][3];


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

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

}


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;
int32 sc, sc2, sc3;



#ifdef __OPT01__
void DSPOp01()
{
   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()
{
   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()
{
   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()
{
   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()
{
   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()
{
   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()
{
   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()
{
   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()
{
   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;
int32 Op14Temp;
void DSPOp14()
{
   //TODO
   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()
{

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

}

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()
{
   //Op0BS = (Op0BX*matrixA[0][0]+Op0BY*matrixA2[0][1]+Op0BZ*matrixA2[0][2]);
   Op0BS = (Op0BX * matrixA[0][0] + Op0BY * matrixA2[0][1] + Op0BZ * matrixA2[0][2]) >> _FIX_SHIFT_;
}

void DSPOp1B()
{
   //Op1BS = (Op1BX*matrixA2[0][0]+Op1BY*matrixA2[0][1]+Op1BZ*matrixA2[0][2]);
   Op1BS = (Op1BX * matrixA2[0][0] + Op1BY * matrixA2[0][1] + Op1BZ * matrixA2[0][2]) >> _FIX_SHIFT_;

}

void DSPOp2B()
{
   //Op2BS = (Op2BX*matrixA3[0][0]+Op2BY*matrixA3[0][1]+Op2BZ*matrixA3[0][2]);
   Op2BS = (Op2BX * matrixA3[0][0] + Op2BY * matrixA3[0][1] + Op2BZ * matrixA3[0][2]) >> _FIX_SHIFT_;
}

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

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

short Op18X, Op18Y, Op18Z, Op18R, Op18D;

void DSPOp18()
{
   //double x,y,z,r;
   int32 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 Op38X, Op38Y, Op38Z, Op38R, Op38D;

void DSPOp38()
{
   Op38D = (Op38X * Op38X + Op38Y * Op38Y + Op38Z * Op38Z - Op38R * Op38R) >> 15;
   Op38D++;

}


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

void DSPOp28()
{
   //to optimize... sqrt
   Op28R = (short)sqrt((double)(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()
{
   short ya, xa, za;
   ya = Angle(Op1CX);
   xa = Angle(Op1CY);
   za = Angle(Op1CZ);

   // rotate around Z
   //Op1CX1=(Op1CXBR*Cos(za)+Op1CYBR*Sin(za));
   Op1CX1 = (Op1CXBR * Cos(za) + Op1CYBR * Sin(za)) >> _FIX_SHIFT_;
   //Op1CY1=(Op1CXBR*-Sin(za)+Op1CYBR*Cos(za));
   Op1CY1 = (Op1CXBR * -Sin(za) + Op1CYBR * Cos(za)) >> _FIX_SHIFT_;
   Op1CZ1 = Op1CZBR;
   // rotate around Y
   //Op1CX2=(Op1CX1*Cos(ya)+Op1CZ1*-Sin(ya));
   Op1CX2 = (Op1CX1 * Cos(ya) + Op1CZ1 * -Sin(ya)) >> _FIX_SHIFT_;
   Op1CY2 = Op1CY1;
   //Op1CZ2=(Op1CX1*Sin(ya)+Op1CZ1*Cos(ya));
   Op1CZ2 = (Op1CX1 * Sin(ya) + Op1CZ1 * Cos(ya)) >> _FIX_SHIFT_;
   // rotate around X
   Op1CXAR = Op1CX2;
   //Op1CYAR=(Op1CY2*Cos(xa)+Op1CZ2*Sin(xa));
   Op1CYAR = (Op1CY2 * Cos(xa) + Op1CZ2 * Sin(xa)) >> _FIX_SHIFT_;
   //Op1CZAR=(Op1CY2*-Sin(xa)+Op1CZ2*Cos(xa));
   Op1CZAR = (Op1CY2 * -Sin(xa) + Op1CZ2 * Cos(xa)) >> _FIX_SHIFT_;
}
#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

unsigned short Op0FRamsize;
unsigned short Op0FPass;

void DSPOp0F()
{
   Op0FPass = 0x0000;
}


short Op2FUnknown;
short Op2FSize;

void DSPOp2F()
{
   Op2FSize = 0x100;
}
