home *** CD-ROM | disk | FTP | other *** search
/ Virtual Reality Homebrewer's Handbook / vr.iso / vroom / headtrac.pas < prev    next >
Pascal/Delphi Source File  |  1996-03-19  |  6KB  |  187 lines
  1. unit headtracker;
  2.  
  3. interface
  4.  
  5. uses vroom,crt,fix_math;
  6.  
  7. procedure Initialise_Head_Tracker;
  8. procedure Scale_Head_Tracker(scale_val : real);
  9. procedure Read_Head_Tracker;
  10.  
  11. var
  12.      htz, hts : array[0..5] of real;
  13.      last_ht_angle : array[0..5] of integer;
  14.      l0, l1, l2, l3, l4, thx, thy, thz : longint;
  15.      l0a, l1a, l2a, l3a, l4a, thxa, thya, thza : longint;
  16.  
  17. implementation
  18.  
  19. procedure Initialise_Head_Tracker;
  20. const adport=624;
  21.       delay_no=20;
  22. var channel, junk : integer;
  23. begin
  24.      htz[0]:=1800;
  25.      htz[1]:=2065 {2025};
  26.      htz[2]:=2010 {2080};
  27.      htz[3]:=1831 {1859};
  28.      htz[4]:=1950 {2160};
  29.      htz[5]:=1835 {1816};
  30.      hts[0]:=0.96;
  31.      hts[1]:=-0.96;
  32.      hts[2]:=-0.96;
  33.      hts[3]:=0.96;
  34.      hts[4]:=-0.96;
  35.      hts[5]:=-0.96;
  36.      l0a:=0;
  37.      l1a:=0;
  38.      l2a:=0;
  39.      l3a:=0;
  40.      l4a:=0;
  41.      thxa:=0;
  42.      thya:=0;
  43.      thza:=0;
  44.      for channel:=0 to 5 do
  45.      begin
  46.           port[adport+3]:=0;
  47.           port[adport]:=channel;
  48.           junk:=port[adport+4];
  49.           for junk:=1 to delay_no do;
  50.           junk:=port[adport+4];
  51.           for junk:=1 to delay_no do;
  52.           junk:=port[adport+4];
  53.           for junk:=1 to delay_no do;
  54.           junk:=port[adport+4];
  55.           for junk:=1 to delay_no do;
  56.           junk:=port[adport+4];
  57.           for junk:=1 to delay_no do;
  58.           junk:=port[adport+4];
  59.           for junk:=1 to delay_no do;
  60.           junk:=port[adport+4];
  61.           for junk:=1 to delay_no do;
  62.           junk:=port[adport+5];
  63.           for junk:=1 to delay_no do;
  64.           junk:=port[adport+5];
  65.           for junk:=1 to delay_no do;
  66.           junk:=port[adport+5];
  67.           for junk:=1 to delay_no do;
  68.           junk:=port[adport+5];
  69.           for junk:=1 to delay_no do;
  70.           junk:=port[adport+5];
  71.           for junk:=1 to delay_no do;
  72.           junk:=port[adport+5];
  73.           for junk:=1 to delay_no do;
  74.           junk:=port[adport+5];
  75.           for junk:=1 to delay_no do;
  76.           last_ht_angle[channel]:=(port[adport+2] and 15)*256+(port[adport+1]);
  77.      end;
  78. end;
  79.  
  80. procedure Scale_Head_Tracker(scale_val : real);
  81. var fixed_scale_val : longint;
  82. begin
  83.      fixed_scale_val:=real_to_fixed(scale_val);
  84.      l0:=fixedmul(l0a,fixed_scale_val);
  85.      l1:=fixedmul(l1a,fixed_scale_val);
  86.      l2:=fixedmul(l2a,fixed_scale_val);
  87.      l3:=fixedmul(l3a,fixed_scale_val);
  88.      l4:=fixedmul(l4a,fixed_scale_val);
  89.      thx:=fixedmul(thxa,fixed_scale_val);
  90.      thy:=fixedmul(thya,fixed_scale_val);
  91.      thz:=fixedmul(thza,fixed_scale_val);
  92. end;
  93.  
  94. procedure Read_Head_Tracker;
  95. const adport=624;
  96.       delay_no=20;
  97. var channel,junk : integer;
  98.     angle, anglead : array [0..5] of integer;
  99.     mat0, mat1, mat2, mat3, mat4, mat5, tempmat1, tempmat2, boom_tfm : fixed_matrix_4x4;
  100.     cth0, cth1, cth2, cth3, cth4, cth5 : longint;
  101.     sth0, sth1, sth2, sth3, sth4, sth5 : longint;
  102. begin
  103.      for channel:=0 to 5 do
  104.      begin
  105.           port[adport+3]:=0;
  106.           port[adport]:=channel;
  107.           junk:=port[adport+4];
  108.           for junk:=1 to delay_no do;
  109.           junk:=port[adport+4];
  110.           for junk:=1 to delay_no do;
  111.           junk:=port[adport+4];
  112.           for junk:=1 to delay_no do;
  113.           junk:=port[adport+4];
  114.           for junk:=1 to delay_no do;
  115.           junk:=port[adport+4];
  116.           for junk:=1 to delay_no do;
  117.           junk:=port[adport+4];
  118.           for junk:=1 to delay_no do;
  119.           junk:=port[adport+4];
  120.           for junk:=1 to delay_no do;
  121.           junk:=port[adport+5];
  122.           for junk:=1 to delay_no do;
  123.           junk:=port[adport+5];
  124.           for junk:=1 to delay_no do;
  125.           junk:=port[adport+5];
  126.           for junk:=1 to delay_no do;
  127.           junk:=port[adport+5];
  128.           for junk:=1 to delay_no do;
  129.           junk:=port[adport+5];
  130.           for junk:=1 to delay_no do;
  131.           junk:=port[adport+5];
  132.           for junk:=1 to delay_no do;
  133.           junk:=port[adport+5];
  134.           for junk:=1 to delay_no do;
  135.           anglead[channel]:=(port[adport+2] and 15)*256+(port[adport+1]);
  136.           if abs(last_ht_angle[channel]-anglead[channel])>4 then last_ht_angle[channel]:=anglead[channel]
  137.           else anglead[channel]:=last_ht_angle[channel];
  138.           angle[channel]:=round((anglead[channel]-htz[channel])*hts[channel]);
  139.      end;
  140.  
  141.      CosSin(angle[0],cth0,sth0);
  142.      CosSin(angle[1],cth1,sth1);
  143.      CosSin(angle[2],cth2,sth2);
  144.      CosSin(angle[3],cth3,sth3);
  145.      CosSin(angle[4],cth4,sth4);
  146.      CosSin(angle[5],cth5,sth5);
  147.  
  148.      mat0.data( cth0, sth0,     0,     0,
  149.                -sth0, cth0,     0,     0,
  150.                    0,    0, 65536,     0,
  151.                   l0,    0,     0, 65536);
  152.  
  153.      mat1.data(cth1,     0, -sth1,     0,
  154.                   0, 65536,     0,     0,
  155.                sth1,     0,  cth1,     0,
  156.                  l1,     0,     0, 65536);
  157.  
  158.      mat2.data(cth2,     0, -sth2,     0,
  159.                   0, 65536,     0,     0,
  160.                sth2,     0,  cth2,     0,
  161.                  l2,     0,     0, 65536);
  162.  
  163.      mat3.data(cth3,     0, -sth3,     0,
  164.                   0, 65536,     0,     0,
  165.                sth3,     0,  cth3,     0,
  166.                   0,     0,    l3, 65536);
  167.  
  168.      mat4.data(65536,     0,    0,     0,
  169.                    0,  cth4, sth4,     0,
  170.                    0, -sth4, cth4,     0,
  171.                    0,     0,   l4, 65536);
  172.  
  173.      mat5.data( cth5, sth5,       0,     0,
  174.                -sth5, cth5,       0,     0,
  175.                    0,    0,   65536,     0,
  176.                  thx,  thy,     thz, 65536);
  177.  
  178.      multiply_4x4(mat0, mat1, tempmat1);
  179.      multiply_4x4(tempmat1, mat2, tempmat2);
  180.      multiply_4x4(tempmat2, mat3, tempmat1);
  181.      multiply_4x4(tempmat1, mat4, tempmat2);
  182.      multiply_4x4(tempmat2, mat5, boom_tfm);
  183.      multiply_4x4(w2v,boom_tfm,main_matrix);
  184.      redraw_all:=true;
  185. end;
  186.  
  187. end.