FixObj/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(double *result_vec, register double *vec, register double (*mat)[4])
  35 {
  36    MAT3vec              tempvec;
  37    register double      *temp = tempvec;
  38
  39    temp[0] =    vec[0] * mat[0][0] + vec[1] * mat[1][0] +
  40                 vec[2] * mat[2][0] +          mat[3][0];
  41    temp[1] =    vec[0] * mat[0][1] + vec[1] * mat[1][1] +
  42                 vec[2] * mat[2][1] +          mat[3][1];
  43    temp[2] =    vec[0] * mat[0][2] + vec[1] * mat[1][2] +
  44                 vec[2] * mat[2][2] +          mat[3][2];
  45
  46    MAT3_COPY_VEC(result_vec, temp);
  47 }
  48
  49 /*
  50  * Multiplies a vector of size 4 by a matrix, setting the result vector.
  51  * The fourth element of the vector is the homogeneous coordinate, which
  52  * may or may not be 1.  If the "normalize" parameter is TRUE, then the
  53  * result vector will be normalized so that the homogeneous coordinate is 1.
  54  * The two vectors involved may be the same.
  55  * This returns zero if the vector was to be normalized, but couldn't be.
  56  */
  57
  58 int
  59 MAT3mult_hvec(double *result_vec, register double *vec, register double (*mat)[4], int normalize)
  60 {
  61    MAT3hvec             tempvec;
  62    double               norm_fac;
  63    register double      *temp = tempvec;
  64    register int         ret = TRUE;
  65
  66    temp[0] =    vec[0] * mat[0][0] + vec[1] * mat[1][0] +
  67                 vec[2] * mat[2][0] + vec[3] * mat[3][0];
  68    temp[1] =    vec[0] * mat[0][1] + vec[1] * mat[1][1] +
  69                 vec[2] * mat[2][1] + vec[3] * mat[3][1];
  70    temp[2] =    vec[0] * mat[0][2] + vec[1] * mat[1][2] +
  71                 vec[2] * mat[2][2] + vec[3] * mat[3][2];
  72    temp[3] =    vec[0] * mat[0][3] + vec[1] * mat[1][3] +
  73                 vec[2] * mat[2][3] + vec[3] * mat[3][3];
  74
  75    /* Normalize if asked for, possible, and necessary */
  76    if (normalize) {
  77       if (MAT3_IS_ZERO(temp[3])) {
  78 #ifndef THINK_C
  79          fprintf (stderr,
  80                   "Can't normalize vector: homogeneous coordinate is 0");
  81 #endif
  82          ret = FALSE;
  83       }
  84       else {
  85          norm_fac = 1.0 / temp[3];
  86          MAT3_SCALE_VEC(result_vec, temp, norm_fac);
  87          result_vec[3] = 1.0;
  88       }
  89    }
  90    else MAT3_COPY_HVEC(result_vec, temp);
  91
  92    return(ret);
  93 }
  94
  95 /*
  96  * Sets the first vector to be the cross-product of the last two vectors.
  97  */
  98
  99 void
 100 MAT3cross_product(double *result_vec, register double *vec1, register double *vec2)
 101 {
 102    MAT3vec              tempvec;
 103    register double      *temp = tempvec;
 104
 105    temp[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
 106    temp[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
 107    temp[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
 108
 109    MAT3_COPY_VEC(result_vec, temp);
 110 }
 111
 112 /*
 113  * Finds a vector perpendicular to vec and stores it in result_vec.
 114  * Method:  take any vector (we use <0,1,0>) and subtract the
 115  * portion of it pointing in the vec direction.  This doesn't
 116  * work if vec IS <0,1,0> or is very near it.  So if this is
 117  * the case, use <0,0,1> instead.
 118  * If "is_unit" is TRUE, the given vector is assumed to be unit length.
 119  */
 120
 121 #define SELECT  .7071   /* selection constant (roughly .5*sqrt(2) */
 122
 123 void
 124 MAT3perp_vec(double *result_vec, double *vec, int is_unit)
 125 {
 126    MAT3vec      norm;
 127    double       dot;
 128
 129    MAT3_SET_VEC(result_vec, 0.0, 1.0, 0.0);
 130
 131    MAT3_COPY_VEC(norm, vec);
 132
 133    if (! is_unit) MAT3_NORMALIZE_VEC(norm, dot);
 134
 135    /* See if vector is too close to <0,1,0>.  If so, use <0,0,1> */
 136    if ((dot = MAT3_DOT_PRODUCT(norm, result_vec)) > SELECT || dot < -SELECT) {
 137       result_vec[1] = 0.0;
 138       result_vec[2] = 1.0;
 139       dot = MAT3_DOT_PRODUCT(norm, result_vec);
 140    }
 141
 142    /* Subtract off non-perpendicular part */
 143    result_vec[0] -= dot * norm[0];
 144    result_vec[1] -= dot * norm[1];
 145    result_vec[2] -= dot * norm[2];
 146
 147    /* Make result unit length */
 148    MAT3_NORMALIZE_VEC(result_vec, dot);
 149 }