home *** CD-ROM | disk | FTP | other *** search
/ Virtual Reality Homebrewer's Handbook / vr.iso / vr386 / matrix.c < prev    next >
C/C++ Source or Header  |  1996-03-19  |  7KB  |  280 lines
  1. // Non-assembler matrix math routines
  2. // All code by Dave Stampe, last updated 23/12/93
  3.  
  4. /*
  5.  This code is part of the VR-386 project, created by Dave Stampe.
  6.  VR-386 is a desendent of REND386, created by Dave Stampe and
  7.  Bernie Roehl.  Almost all the code has been rewritten by Dave
  8.  Stampre for VR-386.
  9.  
  10.  Copyright (c) 1994 by Dave Stampe:
  11.  May be freely used to write software for release into the public domain
  12.  or for educational use; all commercial endeavours MUST contact Dave Stampe
  13.  (dstampe@psych.toronto.edu) for permission to incorporate any part of
  14.  this software or source code into their products!  Usually there is no
  15.  charge for under 50-100 items for low-cost or shareware products, and terms
  16.  are reasonable.  Any royalties are used for development, so equipment is
  17.  often acceptable payment.
  18.  
  19.  ATTRIBUTION:  If you use any part of this source code or the libraries
  20.  in your projects, you must give attribution to VR-386 and Dave Stampe,
  21.  and any other authors in your documentation, source code, and at startup
  22.  of your program.  Let's keep the freeware ball rolling!
  23.  
  24.  DEVELOPMENT: VR-386 is a effort to develop the process started by
  25.  REND386, improving programmer access by rewriting the code and supplying
  26.  a standard API.  If you write improvements, add new functions rather
  27.  than rewriting current functions.  This will make it possible to
  28.  include you improved code in the next API release.  YOU can help advance
  29.  VR-386.  Comments on the API are welcome.
  30.  
  31.  CONTACT: dstampe@psych.toronto.edu
  32. */
  33.  
  34.  
  35. #include <mem.h>       /* memcpy() */
  36.  
  37. #include "intmath.h"
  38.  
  39.  
  40. /******************** MISC. VECTOR MATH ****************/
  41.  
  42.         /* replaces column N of a matrix with cross of other 2 */
  43.         /* used to speed computations, repair matrix scaling   */
  44.  
  45. void cross_column(MATRIX m, int col)
  46. {
  47.  extern void cross_product(long *col1, long *col2, long *col3);    // asm code
  48.  
  49.  long *c1, *c2, *c3;
  50.  
  51.  switch (col)
  52.   {
  53.    case 0:
  54.     c1 = &(m[0][1]);
  55.     c2 = &(m[0][2]);
  56.     c3 = &(m[0][0]);
  57.     break;
  58.  
  59.    case 1:
  60.     c1 = &(m[0][2]);
  61.     c2 = &(m[0][0]);
  62.     c3 = &(m[0][1]);
  63.     break;
  64.  
  65.    case 2:
  66.     c1 = &(m[0][0]);
  67.     c2 = &(m[0][1]);
  68.     c3 = &(m[0][2]);
  69.     break;
  70.   }
  71.  
  72.  cross_product(c1, c2, c3);
  73. }
  74.  
  75.  
  76. /****************** MATRIX MANIPULATION ***************/
  77.  
  78.  
  79. #define XFSC 536870912   /* 2**29 for shifting real to <3.29 fixed point */
  80.  
  81. void identity_matrix(MATRIX m)
  82. {
  83.  int i, j;
  84.  
  85.  m[0][0] = m[1][1] = m[2][2] = XFSC;
  86.  m[1][0] = m[2][0] = m[3][0] = 0;
  87.  m[0][1] = m[2][1] = m[3][1] = 0;
  88.  m[0][2] = m[1][2] = m[3][2] = 0;
  89. }
  90.  
  91.             /* copy 3x4 matrix */
  92.  
  93. void matrix_copy(MATRIX m, MATRIX n)
  94. {
  95.  memcpy (n, m, sizeof(MATRIX));
  96. }
  97.  
  98.             /* copy 3x3 rotation part of matrix */
  99.  
  100. void matrix_rot_copy(MATRIX m, MATRIX n)
  101. {
  102.  memcpy (n, m, sizeof(MATRIX));
  103.  n[3][0] = n[3][1] = n[3][2] = 0;
  104. }
  105.  
  106.  
  107.  
  108. /*************** ANGLE/POSITION TO HOMOGENOUS MATRIX ************/
  109.  
  110. // assembler from matrixm.asm
  111.  
  112. extern void matrix_RXYZ(MATRIX m, long rx, long ry, long rz,
  113.     long tx, long ty, long tz);
  114.  
  115. extern void matrix_RYXZ(MATRIX m, long rx, long ry, long rz,
  116.     long tx, long ty, long tz);
  117.  
  118. extern void matrix_RXZY(MATRIX m, long rx, long ry, long rz,
  119.     long tx, long ty, long tz);
  120.  
  121.  
  122. #define RXYZ 1        /* matrix rotation types */
  123. #define RYXZ 0
  124. #define RXZY 2
  125. #define RZYX 5
  126. #define RZXY 4
  127. #define RYZX 6                       /* create rotation/translation */
  128.                      /* "matrix" from angle data    */
  129.  
  130.  
  131. void multi_matrix(MATRIX m, long rx, long ry, long rz,
  132.           long tx, long ty, long tz, int type )
  133. {
  134.  if (rx == 0)         /* tests for single-axis rotates */
  135.   {                   /* much faster to directly make  */
  136.    if (ry == 0)
  137.     {
  138.      identity_matrix(m);
  139.      if (rz != 0)
  140.       {
  141.        m[0][0] = m[1][1] = icosine(rz);
  142.        m[1][0] = isine(rz);
  143.        m[0][1] = -m[1][0];
  144.       }
  145.      goto shortcut;
  146.     }
  147.    else if (rz == 0)
  148.     {
  149.      identity_matrix(m);
  150.      m[0][0] = m[2][2] = icosine(ry);
  151.      m[0][2] = isine(ry);
  152.      m[2][0] = -m[0][2];
  153.      goto shortcut;
  154.     }
  155.   }
  156.  else if (ry == 0 && rz == 0)
  157.   {
  158.    identity_matrix(m);
  159.    m[1][1] = m[2][2] = icosine(rx);
  160.    m[2][1] = isine(rx);
  161.    m[1][2] = -m[2][1];
  162.    goto shortcut;
  163.   }
  164.  
  165.  switch(type&7)
  166.   {
  167.     case RXYZ:
  168.     matrix_RXYZ(m,rx,ry,rz,tx,ty,tz);
  169.     break;
  170.  
  171.     case RZYX:
  172.     matrix_RXYZ(m,-rx,-ry,-rz,tx,ty,tz);  // reverse order: just
  173.     matrix_transpose(m,m);                // negate angles, take
  174.     break;                                // rotational inverse
  175.  
  176.     case RYXZ:
  177.     matrix_RYXZ(m,rx,ry,rz,tx,ty,tz);
  178.     break;
  179.  
  180.     case RZXY:
  181.     matrix_RYXZ(m,-rx,-ry,-rz,tx,ty,tz);
  182.     matrix_transpose(m,m);
  183.     break;
  184.  
  185.     case RXZY:
  186.     matrix_RXZY(m,rx,ry,rz,tx,ty,tz);
  187.     break;
  188.  
  189.     case RYZX:
  190.     matrix_RXZY(m,-rx,-ry,-rz,tx,ty,tz);
  191.     matrix_transpose(m,m);
  192.     break;
  193.   }
  194.  return;
  195.  
  196. shortcut:
  197.  m[3][0] = tx;
  198.  m[3][1] = ty;
  199.  m[3][2] = tz;
  200. }
  201.  
  202.         /* the default matrix: RYXZ  */
  203.         /* used for camera transform */
  204.  
  205. void std_matrix(MATRIX m, long rx, long ry, long rz,
  206.     long tx, long ty, long tz)
  207. {
  208.  multi_matrix(m, rx, ry, rz, tx, ty, tz, RYXZ);
  209. }
  210.  
  211.  
  212. /*************** MATRIX-TO-ANGLES (RYXZ ONLY) *************/
  213.  
  214.  
  215. void matrix_to_angle(MATRIX m, long *rx, long *ry, long *rz)
  216. {
  217.  long t,p,a;
  218.  long c2;
  219.  
  220.  if (m[1][2] > 536334041 || m[1][2] < -536334041)
  221.   {
  222.    c2 = magnitude32(m[0][2],m[2][2],0);    /* need accuracy for this one */
  223.    if (c2 > 2000000)
  224.     {
  225.      p = arccosine(c2);
  226.          if (m[1][2] < 0) p = -p;
  227.     }
  228.    else
  229.     {
  230.      t = (m[1][2] < 0) ? 90*65536L : -90*65536L;  /* works with Z-axis rule */
  231.      a = 0;
  232.      p = arccosine(m[1][0]);
  233.      if (m[1][2] > 0) p = -p;
  234.  
  235.      goto assign;
  236.     }
  237.   }
  238.  else p = -arcsine(m[1][2]);
  239.  
  240.  t = arctan2(m[0][2], m[2][2]);
  241.  a = arctan2(m[1][0], m[1][1]);
  242.  
  243. assign:
  244.  
  245.  *ry = t;
  246.  *rx = p;
  247.  *rz = a;
  248. }
  249.  
  250.  
  251. /******************* MATRIX RENORMALIZE ***************/
  252.  
  253.  
  254. #define XFLC 536870912    // fixed -> <3.29>
  255.  
  256. void renormalize_matrix(MATRIX m) /* slow but sure: do once every 1000 matrix mults */
  257. {
  258.     set_vector_magnitude32(XFLC, &m[0][0], &m[0][1], &m[0][2]);
  259.     set_vector_magnitude32(XFLC, &m[1][0], &m[1][1], &m[1][2]);
  260.     set_vector_magnitude32(XFLC, &m[2][0], &m[2][1], &m[2][2]);
  261.     set_vector_magnitude32(XFLC, &m[0][0], &m[1][0], &m[2][0]);
  262.     set_vector_magnitude32(XFLC, &m[0][1], &m[1][1], &m[2][1]);
  263.     set_vector_magnitude32(XFLC, &m[0][2], &m[1][2], &m[2][2]);
  264. }
  265.  
  266.  
  267. /******************* MATRIX FROM VECTOR ***************/
  268.  
  269.  
  270. void vector_to_matrix(MATRIX m, long x, long y, long z)
  271. {
  272.  long ya = arctan2(x, z);
  273.  long sz = magnitude32(x,y,z);
  274.  long r =  divide_29(y, sz );
  275.  long xa = -arcsine(r);
  276.  
  277.  std_matrix(m, xa, ya, 0, 0, 0, 0);
  278. }
  279.  
  280.