!best002.c

supplementary source file 1 for The BESTLibrary

/*==========================================================================
 *
 *  !BEST002.C                                       Tuesday, April 12, 1994
 *
 *  source file 2D and 3D matrix/vector library
 *  supplementary source file 2 for The BESTLibrary
 *
 *  Authored by George Vanous with reference to the 2D and 3D vector library
 *  by Andrew Glassner from "Graphics Gems", Academic Press, 1990
 *
 *==========================================================================*/


/* ------------------------------------------------------------------------ */
/* ----------------------------  INCLUDE FILES  --------------------------- */

#include <math.h>
#include "!best002.h"                  /* include !BEST002.H in compilation */

/*==========================================================================
 *                               3X3 MATRICES
 *--------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------
 * C = A + B
 * RETURNS: C
 */
matrix3 *m3_add(matrix3 *A, matrix3 *B, matrix3 *C)
{
  word i;
  register word j;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++)
      C->element[i][j] = A->element[i][j] + B->element[i][j];
  }
  return( C );
}

/*----------------------------------------------------------------------------
 * RETURNS: copy of A
 */
matrix3 *m3_copy(matrix3 *A)
{
  matrix3 *B = NEWTYPE(matrix3);
  register word i, j;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++)
      B->element[i][j] = A->element[i][j];
  }
  return( B );
}

/*----------------------------------------------------------------------------
 * C = A * B
 * RETURNS: C
 */
matrix3 *m3_mul(matrix3 *A, matrix3 *B, matrix3 *C)
{
  word i;
  register word j, k;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      C->element[i][j] = 0;
      for (k = 0; k < 3; k++)
        C->element[i][j] += A->element[i][k] * B->element[k][j];
    }
  }
  return( C );
}

/*----------------------------------------------------------------------------
 * RETURNS: new 3x3 matrix initialized to a[3][3]
 */
matrix3 *m3_new(double *a[3][3])
{
  matrix3 *A = NEWTYPE(matrix3);
  register word i, j;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++)
      A->element[i][j] = *a[i][j];
  }
  return( A );
}
/*----------------------------------------------------------------------------
 * C = A - B
 * RETURNS: C
 */
matrix3 *m3_sub(matrix3 *A, matrix3 *B, matrix3 *C)
{
  word i;
  register word j;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++)
      C->element[i][j] = A->element[i][j] - B->element[i][j];
  }
  return( C );
}

/*----------------------------------------------------------------------------
 * B = transpose(A)
 * RETURNS: B
 */
matrix3 *m3_transpose(matrix3 *A, matrix3 *B)
{
  register word i, j;

  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++)
      B->element[i][j] = A->element[j][i];
  }
  return( B );
}

/*==========================================================================
 *                               4x4 MATRICES
 *--------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------
 * C = A + B
 * RETURNS: C
 */
matrix4 *m4_add(matrix4 *A, matrix4 *B, matrix4 *C)
{
  word i;
  register word j;

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      C->element[i][j] = A->element[i][j] + B->element[i][j];
  }
  return( C );
}

/*----------------------------------------------------------------------------
 * RETURNS: copy of A
 */
matrix4 *m4_copy(matrix4 *A)
{
  matrix4 *B = NEWTYPE(matrix4);
  register word i, j;

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      B->element[i][j] = A->element[i][j];
  }
  return( B );
}

/*----------------------------------------------------------------------------
 * C = A * B
 * RETURNS: C
 */
matrix4 *m4_mul(matrix4 *A, matrix4 *B, matrix4 *C)
{
  word i;
  register word j, k;

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++) {
      c->element[i][j] = 0;
      for (k = 0; k < 4; k++)
        C->element[i][j] += A->element[i][k] * B->element[k][j];
    }
  }
  return( C );
}

/*----------------------------------------------------------------------------
 * RETURNS: new 4x4 matrix initialized to a[4][4]
 */
matrix4 *m4_new(double *a[4][4])
{
  matrix4 *A = NEWTYPE(matrix4);
  register word i, j;

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      A->element[i][j] = *a[i][j];
  }
  return( A );
}

/*----------------------------------------------------------------------------
 * C = A - B
 * RETURNS: C
 */
matrix4 *m4_sub(matrix4 *A, matrix4 *B, matrix4 *C)
{
  word i;
  register word j;

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      C->element[i][j] = A->element[i][j] - B->element[i][j];
  }
  return( C );
}

/*----------------------------------------------------------------------------
 * B = transpose(A)
 * RETURNS: B
 */
matrix4 *m4_transpose(matrix4 *A, matrix4 *B)
{
  register word i, j;

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      B->element[i][j] = A->element[j][i];
  }
  return( B );
}

/*==========================================================================
 *                                2D VECTORS
 *--------------------------------------------------------------------------*/

/*----------------------------------------------------------------------------
 * w = u + v
 * RETURNS: w
 */
vector2 *v2_add(vector2 *u, vector2 *v, vector2 *w)
{
  w->x = u->x + v->x, w->y = u->y + v->y;
  return( w );
}
        
/*----------------------------------------------------------------------------
 * w = c1(u) + c2(v)
 * RETURNS: w
 */
vector2 *v2_combine(vector2 *u, double c1,
                    vector2 *v, double c2, vector2 *w)
{
  w->x = (c1 * u->x) + (c2 * v->x);
  w->y = (c1 * u->y) + (c2 * v->y);
  return( w );
}

/*----------------------------------------------------------------------------
 * RETURNS: copy of u
 */
vector2 *v2_copy(vector2 *v)
{
  vector2 *w = NEWTYPE(vector2);

  w->x = v->x, w->y = v->y;
  return( w );
}
        
/*----------------------------------------------------------------------------
 * w = u dot v
 * RETURNS: w
 */
double v2_dot(vector2 *u, vector2 *v)
{
  return( (u->x * v->x) + (u->y * v->y) );
}

/*----------------------------------------------------------------------------
 * RETURNS: magnitude of v
 */
double v2_len(vector2 *v)
{
  return( sqrt(V2SquaredLength(v)) );
}
        
/*----------------------------------------------------------------------------
 * RETURNS: magnitude of v squared
 */
double v2_len_sqr(vector2 *v)
{
  return( (v->x * v->x) + (v->y * v->y) );
}

/*----------------------------------------------------------------------------
 * w = linear interpolation between lo and hi by amount alpha
 * RETURNS: w
 */
vector2 *v2_lerp(vector2 *lo, vector2 *hi, double alpha, vector2 *w)
{
  w->x = LERP(alpha, lo->x, hi->x);
  w->y = LERP(alpha, lo->y, hi->y);
  return( w );
}

/*----------------------------------------------------------------------------
 * w = u * v (multiplication component-wise)
 * RETURNS: w
 */
vector2 *v2_mul(vector2 *u, vector2 *v, vector2 *w)
{
  w->x = u->x * v->x;
  w->y = u->y * v->y;
  return( w );
}

/*----------------------------------------------------------------------------
 * RETURNS: transformed point p * projection matrix m
 */
point2 *v2_mul_by_proj(point2 *p, matrix3 *A)
{
  double w;
  point2 q;

  q.x = (p->x * A->element[0][0]) +
        (p->y * A->element[1][0]) + A->element[2][0];
  q.y = (p->x * A->element[0][1]) +
        (p->y * A->element[1][1]) + A->element[2][1];
  w = (p->x * A->element[0][2]) +
      (p->y * A->element[1][2]) + A->element[2][2];
  if (w != 0.0)
    q.x /= w, q.y /= w;
  *p = q;
  return( q );
}

/*----------------------------------------------------------------------------
 * v is negated
 * RETURNS: v
 */
vector2 *v2_neg(vector2 *v)
{
  v->x = -v->x, v->y = -v->y;
  return( v );
}

/*----------------------------------------------------------------------------
 * RETURNS: new 2D vector initialized to (x,y)
 */
vector2 *v2_new(double x, double y)
{
  vector2 *u = NEWTYPE(vector2);

  u->x = x, u->y = y;
  return( u );
}
        
/*----------------------------------------------------------------------------
 * v is normalized (becomes unit length)
 * RETURNS: v
 */
vector2 *v2_norm(vector2 *v)
{
  double len = V2Length(v);

  if (len != 0.0)
    v->x /= len, v->y /= len;
  return( v );
}

/*----------------------------------------------------------------------------
 * u = some orthogonal vector to v
 * RETURNS: u
 */
vector2 *v2_ortho(vector2 *u, vector2 *v)
{
  v->x = -u->y;
  v->y =  u->x;
  return( v );
}