--- ray/src/common/fvect.c 2003/09/16 06:30:20 2.8 +++ ray/src/common/fvect.c 2022/04/21 22:31:42 2.25 @@ -1,5 +1,5 @@ #ifndef lint -static const char RCSid[] = "$Id: fvect.c,v 2.8 2003/09/16 06:30:20 greg Exp $"; +static const char RCSid[] = "$Id: fvect.c,v 2.25 2022/04/21 22:31:42 greg Exp $"; #endif /* * fvect.c - routines for floating-point vector calculations @@ -7,14 +7,35 @@ static const char RCSid[] = "$Id: fvect.c,v 2.8 2003/0 #include "copyright.h" +#define _USE_MATH_DEFINES #include #include "fvect.h" +#include "random.h" +double +Acos(double x) /* insurance for touchy math library */ +{ + if (x <= -1.+FTINY*FTINY) + return(M_PI); + if (x >= 1.-FTINY*FTINY) + return(.0); + return(acos(x)); +} double +Asin(double x) /* insurance for touchy math library */ +{ + if (x <= -1.+FTINY*FTINY) + return(-M_PI/2.); + if (x >= 1.-FTINY*FTINY) + return(M_PI/2); + return(asin(x)); +} + +double fdot( /* return the dot product of two vectors */ -register FVECT v1, -register FVECT v2 +const FVECT v1, +const FVECT v2 ) { return(DOT(v1,v2)); @@ -23,15 +44,13 @@ register FVECT v2 double dist2( /* return square of distance between points */ -register FVECT p1, -register 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,12 +58,12 @@ register 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 */ ) { - register double d, d1, d2; + double d, d1, d2; d = dist2(ep1, ep2); d1 = dist2(ep1, p); @@ -56,12 +75,12 @@ 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 */ ) { - register double d, d1, d2; + double d, d1, d2; d = dist2(ep1, ep2); d1 = dist2(ep1, p); @@ -82,49 +101,53 @@ FVECT ep2 /* the end points */ void fcross( /* vres = v1 X v2 */ -register FVECT vres, -register FVECT v1, -register FVECT 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]; - vres[2] = v1[0]*v2[1] - v1[1]*v2[0]; + if ((vres == v1) | (vres == v2)) { + FVECT vtmp; + VCROSS(vtmp, v1, v2); + VCOPY(vres, vtmp); + return; + } + VCROSS(vres, v1, v2); } void fvsum( /* vres = v0 + f*v1 */ -register FVECT vres, -register FVECT v0, -register FVECT v1, -register double f +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]; + VSUM(vres, v0, v1, f); } double normalize( /* normalize a vector, return old magnitude */ -register FVECT v +FVECT v ) { - register double len, d; + double len, d; d = DOT(v, v); - if (d <= 0.0) + 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; @@ -133,45 +156,57 @@ register 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) */ +getperpendicular( /* choose perpedicular direction */ +FVECT vp, /* returns normalized */ +const FVECT v, /* input vector must be normalized */ +int randomize /* randomize orientation */ ) { - double dotprod = DOT(rdir0, rdir1); - double denom = 1. - dotprod*dotprod; - double o1o2_d1; - FVECT o0o1; + int ord[3]; + FVECT v1; + int i; - if (denom <= FTINY) { /* check if lines are parallel */ - t[0] = t[1] = 0.0; - return(0); + if (randomize) { /* randomize coordinates? */ + v1[0] = 0.5 - frandom(); + v1[1] = 0.5 - frandom(); + v1[2] = 0.5 - frandom(); + switch (irandom(6)) { + case 0: ord[0] = 0; ord[1] = 1; ord[2] = 2; break; + case 1: ord[0] = 0; ord[1] = 2; ord[2] = 1; break; + case 2: ord[0] = 1; ord[1] = 0; ord[2] = 2; break; + case 3: ord[0] = 1; ord[1] = 2; ord[2] = 0; break; + case 4: ord[0] = 2; ord[1] = 0; ord[2] = 1; break; + default: ord[0] = 2; ord[1] = 1; ord[2] = 0; break; + } + } else { + v1[0] = v1[1] = v1[2] = 0.0; + ord[0] = 0; ord[1] = 1; ord[2] = 2; } - 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); + + for (i = 3; i--; ) + if ((-0.6 < v[ord[i]]) & (v[ord[i]] < 0.6)) + break; + if (i < 0) + return(0); + + v1[ord[i]] = 1.0; + fcross(vp, v1, v); + + return(normalize(vp) > 0.0); } -#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) */ +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 d0n2 = DOT(rdir0, rdir0); - double d1n2 = DOT(rdir1, rdir1); - double denom = d0n2*d1n2 - dotprod*dotprod; + double denom = 1. - dotprod*dotprod; double o1o2_d1; FVECT o0o1; @@ -181,24 +216,23 @@ FVECT rdir1 /* second direction (unnormalized) */ } 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; + t[0] = (o1o2_d1*dotprod - DOT(o0o1,rdir0)) / denom; + t[1] = o1o2_d1 + t[0]*dotprod; 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 */ +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; FVECT vperp; - register int i; + int i; if (theta == 0.0) { if (vres != vorig) @@ -208,7 +242,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 */ }