--- ray/src/common/fvect.c	2009/05/07 21:38:35	2.11
+++ ray/src/common/fvect.c	2013/04/03 00:22:12	2.18
@@ -1,5 +1,5 @@
 #ifndef lint
-static const char	RCSid[] = "$Id: fvect.c,v 2.11 2009/05/07 21:38:35 greg Exp $";
+static const char	RCSid[] = "$Id: fvect.c,v 2.18 2013/04/03 00:22:12 greg Exp $";
 #endif
 /*
  *  fvect.c - routines for floating-point vector calculations
@@ -13,8 +13,8 @@ static const char	RCSid[] = "$Id: fvect.c,v 2.11 2009/
 
 double
 fdot(				/* return the dot product of two vectors */
-FVECT v1,
-FVECT v2
+const FVECT v1,
+const FVECT v2
 )
 {
 	return(DOT(v1,v2));
@@ -23,15 +23,13 @@ FVECT v2
 
 double
 dist2(				/* return square of distance between points */
-FVECT p1,
-FVECT p2
+const FVECT p1,
+const FVECT p2
 )
 {
 	FVECT  delta;
 
-	delta[0] = p2[0] - p1[0];
-	delta[1] = p2[1] - p1[1];
-	delta[2] = p2[2] - p1[2];
+	VSUB(delta, p2, p1);
 
 	return(DOT(delta, delta));
 }
@@ -39,9 +37,9 @@ FVECT p2
 
 double
 dist2line(			/* return square of distance to line */
-FVECT p,		/* the point */
-FVECT ep1,
-FVECT ep2		/* points on the line */
+const FVECT p,		/* the point */
+const FVECT ep1,
+const FVECT ep2		/* points on the line */
 )
 {
 	double  d, d1, d2;
@@ -56,9 +54,9 @@ FVECT ep2		/* points on the line */
 
 double
 dist2lseg(			/* return square of distance to line segment */
-FVECT p,		/* the point */
-FVECT ep1,
-FVECT ep2		/* the end points */
+const FVECT p,		/* the point */
+const FVECT ep1,
+const FVECT ep2		/* the end points */
 )
 {
 	double  d, d1, d2;
@@ -83,27 +81,23 @@ FVECT ep2		/* the end points */
 void
 fcross(				/* vres = v1 X v2 */
 FVECT vres,
-FVECT v1,
-FVECT v2
+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];
-	vres[2] = v1[0]*v2[1] - v1[1]*v2[0];
+	VCROSS(vres, v1, v2);
 }
 
 
 void
 fvsum(				/* vres = v0 + f*v1 */
 FVECT vres,
-FVECT v0,
-FVECT v1,
+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];
+	VSUM(vres, v0, v1, f);
 }
 
 
@@ -119,12 +113,14 @@ FVECT  v
 	if (d == 0.0)
 		return(0.0);
 	
-	if (d <= 1.0+FTINY && d >= 1.0-FTINY)
+	if ((d <= 1.0+FTINY) & (d >= 1.0-FTINY)) {
 		len = 0.5 + 0.5*d;	/* first order approximation */
-	else
+		d = 2.0 - len;
+	} else {
 		len = sqrt(d);
-
-	v[0] *= d = 1.0/len;
+		d = 1.0/len;
+	}
+	v[0] *= d;
 	v[1] *= d;
 	v[2] *= d;
 
@@ -135,10 +131,10 @@ FVECT  v
 int
 closestapproach(			/* closest approach of two rays */
 RREAL t[2],		/* returned distances along each ray */
-FVECT rorg0,		/* first origin */
-FVECT rdir0,		/* first direction (normalized) */
-FVECT rorg1,		/* second origin */
-FVECT rdir1		/* second direction (normalized) */
+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);
@@ -160,10 +156,10 @@ FVECT rdir1		/* second direction (normalized) */
 
 void
 spinvector(				/* rotate vector around normal */
-FVECT vres,		/* returned vector */
-FVECT vorig,		/* original vector */
-FVECT vnorm,		/* normalized vector for rotation */
-double theta		/* left-hand radians */
+FVECT vres,		/* returned vector (same magnitude as vorig) */
+const FVECT vorig,		/* original vector */
+const FVECT vnorm,		/* normalized vector for rotation */
+double theta		/* right-hand radians */
 )
 {
 	double  sint, cost, normprod;
@@ -178,7 +174,52 @@ double theta		/* left-hand radians */
 	cost = cos(theta);
 	sint = sin(theta);
 	normprod = DOT(vorig, vnorm)*(1.-cost);
-	fcross(vperp, vnorm, vorig);
+	VCROSS(vperp, vnorm, vorig);
 	for (i = 0; i < 3; i++)
 		vres[i] = vorig[i]*cost + vnorm[i]*normprod + vperp[i]*sint;
+}
+
+double
+geodesic(		/* rotate vector on great circle towards target */
+FVECT vres,		/* returned vector (same magnitude as vorig) */
+const FVECT vorig,	/* original vector */
+const FVECT vtarg,	/* vector we are rotating towards */
+double t,		/* amount along arc directed towards vtarg */
+int meas		/* distance measure (radians, absolute, relative) */
+)
+{
+	FVECT	normtarg;
+	double	volen, dotprod, sintr, cost;
+	int	i;
+
+	VCOPY(normtarg, vtarg);		/* in case vtarg==vres */
+	if (vres != vorig)
+		VCOPY(vres, vorig);
+	if (t == 0.0)
+		return(VLEN(vres));	/* no rotation requested */
+	if ((volen = normalize(vres)) == 0.0)
+		return(0.0);
+	if (normalize(normtarg) == 0.0)
+		return(0.0);		/* target vector is zero */
+	dotprod = DOT(vres, normtarg);
+					/* check for colinear */
+	if (dotprod >= 1.0-FTINY*FTINY) {
+		if (meas != GEOD_REL)
+			return(0.0);
+		vres[0] *= volen; vres[1] *= volen; vres[2] *= volen;
+		return(volen);
+	}
+	if (dotprod <= -1.0+FTINY*FTINY)
+		return(0.0);
+	if (meas == GEOD_ABS)
+		t /= volen;
+	else if (meas == GEOD_REL)
+		t *= acos(dotprod);
+	cost = cos(t);
+	sintr = sin(t) / sqrt(1. - dotprod*dotprod);
+	for (i = 0; i < 3; i++)
+		vres[i] = volen*( cost*vres[i] +
+				  sintr*(normtarg[i] - dotprod*vres[i]) );
+
+	return(volen);			/* return vector length */
 }