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