--- ray/src/common/fvect.c 1989/05/08 08:40:56 1.3 +++ ray/src/common/fvect.c 2005/03/23 01:54:55 2.9 @@ -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.9 2005/03/23 01:54:55 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 #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 */ +register FVECT v1, +register 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 */ +register FVECT p1, +register 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 */ +FVECT p, /* the point */ +FVECT ep1, +FVECT ep2 /* points on the line */ +) { - static double d, d1, d2; + register 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 */ +FVECT p, /* the point */ +FVECT ep1, +FVECT ep2 /* the end points */ +) { - static double d, d1, d2; + register 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 */ +register FVECT vres, +register FVECT v1, +register 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,122 @@ register FVECT vres, v1, v2; } +void +fvsum( /* vres = v0 + f*v1 */ +register FVECT vres, +register FVECT v0, +register FVECT v1, +register 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 */ +register FVECT v +) { - static double len; + register double len, d; - len = DOT(v, v); + d = DOT(v, v); - if (len <= 0.0) + if (d <= FTINY*FTINY) 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 */ + else + len = sqrt(d); - len = sqrt(len); - v[0] /= len; - v[1] /= len; - v[2] /= len; + v[0] *= d = 1.0/len; + v[1] *= d; + v[2] *= d; + return(len); +} + + +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) */ +) +{ + 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); +} + + +#if 0 +int +closestapproach( /* closest approach of two rays */ +RREAL t[2], /* returned distances along each ray */ +FVECT rorg0, /* first origin */ +FVECT rdir0, /* first direction (unnormalized) */ +FVECT rorg1, /* second origin */ +FVECT rdir1 /* second direction (unnormalized) */ +) +{ + double dotprod = DOT(rdir0, rdir1); + double d0n2 = DOT(rdir0, rdir0); + double d1n2 = DOT(rdir1, rdir1); + double denom = d0n2*d1n2 - 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)*d1n2) / denom; + t[1] = (o1o2_d1 + t[0]*dotprod) / d1n2; + return(1); +} +#endif + + +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 */ +) +{ + double sint, cost, normprod; + FVECT vperp; + register 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; }