home *** CD-ROM | disk | FTP | other *** search
/ Amiga ACS 1998 #6 / amigaacscoverdisc1998-061998.iso / games / descent / source / main / iglasses.c < prev    next >
C/C++ Source or Header  |  1998-06-08  |  9KB  |  385 lines
  1. /*
  2. THE COMPUTER CODE CONTAINED HEREIN IS THE SOLE PROPERTY OF PARALLAX
  3. SOFTWARE CORPORATION ("PARALLAX").  PARALLAX, IN DISTRIBUTING THE CODE TO
  4. END-USERS, AND SUBJECT TO ALL OF THE TERMS AND CONDITIONS HEREIN, GRANTS A
  5. ROYALTY-FREE, PERPETUAL LICENSE TO SUCH END-USERS FOR USE BY SUCH END-USERS
  6. IN USING, DISPLAYING,  AND CREATING DERIVATIVE WORKS THEREOF, SO LONG AS
  7. SUCH USE, DISPLAY OR CREATION IS FOR NON-COMMERCIAL, ROYALTY OR REVENUE
  8. FREE PURPOSES.  IN NO EVENT SHALL THE END-USER USE THE COMPUTER CODE
  9. CONTAINED HEREIN FOR REVENUE-BEARING PURPOSES.  THE END-USER UNDERSTANDS
  10. AND AGREES TO THE TERMS HEREIN AND ACCEPTS THE SAME BY USE OF THIS FILE.  
  11. COPYRIGHT 1993-1998 PARALLAX SOFTWARE CORPORATION.  ALL RIGHTS RESERVED.
  12. */
  13. /*
  14.  * $Source: f:/miner/source/main/rcs/iglasses.c $
  15.  * $Revision: 2.8 $
  16.  * $Author: mike $
  17.  * $Date: 1995/03/30 16:36:31 $
  18.  * 
  19.  * Routines for the i-glasses VR stuff.
  20.  * 
  21.  * $Log: iglasses.c $
  22.  * Revision 2.8  1995/03/30  16:36:31  mike
  23.  * text localization.
  24.  * 
  25.  * Revision 2.7  1995/03/24  13:10:37  john
  26.  * Synced with shareware.
  27.  * 
  28.  * Revision 2.6  1995/03/09  18:07:35  john
  29.  * Fixed bug with iglasses tracking not "centering" right.
  30.  * Made VFX have bright headlight lighting.
  31.  * 
  32.  * Revision 2.5  1995/03/09  15:33:23  john
  33.  * Fixed bug with iglasses timeout too long, and objects
  34.  * disappearing from left eye.
  35.  * 
  36.  * Revision 2.4  1995/03/06  15:23:13  john
  37.  * New screen techniques.
  38.  * 
  39.  * Revision 2.3  1995/03/03  22:38:30  john
  40.  * Tweaked the filtering,.
  41.  * 
  42.  * Revision 2.2  1995/03/03  22:28:22  john
  43.  * Added code to detect invalid serial ports.
  44.  * 
  45.  * Revision 2.1  1995/03/03  22:24:00  john
  46.  * Added code to make iglasses work with Greenleaf.
  47.  * 
  48.  * Revision 1.2  1995/02/09  22:01:01  john
  49.  * Added i-glasses tracking.
  50.  * 
  51.  * Revision 1.1  1995/02/09  15:53:47  john
  52.  * Initial revision
  53.  * 
  54.  * 
  55.  */
  56.  
  57.  
  58. #pragma off (unreferenced)
  59. static char rcsid[] = "$Id: iglasses.c 2.8 1995/03/30 16:36:31 mike Exp $";
  60. #pragma on (unreferenced)
  61.  
  62. #define DOS4G        
  63.  
  64. #include <stdio.h>
  65. #include <stdlib.h>
  66. #include <math.h>
  67. #include <conio.h>
  68.  
  69. #include "error.h"
  70. #include "mono.h"
  71. #include "args.h"
  72. #include "text.h"
  73. #include "iglasses.h"
  74. #include "key.h"
  75. //#include "commlib.h" //nien - These aren't included -KRB
  76. //#include "fast.h"
  77. #include "timer.h"
  78.  
  79. int iglasses_headset_installed=0;
  80.  
  81. void iglasses_close_tracking();
  82.  
  83. //*******************************************
  84.  
  85. typedef struct  {
  86.     int status;
  87.     int count;
  88.     
  89. } PORT; //I added this it will compile, but I doubt it works. -KRB
  90. //*******************************************
  91.  
  92.  
  93. PORT * Iport = NULL;
  94.  
  95. #define USE_FILTERS 1
  96.  
  97. #ifdef USE_FILTERS 
  98. #define FILTER_LENGTH 6
  99. typedef struct filter {
  100.   fix history[FILTER_LENGTH];
  101.   fix weights[FILTER_LENGTH];
  102.   long len;
  103.   fix * hCurrent,* hEnd,* hRestart;
  104. } filter;  
  105.  
  106. void initFIR(filter * f);
  107. fix filterFIR(filter * f,fix newval);
  108. static filter X_filter, Y_filter, Z_filter;
  109. #endif
  110.  
  111. void iglasses_init_tracking(int serial_port)    
  112. {
  113.     fix t1;
  114.     int c;
  115.  
  116.     if (iglasses_headset_installed) return;
  117.  
  118.     if ( (serial_port < 1) || (serial_port > 4) )    {
  119.         Error( TXT_IGLASSES_ERROR_1 );
  120.     }
  121.     printf( "\n\n%s %d\n", TXT_IGLASSES_INIT, serial_port );
  122.     printf( "%s\n", TXT_IGLASSES_ON);
  123.     printf( "%s\n", TXT_PRESS_ESC_TO_ABORT);
  124.     Iport = PortOpenGreenleafFast(serial_port-1, 9600, 'N', 8, 1 );
  125.     if ( !Iport )    {
  126.         printf( "%s\n", TXT_SERIAL_FAILURE, Iport->status );
  127.         return;
  128.     }
  129.     
  130.     SetDtr( Iport, 1 );
  131.     SetRts( Iport, 1 );
  132.     UseRtsCts( Iport, 0 );
  133.     
  134.     iglasses_headset_installed = 1;
  135.     atexit( iglasses_close_tracking );
  136.  
  137.     while( 1 )    {
  138.         printf( "." );
  139.         t1 = timer_get_fixed_seconds() + F1_0;
  140.         ClearRXBuffer(Iport);
  141.         ClearTXBuffer(Iport);
  142.         WriteBuffer( Iport, "!\r", 2 );
  143.         while ( timer_get_fixed_seconds() < t1 )    {
  144.             if ( key_inkey() == KEY_ESC ) exit(0);
  145.             c = ReadChar( Iport );
  146.             if ( c == 'O' )    {
  147.                 goto TrackerOK1;
  148.             }     
  149.         }
  150.     }
  151.  
  152. TrackerOK1:
  153.  
  154.     while( 1 )    {
  155.         printf( "." );
  156.         t1 = timer_get_fixed_seconds() + F1_0;
  157.         ClearRXBuffer(Iport);
  158.         ClearTXBuffer(Iport);
  159.         // M2 = p,b,h
  160.         // M1 = all data
  161.         WriteBuffer( Iport, "!M1,P,B\r", 8 );
  162.         while ( timer_get_fixed_seconds() < t1 )    {
  163.             if ( key_inkey() == KEY_ESC ) exit(0);
  164.             c = ReadChar( Iport );
  165.             if ( c == 'O' )    {
  166.                 goto TrackerOK2;
  167.             }     
  168.         }
  169.     }
  170.  
  171. TrackerOK2:
  172.     printf( ".\n" );
  173.     ClearRXBuffer(Iport);
  174.     ClearTXBuffer(Iport);
  175.  
  176.      WriteChar( Iport, 'S' );
  177.  
  178. #ifdef USE_FILTERS 
  179.     initFIR( &X_filter );
  180.     initFIR( &Y_filter );
  181.     initFIR( &Z_filter );
  182. #endif
  183.  
  184. //    {
  185. //        fix y,p,r;
  186. //        while( 1 )    {
  187. //            iglasses_read_headset( &y, &p, &r);
  188. //             //printf( "%d\t%d\t%d\n", y, p, r );
  189. //             printf( "%8.2f\n", f2fl(y) );
  190. //            if (key_inkey()==KEY_ESC) break;
  191. //        }
  192. //    }
  193.  
  194. }
  195.  
  196. void iglasses_close_tracking()    {
  197.     if ( iglasses_headset_installed )    {
  198.         iglasses_headset_installed = 0;
  199.         PortClose(Iport);
  200.         Iport =  NULL;
  201.     }
  202. }
  203.  
  204. //UNUSED int iglasses_read_headset1( fix *yaw, fix *pitch, fix *roll )
  205. //UNUSED {
  206. //UNUSED     int y,p,r;
  207. //UNUSED     static unsigned char buff[8];
  208. //UNUSED     unsigned char checksum;
  209. //UNUSED     int i,count;
  210. //UNUSED 
  211. //UNUSED     ReadBufferTimed(Iport, buff, 8, 1000);
  212. //UNUSED     checksum = 0;
  213. //UNUSED     count = Iport->count;
  214. //UNUSED     for (i=0; i < 7; i++) checksum += buff[i];
  215. //UNUSED     if ( (count < 8) || (checksum != buff[7]) )    {
  216. //UNUSED         ClearRXBuffer(Iport);
  217. //UNUSED         WriteChar( Iport, 'S' );
  218. //UNUSED         *yaw = *pitch = *roll = 0;
  219. //UNUSED         return 0;
  220. //UNUSED     }
  221. //UNUSED     WriteChar( Iport, 'S' );
  222. //UNUSED 
  223. //UNUSED     y  =  (short)(buff[1] << 8) | buff[2];
  224. //UNUSED     p  =  (short)(buff[3] << 8) | buff[4];
  225. //UNUSED     r  =  (short)(buff[5] << 8) | buff[6];
  226. //UNUSED 
  227. //UNUSED     *yaw         = y;
  228. //UNUSED     *pitch     = p;
  229. //UNUSED     *roll     = r;
  230. //UNUSED 
  231. //UNUSED     return 1;
  232. //UNUSED }
  233.  
  234. #define M_PI 3.14159265358979323846264338327950288
  235. #define FBITS 16384.
  236. #define TO_RADIANS (M_PI/FBITS)
  237. #define TO_DEGREES (180./M_PI)
  238. #define TO_FIX (0.5/M_PI)
  239.  
  240. fix i_yaw = 0;
  241. fix i_pitch = 0;
  242. fix i_roll = 0;
  243.  
  244. int Num_readings = 0;
  245. fix BasisYaw, BasisPitch, BasisBank;
  246.  
  247. int iglasses_read_headset( fix *yaw, fix *pitch, fix *roll )
  248. {
  249.     static unsigned char buff[16];
  250.     float radPitch,radRoll,sinPitch,sinRoll,cosPitch,cosRoll;
  251.     float rotx,roty,rotz;
  252.     float fx,fy,fz;
  253.     long x,y,z,p,r;
  254.     unsigned char checksum;
  255.     long i,count;
  256.  
  257.     ReadBufferTimed(Iport, buff, 12, 10 );            // Wait 1/100 second for timeout.
  258.     checksum = 0;
  259.     count = Iport->count;
  260.     ClearRXBuffer(Iport);
  261.     WriteChar( Iport, 'S' );
  262.     for (i=0; i < 11; i++) checksum += buff[i];
  263.     if ( (count < 12) || (checksum != buff[11]) )    {
  264.         *yaw = i_yaw;
  265.         *pitch = i_pitch;
  266.         *roll = i_roll;
  267.         return 0;
  268.     }
  269.  
  270.     x  =  (short)(buff[1] << 8) | buff[2];
  271.     y  =  (short)(buff[3] << 8) | buff[4];
  272.     z  =  (short)(buff[5] << 8) | buff[6];
  273.     p  =  (short)(buff[7] << 8) | buff[8];
  274.     r  =  (short)(buff[9] << 8) | buff[10];
  275.  
  276.     fx = (float)x/FBITS;
  277.     fy = (float)y/FBITS;
  278.     fz = (float)z/FBITS;
  279.     radPitch = (float)p*TO_RADIANS;
  280.     radRoll  = (float)r*TO_RADIANS;
  281.     sinPitch = sin(radPitch);
  282.     cosPitch = cos(radPitch);
  283.     sinRoll  = sin(radRoll);
  284.     cosRoll  = cos(radRoll);
  285.  
  286.     roty = cosPitch*fy - sinPitch*fz;
  287.     rotz = sinPitch*fy + cosPitch*fz;
  288.     rotx = cosRoll*fx  - sinRoll*roty;
  289.  
  290. #ifdef USE_FILTERS 
  291.     *yaw   = filterFIR( &X_filter,fl2f(-atan2(rotz,rotx)*M_PI/2.0));
  292.     *pitch = filterFIR( &Y_filter,fl2f(-radPitch*M_PI/2.0));
  293.     *roll  = filterFIR( &Z_filter,fl2f(radRoll*M_PI/2.0));
  294.     if ( Num_readings < 30 )    {
  295.         Num_readings++;
  296.         BasisYaw = *yaw;
  297.         BasisPitch = *pitch;
  298.         BasisBank = *roll;
  299.     }
  300.     *yaw -= BasisYaw;
  301.     *pitch -= BasisPitch;
  302.     *roll -= BasisBank;
  303. #else
  304.     *yaw   = fl2f(-atan2(rotz,rotx)*M_PI/2.0);
  305.     *pitch = fl2f(-radPitch*M_PI/2.0);
  306.     *roll  = fl2f(radRoll*M_PI/2.0);
  307. #endif
  308.  
  309.     i_yaw = *yaw;
  310.     i_pitch = *pitch;
  311.     i_roll = *roll;
  312.  
  313.     return 1;
  314. }
  315.  
  316.  
  317. #ifdef USE_FILTERS 
  318. void initWeights(filter * f) 
  319. {
  320.     fix sum;
  321.     long i;
  322.  
  323.     // Generate weights.
  324.     sum = 0;
  325.     for (i=0; i < f->len; i++) {
  326.         //f->weights[i] = i;        // Linear ramp
  327.         //f->weights[i] = i*i;        // Exp ramp
  328.         f->weights[i] = F1_0;        // Average
  329.  
  330.     }
  331.  
  332.     // Summate.
  333.     for (i=0; i < f->len; i++) {
  334.         sum += f->weights[i];
  335.     }
  336.  
  337.     // Normalize and convert to fixed point.
  338.     for (i=0; i < f->len; i++) {
  339.         f->weights[i] = fixdiv(f->weights[i],sum);
  340.     } 
  341. }
  342.  
  343. void initHistory(filter * f) 
  344. {
  345.     long i;
  346.     for (i=0; i < f->len; i++) {
  347.         f->history[i] = F1_0;
  348.     }
  349.     f->hCurrent = f->history;
  350.     f->hEnd     = &f->history[f->len-1];
  351.     f->hRestart = &f->history[-1];
  352. } 
  353.  
  354. void initFIR(filter * f) 
  355. {
  356.   f->len = FILTER_LENGTH;
  357.   initWeights(f);
  358.   initHistory(f);
  359. } 
  360.  
  361. fix filterFIR(filter * f,fix newval) 
  362. {
  363.     fix * currp,* last;
  364.     fix * weightp;
  365.     fix sum;
  366.  
  367.     currp   = f->hCurrent;
  368.     last    = f->hCurrent;
  369.     weightp = f->weights;
  370.  
  371.     *(f->hCurrent)++ = newval;
  372.     if (f->hCurrent == &f->history[f->len]) f->hCurrent = f->history;
  373.  
  374.     // Compute FIR filter
  375.     sum = 0;
  376.     while (1) {
  377.         sum += fixmul( (*currp--), (*weightp++) );
  378.         if (currp == f->hRestart) currp = f->hEnd;
  379.         if (currp == last) break;
  380.     } /* while */
  381.     return sum/f->len;
  382. } /* filterFIR */
  383.  
  384. #endif
  385.