The MAT3 routines from SRGP.

Math/MAT3vec.c

@@ -0,0 +1,159 @@
+/* Copyright 1988, Brown Computer Graphics Group.  All Rights Reserved. */
+
+/* --------------------------------------------------------------------------
+ * This file contains routines that operate on matrices and vectors, or
+ * vectors and vectors.
+ * -------------------------------------------------------------------------*/
+
+/* #include "sphigslocal.h" */
+
+/* --------------------------  Static Routines	---------------------------- */
+
+/* -------------------------  Internal Routines  --------------------------- */
+
+/* --------------------------  Public Routines	---------------------------- */
+
+/*
+ * Multiplies a vector by a matrix, setting the result vector.
+ * It assumes all homogeneous coordinates are 1.
+ * The two vectors involved may be the same.
+ */
+
+#include "mat3.h"
+
+#ifndef TRUE
+#  define TRUE 1
+#endif
+
+#ifndef FALSE
+#  define FALSE 0
+#endif
+
+
+void
+MAT3mult_vec(result_vec, vec, mat)
+MAT3vec 	 result_vec;
+register MAT3vec vec;
+register MAT3mat mat;
+{
+   MAT3vec		tempvec;
+   register double	*temp = tempvec;
+
+   temp[0] =	vec[0] * mat[0][0] + vec[1] * mat[1][0] +
+		vec[2] * mat[2][0] +	      mat[3][0];
+   temp[1] =	vec[0] * mat[0][1] + vec[1] * mat[1][1] +
+		vec[2] * mat[2][1] +	      mat[3][1];
+   temp[2] =	vec[0] * mat[0][2] + vec[1] * mat[1][2] +
+		vec[2] * mat[2][2] +	      mat[3][2];
+
+   MAT3_COPY_VEC(result_vec, temp);
+}
+
+/*
+ * Multiplies a vector of size 4 by a matrix, setting the result vector.
+ * The fourth element of the vector is the homogeneous coordinate, which
+ * may or may not be 1.  If the "normalize" parameter is TRUE, then the
+ * result vector will be normalized so that the homogeneous coordinate is 1.
+ * The two vectors involved may be the same.
+ * This returns zero if the vector was to be normalized, but couldn't be.
+ */
+
+int
+MAT3mult_hvec(result_vec, vec, mat, normalize)
+MAT3hvec          result_vec;
+register MAT3hvec vec;
+register MAT3mat  mat;
+{
+   MAT3hvec             tempvec;
+   double		norm_fac;
+   register double	*temp = tempvec;
+   register int 	ret = TRUE;
+
+   temp[0] =	vec[0] * mat[0][0] + vec[1] * mat[1][0] +
+		vec[2] * mat[2][0] + vec[3] * mat[3][0];
+   temp[1] =	vec[0] * mat[0][1] + vec[1] * mat[1][1] +
+		vec[2] * mat[2][1] + vec[3] * mat[3][1];
+   temp[2] =	vec[0] * mat[0][2] + vec[1] * mat[1][2] +
+		vec[2] * mat[2][2] + vec[3] * mat[3][2];
+   temp[3] =	vec[0] * mat[0][3] + vec[1] * mat[1][3] +
+		vec[2] * mat[2][3] + vec[3] * mat[3][3];
+
+   /* Normalize if asked for, possible, and necessary */
+   if (normalize) {
+      if (MAT3_IS_ZERO(temp[3])) {
+#ifndef THINK_C
+	 fprintf (stderr,
+		  "Can't normalize vector: homogeneous coordinate is 0");
+#endif
+	 ret = FALSE;
+      }
+      else {
+	 norm_fac = 1.0 / temp[3];
+	 MAT3_SCALE_VEC(result_vec, temp, norm_fac);
+	 result_vec[3] = 1.0;
+      }
+   }
+   else MAT3_COPY_HVEC(result_vec, temp);
+
+   return(ret);
+}
+
+/*
+ * Sets the first vector to be the cross-product of the last two vectors.
+ */
+
+void
+MAT3cross_product(result_vec, vec1, vec2)
+MAT3vec 	 result_vec;
+register MAT3vec vec1, vec2;
+{
+   MAT3vec		tempvec;
+   register double	*temp = tempvec;
+
+   temp[0] = vec1[1] * vec2[2] - vec1[2] * vec2[1];
+   temp[1] = vec1[2] * vec2[0] - vec1[0] * vec2[2];
+   temp[2] = vec1[0] * vec2[1] - vec1[1] * vec2[0];
+
+   MAT3_COPY_VEC(result_vec, temp);
+}
+
+/*
+ * Finds a vector perpendicular to vec and stores it in result_vec.
+ * Method:  take any vector (we use <0,1,0>) and subtract the
+ * portion of it pointing in the vec direction.  This doesn't
+ * work if vec IS <0,1,0> or is very near it.  So if this is
+ * the case, use <0,0,1> instead.
+ * If "is_unit" is TRUE, the given vector is assumed to be unit length.
+ */
+
+#define SELECT	.7071	/* selection constant (roughly .5*sqrt(2) */
+
+void
+MAT3perp_vec(result_vec, vec, is_unit)
+MAT3vec result_vec, vec;
+int	is_unit;
+{
+   MAT3vec	norm;
+   double	dot;
+
+   MAT3_SET_VEC(result_vec, 0.0, 1.0, 0.0);
+
+   MAT3_COPY_VEC(norm, vec);
+
+   if (! is_unit) MAT3_NORMALIZE_VEC(norm, dot);
+
+   /* See if vector is too close to <0,1,0>.  If so, use <0,0,1> */
+   if ((dot = MAT3_DOT_PRODUCT(norm, result_vec)) > SELECT || dot < -SELECT) {
+      result_vec[1] = 0.0;
+      result_vec[2] = 1.0;
+      dot = MAT3_DOT_PRODUCT(norm, result_vec);
+   }
+
+   /* Subtract off non-perpendicular part */
+   result_vec[0] -= dot * norm[0];
+   result_vec[1] -= dot * norm[1];
+   result_vec[2] -= dot * norm[2];
+
+   /* Make result unit length */
+   MAT3_NORMALIZE_VEC(result_vec, dot);
+}