//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 #include #include #include #include #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 + 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; }