--- ray/src/common/fvect.c	1989/05/07 21:41:40	1.2
+++ ray/src/common/fvect.c	2011/04/19 21:31:22	2.14
@@ -1,62 +1,67 @@
-/* Copyright (c) 1986 Regents of the University of California */
-
 #ifndef lint
-static char SCCSid[] = "$SunId$ LBL";
+static const char	RCSid[] = "$Id: fvect.c,v 2.14 2011/04/19 21:31:22 greg Exp $";
 #endif
-
 /*
- *  fvect.c - routines for float vector calculations
- *
- *     8/14/85
+ *  fvect.c - routines for floating-point vector calculations
  */
 
+#include "copyright.h"
+
+#include  <math.h>
 #include  "fvect.h"
 
-#define  FTINY		1e-7
 
-
 double
-fdot(v1, v2)			/* return the dot product of two vectors */
-register FVECT  v1, v2;
+fdot(				/* return the dot product of two vectors */
+const FVECT v1,
+const FVECT v2
+)
 {
 	return(DOT(v1,v2));
 }
 
 
 double
-dist2(p1, p2)			/* return square of distance between points */
-register FVECT  p1, p2;
+dist2(				/* return square of distance between points */
+const FVECT p1,
+const FVECT p2
+)
 {
-	static FVECT  delta;
+	FVECT  delta;
 
 	delta[0] = p2[0] - p1[0];
 	delta[1] = p2[1] - p1[1];
 	delta[2] = p2[2] - p1[2];
+
 	return(DOT(delta, delta));
 }
 
 
 double
-dist2line(p, ep1, ep2)		/* return square of distance to line */
-FVECT  p;		/* the point */
-FVECT  ep1, ep2;	/* points on the line */
+dist2line(			/* return square of distance to line */
+const FVECT p,		/* the point */
+const FVECT ep1,
+const FVECT ep2		/* points on the line */
+)
 {
-	static double  d, d1, d2;
+	double  d, d1, d2;
 
 	d = dist2(ep1, ep2);
 	d1 = dist2(ep1, p);
-	d2 = dist2(ep2, p);
+	d2 = d + d1 - dist2(ep2, p);
 
-	return(d1 - (d+d1-d2)*(d+d1-d2)/d/4);
+	return(d1 - 0.25*d2*d2/d);
 }
 
 
 double
-dist2lseg(p, ep1, ep2)		/* return square of distance to line segment */
-FVECT  p;		/* the point */
-FVECT  ep1, ep2;	/* the end points */
+dist2lseg(			/* return square of distance to line segment */
+const FVECT p,		/* the point */
+const FVECT ep1,
+const FVECT ep2		/* the end points */
+)
 {
-	static double  d, d1, d2;
+	double  d, d1, d2;
 
 	d = dist2(ep1, ep2);
 	d1 = dist2(ep1, p);
@@ -69,13 +74,18 @@ FVECT  ep1, ep2;	/* the end points */
 		if (d1 - d2 > d)
 			return(d2);
 	}
+	d2 = d + d1 - d2;
 
-	return(d1 - (d+d1-d2)*(d+d1-d2)/d/4);	/* distance to line */
+	return(d1 - 0.25*d2*d2/d);	/* distance to line */
 }
 
 
-fcross(vres, v1, v2)		/* vres = v1 X v2 */
-register FVECT  vres, v1, v2;
+void
+fcross(				/* vres = v1 X v2 */
+FVECT vres,
+const FVECT v1,
+const FVECT v2
+)
 {
 	vres[0] = v1[1]*v2[2] - v1[2]*v2[1];
 	vres[1] = v1[2]*v2[0] - v1[0]*v2[2];
@@ -83,26 +93,94 @@ register FVECT  vres, v1, v2;
 }
 
 
+void
+fvsum(				/* vres = v0 + f*v1 */
+FVECT vres,
+const FVECT v0,
+const FVECT v1,
+double f
+)
+{
+	vres[0] = v0[0] + f*v1[0];
+	vres[1] = v0[1] + f*v1[1];
+	vres[2] = v0[2] + f*v1[2];
+}
+
+
 double
-normalize(v)			/* normalize a vector, return old magnitude */
-register FVECT  v;
+normalize(			/* normalize a vector, return old magnitude */
+FVECT  v
+)
 {
-	static double  len;
+	double  len, d;
 	
-	len = DOT(v, v);
+	d = DOT(v, v);
 	
-	if (len <= FTINY*FTINY)
+	if (d == 0.0)
 		return(0.0);
 	
-	/****** problematic
-	if (len >= (1.0-FTINY)*(1.0-FTINY) &&
-			len <= (1.0+FTINY)*(1.0+FTINY))
-		return(1.0);
-	******/
+	if (d <= 1.0+FTINY && d >= 1.0-FTINY) {
+		len = 0.5 + 0.5*d;	/* first order approximation */
+		d = 2.0 - len;
+	} else {
+		len = sqrt(d);
+		d = 1.0/len;
+	}
+	v[0] *= d;
+	v[1] *= d;
+	v[2] *= d;
 
-	len = sqrt(len);
-	v[0] /= len;
-	v[1] /= len;
-	v[2] /= len;
 	return(len);
+}
+
+
+int
+closestapproach(			/* closest approach of two rays */
+RREAL t[2],		/* returned distances along each ray */
+const FVECT rorg0,		/* first origin */
+const FVECT rdir0,		/* first direction (normalized) */
+const FVECT rorg1,		/* second origin */
+const FVECT rdir1		/* second direction (normalized) */
+)
+{
+	double	dotprod = DOT(rdir0, rdir1);
+	double	denom = 1. - dotprod*dotprod;
+	double	o1o2_d1;
+	FVECT	o0o1;
+
+	if (denom <= FTINY) {		/* check if lines are parallel */
+		t[0] = t[1] = 0.0;
+		return(0);
+	}
+	VSUB(o0o1, rorg0, rorg1);
+	o1o2_d1 = DOT(o0o1, rdir1);
+	t[0] = (o1o2_d1*dotprod - DOT(o0o1,rdir0)) / denom;
+	t[1] = o1o2_d1 + t[0]*dotprod;
+	return(1);
+}
+
+
+void
+spinvector(				/* rotate vector around normal */
+FVECT vres,		/* returned vector */
+const FVECT vorig,		/* original vector */
+const FVECT vnorm,		/* normalized vector for rotation */
+double theta		/* right-hand radians */
+)
+{
+	double  sint, cost, normprod;
+	FVECT  vperp;
+	int  i;
+	
+	if (theta == 0.0) {
+		if (vres != vorig)
+			VCOPY(vres, vorig);
+		return;
+	}
+	cost = cos(theta);
+	sint = sin(theta);
+	normprod = DOT(vorig, vnorm)*(1.-cost);
+	fcross(vperp, vnorm, vorig);
+	for (i = 0; i < 3; i++)
+		vres[i] = vorig[i]*cost + vnorm[i]*normprod + vperp[i]*sint;
 }