Math/MAT3vec.c

   1 /* Copyright 1988, Brown Computer Graphics Group.  All Rights Reserved. */
   2
   3 /* --------------------------------------------------------------------------
   4  * This file contains routines that operate on matrices and vectors, or
   5  * vectors and vectors.
   6  * -------------------------------------------------------------------------*/
   7
   8 /* #include "sphigslocal.h" */
   9
  10 /* --------------------------  Static Routines  ---------------------------- */
  11
  12 /* -------------------------  Internal Routines  --------------------------- */
  13
  14 /* --------------------------  Public Routines  ---------------------------- */
  15
  16 /*
  17  * Multiplies a vector by a matrix, setting the result vector.
  18  * It assumes all homogeneous coordinates are 1.
  19  * The two vectors involved may be the same.
  20  */
  21
  22 #include "mat3.h"
  23
  24 #ifndef TRUE
  25 #  define TRUE 1
  26 #endif
  27
  28 #ifndef FALSE
  29 #  define FALSE 0
  30 #endif
  31
  32
  33 void
  34 MAT3mult_vec(result_vec, vec, mat)
  35 MAT3vec          result_vec;
  36 register MAT3vec vec;
  37 register MAT3mat mat;
  38 {
  39    MAT3vec              tempvec;
  40    register double      *temp = tempvec;
  41
  42    temp[0] =    vec[0] * mat[0][0] + vec[1] * mat[1][0] +
  43                 vec[2] * mat[2][0] +          mat[3][0];
  44    temp[1] =    vec[0] * mat[0][1] + vec[1] * mat[1][1] +
  45                 vec[2] * mat[2][1] +          mat[3][1];
  46    temp[2] =    vec[0] * mat[0][2] + vec[1] * mat[1][2] +
  47                 vec[2] * mat[2][2] +          mat[3][2];
  48
  49    MAT3_COPY_VEC(result_vec, temp);
  50 }
  51
  52 /*
  53  * Multiplies a vector of size 4 by a matrix, setting the result vector.
  54  * The fourth element of the vector is the homogeneous coordinate, which
  55  * may or may not be 1.  If the "normalize" parameter is TRUE, then the
  56  * result vector will be normalized so that the homogeneous coordinate is 1.
  57  * The two vectors involved may be the same.
  58  * This returns zero if the vector was to be normalized, but couldn't be.
  59  */
  60
  61 int
  62 MAT3mult_hvec(result_vec, vec, mat, normalize)
  63 MAT3hvec          result_vec;
  64 register MAT3hvec vec;
  65 register MAT3mat  mat;
  66 {
  67    MAT3hvec             tempvec;
  68    double               norm_fac;
  69    register double      *temp = tempvec;
  70    register int         ret = TRUE;
  71
  72    temp[0] =    vec[0] * mat[0][0] + vec[1] * mat[1][0] +
  73                 vec[2] * mat[2][0] + vec[3] * mat[3][0];
  74    temp[1] =    vec[0] * mat[0][1] + vec[1] * mat[1][1] +
  75                 vec[2] * mat[2][1] + vec[3] * mat[3][1];
  76    temp[2] =    vec[0] * mat[0][2] + vec[1] * mat[1][2] +
  77                 vec[2] * mat[2][2] + vec[3] * mat[3][2];
  78    temp[3] =    vec[0] * mat[0][3] + vec[1] * mat[1][3] +
  79                 vec[2] * mat[2][3] + vec[3] * mat[3][3];
  80
  81    /* Normalize if asked for, possible, and necessary */
  82    if (normalize) {
  83       if (MAT3_IS_ZERO(temp[3])) {
  84 #ifndef THINK_C
  85          fprintf (stderr,
  86                   "Can't normalize vector: homogeneous coordinate is 0");
  87 #endif
  88          ret = FALSE;
  89       }
  90       else {
  91          norm_fac = 1.0 / temp[3];
  92          MAT3_SCALE_VEC(result_vec, temp, norm_fac);
  93          result_vec[3] = 1.0;
  94       }
  95    }
  96    else MAT3_COPY_HVEC(result_vec, temp);
  97
  98    return(ret);
  99 }
 100
 101 /*
 102  * Sets the first vector to be the cross-product of the last two vectors.
 103  */
 104
 105 void
 106 MAT3cross_product(result_vec, vec1, vec2)
 107 MAT3vec          result_vec;
 108 register MAT3vec vec1, vec2;
 109 {
 110    MAT3vec              tempvec;
 111    register double      *temp = tempvec;
 112
 113    temp[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
 114    temp[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
 115    temp[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
 116
 117    MAT3_COPY_VEC(result_vec, temp);
 118 }
 119
 120 /*
 121  * Finds a vector perpendicular to vec and stores it in result_vec.
 122  * Method:  take any vector (we use <0,1,0>) and subtract the
 123  * portion of it pointing in the vec direction.  This doesn't
 124  * work if vec IS <0,1,0> or is very near it.  So if this is
 125  * the case, use <0,0,1> instead.
 126  * If "is_unit" is TRUE, the given vector is assumed to be unit length.
 127  */
 128
 129 #define SELECT  .7071   /* selection constant (roughly .5*sqrt(2) */
 130
 131 void
 132 MAT3perp_vec(result_vec, vec, is_unit)
 133 MAT3vec result_vec, vec;
 134 int     is_unit;
 135 {
 136    MAT3vec      norm;
 137    double       dot;
 138
 139    MAT3_SET_VEC(result_vec, 0.0, 1.0, 0.0);
 140
 141    MAT3_COPY_VEC(norm, vec);
 142
 143    if (! is_unit) MAT3_NORMALIZE_VEC(norm, dot);
 144
 145    /* See if vector is too close to <0,1,0>.  If so, use <0,0,1> */
 146    if ((dot = MAT3_DOT_PRODUCT(norm, result_vec)) > SELECT || dot < -SELECT) {
 147       result_vec[1] = 0.0;
 148       result_vec[2] = 1.0;
 149       dot = MAT3_DOT_PRODUCT(norm, result_vec);
 150    }
 151
 152    /* Subtract off non-perpendicular part */
 153    result_vec[0] -= dot * norm[0];
 154    result_vec[1] -= dot * norm[1];
 155    result_vec[2] -= dot * norm[2];
 156
 157    /* Make result unit length */
 158    MAT3_NORMALIZE_VEC(result_vec, dot);
 159 }