home *** CD-ROM | disk | FTP | other *** search
/ Virtual Reality Zone / VRZONE.ISO / mac / PC / PCGLOVE / GLOVE / OBJGLV.ZIP / SRC / DEMO4B / INT / MATRIX.CPP < prev    next >
C/C++ Source or Header  |  1993-01-01  |  17KB  |  1,037 lines
  1. /* Matrix math, assembly by Dave Stampe */
  2.  
  3. /* Copyright 1992 by Dave Stampe and Bernie Roehl.
  4.      May be freely used to write software for release into the public domain;
  5.      all commercial endeavours MUST contact Bernie Roehl and Dave Stampe
  6.      for permission to incorporate any part of this software into their
  7.      products!
  8.  */
  9.  
  10. /* Contact: broehl@sunee.waterloo.edu or dstampe@sunee.waterloo.edu */
  11.  
  12. #pragma inline
  13.  
  14. #include <string.h>
  15.  
  16. #include "3dstruct.hpp"
  17.  
  18. #define XFSC 536870912   /* 2**29 for shifting xform coeffs to long */
  19.  
  20. /*************** TRIGNOMETRIC FUNCTIONS *****************/
  21.  
  22. /* (from inttrig.c) */
  23.  
  24. extern long isine(long x);
  25. extern long icosine(long x);
  26. extern long arcsine(long x);
  27. extern long arccosine(long x);
  28. extern long arctan2(long y, long x);
  29.  
  30. /***************** MATRIX-VECTOR OPERATIONS **************/
  31.  
  32.                 /* rotate/translate XYZ by matrix */
  33. // essentially,
  34. // long x = *xp, y = *yp, z = *zp;
  35. // *xp = m[0][0] * x + m[0][1] * y + m[0][2] * z + m[3][0];
  36. // *yp = m[1][0] * x + m[1][1] * y + m[1][2] * z + m[3][1];
  37. // *zp = m[2][0] * x + m[2][1] * y + m[2][2] * z + m[3][2];
  38. //
  39.  
  40. void matrix_point(MATRIX m, long *xp, long *yp, long *zp)
  41. {
  42.  long x = *xp;
  43.  long y = *yp;
  44.  long z = *zp;
  45.  
  46.  long xr,yr,zr;
  47.  
  48.  asm {
  49.     .386
  50.     push    si
  51.     push    di
  52.     push    cx
  53.     push    dx
  54.     les    di,DWORD PTR m
  55.  
  56.     mov    eax,es:[di]
  57.     imul    DWORD PTR x
  58.     mov    ecx,edx
  59.     mov    ebx,eax
  60.     mov    eax,es:[di+4]
  61.     imul    DWORD PTR y
  62.     add    ebx,eax
  63.     adc    ecx,edx
  64.     mov    eax,es:[di+8]
  65.     imul    DWORD PTR z
  66.     add    eax,ebx
  67.     adc    edx,ecx
  68.     shrd    eax,edx,29
  69.     adc    eax,es:[di+36]
  70.     mov    xr,eax
  71.  
  72.     mov    eax,es:[di+12]
  73.     imul    DWORD PTR x
  74.     mov    ecx,edx
  75.     mov    ebx,eax
  76.     mov    eax,es:[di+16]
  77.     imul    DWORD PTR y
  78.     add    ebx,eax
  79.     adc    ecx,edx
  80.     mov    eax,es:[di+20]
  81.     imul    DWORD PTR z
  82.     add    eax,ebx
  83.     adc    edx,ecx
  84.     shrd    eax,edx,29
  85.     adc    eax,es:[di+40]
  86.     mov    yr,eax
  87.  
  88.     mov    eax,es:[di+24]
  89.     imul    DWORD PTR x
  90.     mov    ecx,edx
  91.     mov    ebx,eax
  92.     mov    eax,es:[di+28]
  93.     imul    DWORD PTR y
  94.     add    ebx,eax
  95.     adc    ecx,edx
  96.     mov    eax,es:[di+32]
  97.     imul    DWORD PTR z
  98.     add    eax,ebx
  99.     adc    edx,ecx
  100.     shrd    eax,edx,29
  101.     adc    eax,es:[di+44]
  102.     mov    zr,eax
  103.  
  104.     pop    dx
  105.     pop    cx
  106.     pop    di
  107.     pop    si
  108.          }
  109.  
  110.  *xp = xr;
  111.  *yp = yr;
  112.  *zp = zr;
  113. }
  114.  
  115.                 /* rotate XYZ by matrix */
  116.  
  117. void matrix_rotate(MATRIX m, long *xp, long *yp, long *zp)
  118. {
  119.  long x = *xp;
  120.  long y = *yp;
  121.  long z = *zp;
  122.  
  123.  long xr,yr,zr;
  124.  
  125.  asm {
  126.     .386
  127.     push    si
  128.     push    di
  129.     push    cx
  130.     push    dx
  131.     les    di,DWORD PTR m
  132.  
  133.     mov    eax,es:[di]
  134.     imul    DWORD PTR x
  135.     mov    ecx,edx
  136.     mov    ebx,eax
  137.     mov    eax,es:[di+4]
  138.     imul    DWORD PTR y
  139.     add    ebx,eax
  140.     adc    ecx,edx
  141.     mov    eax,es:[di+8]
  142.     imul    DWORD PTR z
  143.     add    eax,ebx
  144.     adc    edx,ecx
  145.     shrd    eax,edx,29
  146.     adc    eax,0
  147.     mov    xr,eax
  148.  
  149.     mov    eax,es:[di+12]
  150.     imul    DWORD PTR x
  151.     mov    ecx,edx
  152.     mov    ebx,eax
  153.     mov    eax,es:[di+16]
  154.     imul    DWORD PTR y
  155.     add    ebx,eax
  156.     adc    ecx,edx
  157.     mov    eax,es:[di+20]
  158.     imul    DWORD PTR z
  159.     add    eax,ebx
  160.     adc    edx,ecx
  161.     shrd    eax,edx,29
  162.     adc    eax,0
  163.     mov    yr,eax
  164.  
  165.     mov    eax,es:[di+24]
  166.     imul    DWORD PTR x
  167.     mov    ecx,edx
  168.     mov    ebx,eax
  169.     mov    eax,es:[di+28]
  170.     imul    DWORD PTR y
  171.     add    ebx,eax
  172.     adc    ecx,edx
  173.     mov    eax,es:[di+32]
  174.     imul    DWORD PTR z
  175.     add    eax,ebx
  176.     adc    edx,ecx
  177.     shrd    eax,edx,29
  178.     adc    eax,0
  179.     mov    zr,eax
  180.  
  181.     pop    dx
  182.     pop    cx
  183.     pop    di
  184.     pop    si
  185.       }
  186.  
  187.  *xp = xr;
  188.  *yp = yr;
  189.  *zp = zr;
  190. }
  191.  
  192.  
  193.  
  194. /******************** MISC. VECTOR MATH ****************/
  195.  
  196.         /* replaces column N of a matrix with cross of other 2 */
  197.         /* used to speed computations, repair matrix scaling   */
  198.  
  199. void cross_column(MATRIX m, int col)
  200. {
  201.  long x, y, z;
  202.  long x1, y1, z1, x2, y2, z2;
  203.  
  204.  switch(col)
  205.   {
  206.    case 0:
  207.     x1 = m[0][1];
  208.     y1 = m[1][1];
  209.     z1 = m[2][1];
  210.     x2 = m[0][2];
  211.     y2 = m[1][2];
  212.     z2 = m[2][2];
  213.     break;
  214.  
  215.    case 1:
  216.     x1 = m[0][2];
  217.     y1 = m[1][2];
  218.     z1 = m[2][2];
  219.     x2 = m[0][0];
  220.     y2 = m[1][0];
  221.     z2 = m[2][0];
  222.     break;
  223.  
  224.    case 2:
  225.     x1 = m[0][0];
  226.     y1 = m[1][0];
  227.     z1 = m[2][0];
  228.     x2 = m[0][1];
  229.     y2 = m[1][1];
  230.     z2 = m[2][1];
  231.     break;
  232.   }
  233.  
  234.  asm {
  235.     push    dx
  236.     push    si
  237.     push    di
  238.  
  239.     mov    eax,y1        /* compute cross product */
  240.     mov    edx,z2
  241.     imul    edx
  242.     mov    edi,edx
  243.     mov    esi,eax
  244.     mov    eax,y2
  245.     mov    edx,z1
  246.     imul    edx
  247.     sub    esi,eax
  248.     sbb    edi,edx
  249.     shrd    esi,edi,29
  250.     adc    esi,0
  251.     mov    x,esi
  252.  
  253.     mov    eax,z1
  254.     mov    edx,x2
  255.     imul    edx
  256.     mov    edi,edx
  257.     mov    esi,eax
  258.     mov    eax,z2
  259.     mov    edx,x1
  260.     imul    edx
  261.     sub    esi,eax
  262.     sbb    edi,edx
  263.     shrd    esi,edi,29
  264.     adc    esi,0
  265.     mov    y,esi
  266.  
  267.     mov    eax,x1
  268.     mov    edx,y2
  269.     imul    edx
  270.     mov    edi,edx
  271.     mov    esi,eax
  272.     mov    eax,x2
  273.     mov    edx,y1
  274.     imul    edx
  275.     sub    esi,eax
  276.     sbb    edi,edx
  277.     shrd    esi,edi,29
  278.     adc    esi,0
  279.     mov    z,esi
  280.  
  281.     pop    di
  282.     pop    si
  283.     pop    dx
  284.      }
  285.  
  286.  
  287.  switch(col)
  288.   {
  289.    case 0:
  290.     m[0][0] = x;
  291.     m[1][0] = y;
  292.     m[2][0] = z;
  293.     break;
  294.  
  295.    case 1:
  296.     m[0][1] = x;
  297.     m[1][1] = y;
  298.     m[2][1] = z;
  299.     break;
  300.  
  301.    case 2:
  302.     m[0][2] = x;
  303.     m[1][2] = y;
  304.     m[2][2] = z;
  305.     break;
  306.   }
  307. }
  308.  
  309.  
  310. long m_mult(long a, long b)    /* perform mult. by matrix element         */
  311. {                              /* where either a or b is in <3.29> format */
  312.  long result;
  313.  
  314.  asm {
  315.     mov    eax,DWORD PTR a
  316.     imul    DWORD PTR b
  317.     shrd    eax,edx,29
  318.     adc    eax,0
  319.     mov    DWORD PTR result,eax
  320.      }
  321.  return result;
  322. }
  323.  
  324.  
  325. long dot_prod_29(long a, long b, long c, long x, long y, long z)
  326. {
  327.  /* computes (Ax + By + Cz)>>29 */
  328.  
  329.  long result;
  330.  
  331.  asm {
  332.     push    si
  333.     push    di
  334.  
  335.     mov    eax,a
  336.     imul    DWORD PTR x
  337.     mov    esi,eax
  338.     mov    edi,edx
  339.  
  340.     mov    eax,b
  341.     imul    DWORD PTR y
  342.     add    esi,eax
  343.     adc    edi,edx
  344.  
  345.     mov    eax,c
  346.     imul    DWORD PTR z
  347.     add    esi,eax
  348.     adc    edi,edx
  349.  
  350.     shrd    esi,edi,29
  351.     adc    esi,0
  352.     mov    result,esi
  353.  
  354.     pop    di
  355.     pop    si
  356.      }
  357.  return result;
  358. }
  359.  
  360. /****************** MATRIX MANIPULATION ***************/
  361.  
  362.  
  363. void matrix_mult(MATRIX a, MATRIX b, MATRIX c)
  364. {
  365.     long r1,r2,r3,r4,r5,r6,r7,r8,r9;
  366.  
  367.  asm {
  368.     .386                    /* 3x3 section of matrixs: A*B->C       */
  369.     push    ds              /* now gets z column from cross product */
  370.     push    si
  371.     push    di
  372.     push    cx
  373.     push    dx
  374.  
  375.     les    si,DWORD PTR a
  376.     lds    di,DWORD PTR b
  377.  
  378.     mov    eax,es:[si]
  379.     imul    DWORD PTR ds:[di]
  380.     mov    ebx,eax
  381.     mov    ecx,edx
  382.     mov    eax,es:[si+4]
  383.     imul    DWORD PTR ds:[di+12]
  384.     add    ebx,eax
  385.     adc    ecx,edx
  386.     mov    eax,es:[si+8]
  387.     imul    DWORD PTR ds:[di+24]
  388.     add    ebx,eax
  389.     adc    ecx,edx
  390.     shrd    ebx,ecx,29
  391.     adc    ebx,0
  392.     mov    r1,ebx
  393.  
  394.     mov    eax,es:[si]
  395.     imul    DWORD PTR ds:[di+4]
  396.     mov    ebx,eax
  397.     mov    ecx,edx
  398.     mov    eax,es:[si+4]
  399.     imul    DWORD PTR ds:[di+16]
  400.     add    ebx,eax
  401.     adc    ecx,edx
  402.     mov    eax,es:[si+8]
  403.     imul    DWORD PTR ds:[di+28]
  404.     add    ebx,eax
  405.     adc    ecx,edx
  406.     shrd    ebx,ecx,29
  407.     adc    ebx,0
  408.     mov    r2,ebx
  409.  
  410. #ifndef CROSS_Z
  411.     mov    eax,es:[si]
  412.     imul    DWORD PTR ds:[di+8]
  413.     mov    ebx,eax
  414.     mov    ecx,edx
  415.     mov    eax,es:[si+4]
  416.     imul    DWORD PTR ds:[di+20]
  417.     add    ebx,eax
  418.     adc    ecx,edx
  419.     mov    eax,es:[si+8]
  420.     imul    DWORD PTR ds:[di+32]
  421.     add    ebx,eax
  422.     adc    ecx,edx
  423.     shrd    ebx,ecx,29
  424.     adc    ebx,0
  425.     mov    r3,ebx
  426. #endif
  427.  
  428.     mov    eax,es:[si+12]
  429.     imul    DWORD PTR ds:[di]
  430.     mov    ebx,eax
  431.     mov    ecx,edx
  432.     mov    eax,es:[si+16]
  433.     imul    DWORD PTR ds:[di+12]
  434.     add    ebx,eax
  435.     adc    ecx,edx
  436.     mov    eax,es:[si+20]
  437.     imul    DWORD PTR ds:[di+24]
  438.     add    ebx,eax
  439.     adc    ecx,edx
  440.     shrd    ebx,ecx,29
  441.     adc    ebx,0
  442.     mov    r4,ebx
  443.  
  444.     mov    eax,es:[si+12]
  445.     imul    DWORD PTR ds:[di+4]
  446.     mov    ebx,eax
  447.     mov    ecx,edx
  448.     mov    eax,es:[si+16]
  449.     imul    DWORD PTR ds:[di+16]
  450.     add    ebx,eax
  451.     adc    ecx,edx
  452.     mov    eax,es:[si+20]
  453.     imul    DWORD PTR ds:[di+28]
  454.     add    ebx,eax
  455.     adc    ecx,edx
  456.     shrd    ebx,ecx,29
  457.     adc    ebx,0
  458.     mov    r5,ebx
  459.  
  460. #ifndef CROSS_Z
  461.     mov    eax,es:[si+12]
  462.     imul    DWORD PTR ds:[di+8]
  463.     mov    ebx,eax
  464.     mov    ecx,edx
  465.     mov    eax,es:[si+16]
  466.     imul    DWORD PTR ds:[di+20]
  467.     add    ebx,eax
  468.     adc    ecx,edx
  469.     mov    eax,es:[si+20]
  470.     imul    DWORD PTR ds:[di+32]
  471.     add    ebx,eax
  472.     adc    ecx,edx
  473.     shrd    ebx,ecx,29
  474.     adc    ebx,0
  475.     mov    r6,ebx
  476. #endif
  477.  
  478.     mov    eax,es:[si+24]
  479.     imul    DWORD PTR ds:[di]
  480.     mov    ebx,eax
  481.     mov    ecx,edx
  482.     mov    eax,es:[si+28]
  483.     imul    DWORD PTR ds:[di+12]
  484.     add    ebx,eax
  485.     adc    ecx,edx
  486.     mov    eax,es:[si+32]
  487.     imul    DWORD PTR ds:[di+24]
  488.     add    ebx,eax
  489.     adc    ecx,edx
  490.     shrd    ebx,ecx,29
  491.     adc    ebx,0
  492.     mov    r7,ebx
  493.  
  494.     mov    eax,es:[si+24]
  495.     imul    DWORD PTR ds:[di+4]
  496.     mov    ebx,eax
  497.     mov    ecx,edx
  498.     mov    eax,es:[si+28]
  499.     imul    DWORD PTR ds:[di+16]
  500.     add    ebx,eax
  501.     adc    ecx,edx
  502.     mov    eax,es:[si+32]
  503.     imul    DWORD PTR ds:[di+28]
  504.     add    ebx,eax
  505.     adc    ecx,edx
  506.     shrd    ebx,ecx,29
  507.     adc    ebx,0
  508.     mov    r8,ebx
  509.  
  510. #ifndef CROSS_Z
  511.     mov    eax,es:[si+24]
  512.     imul    DWORD PTR ds:[di+8]
  513.     mov    ebx,eax
  514.     mov    ecx,edx
  515.     mov    eax,es:[si+28]
  516.     imul    DWORD PTR ds:[di+20]
  517.     add    ebx,eax
  518.     adc    ecx,edx
  519.     mov    eax,es:[si+32]
  520.     imul    DWORD PTR ds:[di+32]
  521.     add    ebx,eax
  522.     adc    ecx,edx
  523.     shrd    ebx,ecx,29
  524.     adc    ebx,0
  525.     mov    r9,ebx
  526. #endif
  527.  
  528. #ifdef CROSS_Z
  529.     mov    eax,r4        /* compute cross product of  */
  530.     mov    edx,r8        /* columns 1&2 to get col. 3 */
  531.     imul    edx
  532.     mov    edi,edx
  533.     mov    esi,eax
  534.     mov    eax,r5
  535.     mov    edx,r7
  536.     imul    edx
  537.     sub    esi,eax
  538.     sbb    edi,edx
  539.     shrd    esi,edi,29
  540.     adc    esi,0
  541.     mov     r3,esi
  542.  
  543.     mov    eax,r7
  544.     mov    edx,r2
  545.     imul    edx
  546.     mov    edi,edx
  547.     mov    esi,eax
  548.     mov    eax,r8
  549.     mov    edx,r1
  550.     imul    edx
  551.     sub    esi,eax
  552.     sbb    edi,edx
  553.     shrd    esi,edi,29
  554.     adc    esi,0
  555.     mov     r6,esi
  556.  
  557.     mov    eax,r1
  558.     mov    edx,r5
  559.     imul    edx
  560.     mov    edi,edx
  561.     mov    esi,eax
  562.     mov    eax,r2
  563.     mov    edx,r4
  564.     imul    edx
  565.     sub    esi,eax
  566.     sbb    edi,edx
  567.     shrd    esi,edi,29
  568.     adc    esi,0
  569.     mov     r9,esi
  570. #endif
  571.  
  572.     les    di,DWORD PTR c
  573.     mov    eax,r1
  574.     mov    es:[di],eax
  575.     mov    eax,r2
  576.     mov    es:[di+4],eax
  577.     mov    eax,r4
  578.     mov    es:[di+12],eax
  579.     mov    eax,r5
  580.     mov    es:[di+16],eax
  581.     mov    eax,r7
  582.     mov    es:[di+24],eax
  583.     mov    eax,r8
  584.     mov    es:[di+28],eax
  585.     mov    eax,r3
  586.     mov    es:[di+8],eax
  587.     mov    eax,r6
  588.     mov    es:[di+20],eax
  589.     mov    eax,r9
  590.     mov    es:[di+32],eax
  591.  
  592.     pop    dx
  593.     pop    cx
  594.     pop    di
  595.     pop    si
  596.     pop    ds
  597.     }
  598.  
  599. }
  600.  
  601.          /* full homogenous matrix multiply */
  602.  
  603. void matrix_product(MATRIX a, MATRIX b, MATRIX c)
  604. {
  605.  MATRIX d;
  606.  long x, y, z;
  607.  matrix_mult(a, b, d);
  608.  x = b[3][0];
  609.  y = b[3][1];
  610.  z = b[3][2];
  611.  matrix_point(a, &x, &y, &z);
  612.  d[3][0] = x;
  613.  d[3][1] = y;
  614.  d[3][2] = z;
  615.  memcpy (c, &d, sizeof(MATRIX));
  616. }
  617.  
  618.  
  619. void matrix_transpose(MATRIX a, MATRIX b)
  620. {
  621.  long s;
  622.  
  623.     b[0][0] = a[0][0];     /* generate inverse of rotate matrix (transpose) */
  624.     b[1][1] = a[1][1];     /* ONLY WORKS FOR ORTHOGONAL MATRICES            */
  625.     b[2][2] = a[2][2];     /* will do self_transpose as well as copy */
  626.  
  627.     s = a[0][1];
  628.     b[0][1] = a[1][0];
  629.     b[1][0] = s;
  630.  
  631.     s = a[2][0];
  632.     b[2][0] = a[0][2];
  633.     b[0][2] = s;
  634.  
  635.     s = a[2][1];
  636.     b[2][1] = a[1][2];
  637.     b[1][2] = s;
  638. }
  639.  
  640.  
  641. void inverse_matrix(MATRIX a, MATRIX b)
  642. {
  643.     long x = -a[3][0];       /* generate inverse of    */
  644.     long y = -a[3][1];       /* full homogenous matrix */
  645.     long z = -a[3][2];
  646.  
  647.     matrix_transpose(a,b);
  648.                 /* old: Ax+b = c      */
  649.     b[3][0] = 0;            /* b' = (1/A)(-b)     */
  650.     b[3][1] = 0;               /* (1/A) = t(A)       */
  651.     b[3][2] = 0;               /* new: (1/A)c+b' = x */
  652.  
  653.     matrix_point(b,&x,&y,&z);
  654.  
  655.     b[3][0] = x;
  656.     b[3][1] = y;
  657.     b[3][2] = z;
  658. }
  659.  
  660.  
  661.  
  662. void identity_matrix(MATRIX m)
  663. {
  664.  int i, j;
  665.  
  666.  m[0][0] = m[1][1] = m[2][2] = XFSC;
  667.  m[1][0] = m[2][0] = m[3][0] = 0;
  668.  m[0][1] = m[2][1] = m[3][1] = 0;
  669.  m[0][2] = m[1][2] = m[3][2] = 0;
  670. }
  671.  
  672.             /* copy 3x4 matrix */
  673.  
  674. void matrix_copy(MATRIX m, MATRIX n)
  675. {
  676.  memcpy (n, m, sizeof(MATRIX));
  677. }
  678.  
  679.             /* copy 3x3 rotation part of matrix */
  680.  
  681. void matrix_rot_copy(MATRIX m, MATRIX n)
  682. {
  683.  memcpy (n, m, sizeof(MATRIX));
  684.  n[3][0] = n[3][1] = n[3][2] = 0;
  685. }
  686.  
  687.  
  688.  
  689. /*************** ANGLE/POSITION TO HOMOGENOUS MATRIX ************/
  690.  
  691.  
  692. #define RXYZ 1        /* matrix rotation types */
  693. #define RYXZ 0          /* ONLY RYXZ guaranteed to be tested */
  694. #define RXZY 2
  695. #define RZYX 5
  696. #define RZXY 4
  697. #define RYZX 6                       /* create rotation/translation */
  698.                      /* "matrix" from angle data    */
  699.  
  700. void multi_matrix(MATRIX m, long rx, long ry, long rz,
  701.           long tx, long ty, long tz, int type )
  702. {
  703.  long sinx,cosx,siny,cosy,sinz,cosz;
  704.  long result;
  705.  
  706.  if(rx==0)            /* tests for single-axis rotates */
  707.   {                   /* much faster to directly make  */
  708.    if(ry==0)
  709.     {
  710.      identity_matrix(m);
  711.      if(rz!=0)
  712.       {
  713.        m[0][0] = m[1][1] = icosine(rz);
  714.        m[1][0] = isine(rz);
  715.        m[0][1] = -m[1][0];
  716.       }
  717.      goto shortcut;
  718.     }
  719.    else if(rz==0)
  720.     {
  721.      identity_matrix(m);
  722.      m[0][0] = m[2][2] = icosine(ry);
  723.          m[0][2] = isine(ry);
  724.      m[2][0] = -m[0][2];
  725.      goto shortcut;
  726.     }
  727.   }
  728.  else if(ry==0 && rz==0)
  729.     {
  730.    identity_matrix(m);
  731.    m[1][1] = m[2][2] = icosine(rx);
  732.    m[2][1] = isine(rx);
  733.    m[1][2] = -m[2][1];
  734.    goto shortcut;
  735.   }
  736.  
  737.  cosx = icosine(rx);          /* angle compute */
  738.  cosy = icosine(ry);
  739.  cosz = icosine(rz);
  740.  sinx = isine(rx);
  741.  siny = isine(ry);
  742.  sinz = isine(rz);
  743.  
  744.  asm {
  745.     .386
  746.     push    si
  747.     push    di
  748.     push    cx
  749.     push    dx
  750.      }
  751.  
  752.  
  753.  if(type&4)              /* negate angles? */
  754.   asm {
  755.     neg    DWORD PTR sinx
  756.     neg    DWORD PTR siny
  757.     neg    DWORD PTR sinz
  758.       }
  759.  
  760.  switch(type&3)
  761.   {
  762.      case RXYZ:
  763.     {
  764.       asm {
  765.     mov    eax,cosz
  766.     imul    DWORD PTR cosy
  767.     shrd    eax,edx,29
  768.     adc    eax,0
  769.     mov    result,eax
  770.      }
  771.       m[0][0] = result;
  772.  
  773.      asm {
  774.     mov    eax,cosx
  775.     imul    DWORD PTR sinz
  776.     shrd    eax,edx,29
  777.     adc    eax,0
  778.     mov    ecx,eax
  779.     mov    eax,cosz
  780.     imul    DWORD PTR sinx
  781.     shrd    eax,edx,29
  782.     adc    eax,0
  783.     imul    DWORD PTR siny
  784.     shrd    eax,edx,29
  785.     adc    ecx,eax
  786.     mov    result,ecx
  787.      }
  788.       m[1][0] = result;
  789.  
  790.      asm {
  791.     mov    eax,sinz
  792.     imul    DWORD PTR sinx
  793.     shrd    eax,edx,29
  794.     adc    eax,0
  795.     mov    ecx,eax
  796.     mov    eax,cosz
  797.     imul    DWORD PTR cosx
  798.     shrd    eax,edx,29
  799.     adc    eax,0
  800.     imul    DWORD PTR siny
  801.     shrd    eax,edx,29
  802.     adc    eax,0
  803.     sub    ecx,eax
  804.     mov    result,ecx
  805.      }
  806.       m[2][0] = result;
  807.  
  808.       m[0][2] = siny;
  809.  
  810.       asm {
  811.     mov    eax,cosy
  812.     imul    DWORD PTR sinx
  813.     shrd    eax,edx,29
  814.     adc    eax,0
  815.     neg    eax
  816.     mov    result,eax
  817.      }
  818.       m[1][2] = result;
  819.  
  820.       asm {
  821.     mov    eax,cosx
  822.     imul    DWORD PTR cosy
  823.     shrd    eax,edx,29
  824.     adc    eax,0
  825.     mov    result,eax
  826.      }
  827.             m[2][2] = result;
  828.  
  829.       cross_column(m,1);
  830.       break;
  831.     }
  832.  
  833.    case RYXZ:
  834.     {
  835.      asm {
  836.     mov    eax,cosz
  837.     imul    DWORD PTR cosy
  838.     shrd    eax,edx,29
  839.     adc    eax,0
  840.     mov    ecx,eax
  841.     mov    eax,sinz
  842.     imul    DWORD PTR sinx
  843.     shrd    eax,edx,29
  844.     adc    eax,0
  845.     imul    DWORD PTR siny
  846.     shrd    eax,edx,29
  847.     adc    ecx,eax
  848.     mov    result,ecx
  849.      }
  850.       m[0][0] = result;
  851.  
  852.       asm {
  853.     mov    eax,cosx
  854.     imul    DWORD PTR sinz
  855.     shrd    eax,edx,29
  856.     adc    eax,0
  857.     mov    result,eax
  858.      }
  859.       m[1][0] = result;
  860.  
  861.          asm {
  862.     mov    eax,cosy
  863.     imul    DWORD PTR sinz
  864.     shrd    eax,edx,29
  865.     adc    eax,0
  866.     imul    DWORD PTR sinx
  867.     shrd    eax,edx,29
  868.     adc    eax,0
  869.     mov    ecx,eax
  870.     mov    eax,cosz
  871.     imul    DWORD PTR siny
  872.     shrd    eax,edx,29
  873.     adc    eax,0
  874.     sub    ecx,eax
  875.     mov    result,ecx
  876.      }
  877.       m[2][0] = result;
  878.  
  879.       asm {
  880.     mov    eax,cosx
  881.     imul    DWORD PTR siny
  882.     shrd    eax,edx,29
  883.     adc    eax,0
  884.     mov    result,eax
  885.      }
  886.       m[0][2] = result;
  887.  
  888.       m[1][2] = -sinx;
  889.  
  890.       asm {
  891.     mov    eax,cosx
  892.     imul    DWORD PTR cosy
  893.     shrd    eax,edx,29
  894.     adc    eax,0
  895.     mov    result,eax
  896.      }
  897.       m[2][2] = result;
  898.  
  899.       cross_column(m,1);
  900.       break;
  901.     }
  902.  
  903.    case RXZY:
  904.     {
  905.       asm {
  906.     mov    eax,cosz
  907.     imul    DWORD PTR cosy
  908.     shrd    eax,edx,29
  909.     adc    eax,0
  910.     mov    result,eax
  911.      }
  912.       m[0][0] = result;
  913.  
  914.      asm {
  915.     mov    eax,sinx
  916.     imul    DWORD PTR siny
  917.     shrd    eax,edx,29
  918.     adc    eax,0
  919.     mov    ecx,eax
  920.     mov    eax,cosx
  921.     imul    DWORD PTR cosy
  922.     shrd    eax,edx,29
  923.     adc    eax,0
  924.     imul    DWORD PTR sinz
  925.     shrd    eax,edx,29
  926.     adc    ecx,eax
  927.     mov    result,ecx
  928.      }
  929.       m[1][0] = result;
  930.  
  931.      asm {
  932.     mov    eax,cosy
  933.     imul    DWORD PTR sinz
  934.     shrd    eax,edx,29
  935.     adc    eax,0
  936.     imul    DWORD PTR sinx
  937.     shrd    eax,edx,29
  938.     adc    eax,0
  939.     mov    ecx,eax
  940.     mov    eax,cosx
  941.     imul    DWORD PTR siny
  942.     shrd    eax,edx,29
  943.     adc    eax,0
  944.     sub    ecx,eax
  945.     mov    result,ecx
  946.      }
  947.       m[2][0] = result;
  948.  
  949.       m[0][1] = -sinz;
  950.  
  951.       asm {
  952.     mov    eax,cosz
  953.     imul    DWORD PTR cosx
  954.     shrd    eax,edx,29
  955.     adc    eax,0
  956.     mov    result,eax
  957.      }
  958.       m[1][1] = result;
  959.  
  960.             asm {
  961.     mov    eax,cosz
  962.     imul    DWORD PTR sinx
  963.     shrd    eax,edx,29
  964.     adc    eax,0
  965.     mov    result,eax
  966.      }
  967.       m[2][1] = result;
  968.  
  969.       cross_column(m,2);
  970.       break;
  971.     }
  972.   }
  973.  
  974.  asm {
  975.     pop    dx
  976.     pop    cx
  977.     pop    di
  978.     pop    si
  979.      }
  980.  
  981.  if(type&4) matrix_transpose(m,m);
  982.  
  983. shortcut:
  984.  m[3][0] = tx;
  985.  m[3][1] = ty;
  986.  m[3][2] = tz;
  987. }
  988.  
  989.         /* the default matrix: RYXZ  */
  990.         /* used for camera transform */
  991.  
  992. void std_matrix(MATRIX m, long rx, long ry, long rz,
  993.     long tx, long ty, long tz)
  994. {
  995.  multi_matrix(m, rx, ry, rz, tx, ty, tz, RYXZ);
  996. }
  997.  
  998.  
  999. /*************** MATRIX-TO-ANGLES (RYXZ ONLY) *************/
  1000.  
  1001.  
  1002. extern long slow_magnitude(long a, long b);  /* done the float way */
  1003.  
  1004. void matrix_to_angle(MATRIX m, long *rx, long *ry, long *rz)
  1005. {
  1006.  long t,p,a;
  1007.  long c2;
  1008.  
  1009.  if(m[1][2]>536334041 || m[1][2]<-536334041)
  1010.   {
  1011.    c2 = slow_magnitude(m[0][2],m[2][2]);    /* need accuracy for this one */
  1012.    if(c2>2000000)
  1013.     {
  1014.      p = arccosine(c2);
  1015.          if(m[1][2]<0) p = -p;
  1016.     }
  1017.    else
  1018.     {
  1019.      t = (m[1][2]<0) ? 90*65536L : -90*65536L;
  1020.      p = 0;
  1021.      a = arctan2(m[0][1], m[0][0]);
  1022.      goto assign;
  1023.     }
  1024.   }
  1025.  else p = -arcsine(m[1][2]);
  1026.  
  1027.  t = arctan2(m[0][2], m[2][2]);
  1028.  a = arctan2(m[1][0], m[1][1]);
  1029.  
  1030. assign:
  1031.  
  1032.  *ry = t;
  1033.  *rx = p;
  1034.  *rz = a;
  1035. }
  1036.  
  1037.