--- ray/src/common/fvect.c 2010/09/26 15:42:48 2.12 +++ ray/src/common/fvect.c 2014/12/08 23:51:12 2.21 @@ -1,5 +1,5 @@ #ifndef lint -static const char RCSid[] = "$Id: fvect.c,v 2.12 2010/09/26 15:42:48 greg Exp $"; +static const char RCSid[] = "$Id: fvect.c,v 2.21 2014/12/08 23:51:12 greg Exp $"; #endif /* * fvect.c - routines for floating-point vector calculations @@ -7,14 +7,35 @@ static const char RCSid[] = "$Id: fvect.c,v 2.12 2010/ #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 */ -FVECT v1, -FVECT v2 +const FVECT v1, +const FVECT v2 ) { return(DOT(v1,v2)); @@ -23,15 +44,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 +58,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 +75,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 +102,29 @@ 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]; + 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 */ 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,7 +140,7 @@ 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 */ d = 2.0 - len; } else { @@ -135,12 +156,35 @@ FVECT v int +getperpendicular( /* choose random perpedicular direction */ +FVECT vp, /* returns normalized */ +const FVECT v /* input vector must be normalized */ +) +{ + FVECT v1; + int i; + /* randomize other coordinates */ + v1[0] = 0.5 - frandom(); + v1[1] = 0.5 - frandom(); + v1[2] = 0.5 - frandom(); + for (i = 3; i--; ) + if ((-0.6 < v[i]) & (v[i] < 0.6)) + break; + if (i < 0) + return(0); + v1[i] = 1.0; + fcross(vp, v1, v); + return(normalize(vp) > 0.0); +} + + +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); @@ -162,10 +206,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; @@ -180,7 +224,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 */ }