Back to index

radiance  4R0+20100331
fvect.c
Go to the documentation of this file.
00001 #ifndef lint
00002 static const char    RCSid[] = "$Id: fvect.c,v 2.11 2009/05/07 21:38:35 greg Exp $";
00003 #endif
00004 /*
00005  *  fvect.c - routines for floating-point vector calculations
00006  */
00007 
00008 #include "copyright.h"
00009 
00010 #include  <math.h>
00011 #include  "fvect.h"
00012 
00013 
00014 double
00015 fdot(                       /* return the dot product of two vectors */
00016 FVECT v1,
00017 FVECT v2
00018 )
00019 {
00020        return(DOT(v1,v2));
00021 }
00022 
00023 
00024 double
00025 dist2(                      /* return square of distance between points */
00026 FVECT p1,
00027 FVECT p2
00028 )
00029 {
00030        FVECT  delta;
00031 
00032        delta[0] = p2[0] - p1[0];
00033        delta[1] = p2[1] - p1[1];
00034        delta[2] = p2[2] - p1[2];
00035 
00036        return(DOT(delta, delta));
00037 }
00038 
00039 
00040 double
00041 dist2line(                  /* return square of distance to line */
00042 FVECT p,             /* the point */
00043 FVECT ep1,
00044 FVECT ep2            /* points on the line */
00045 )
00046 {
00047        double  d, d1, d2;
00048 
00049        d = dist2(ep1, ep2);
00050        d1 = dist2(ep1, p);
00051        d2 = d + d1 - dist2(ep2, p);
00052 
00053        return(d1 - 0.25*d2*d2/d);
00054 }
00055 
00056 
00057 double
00058 dist2lseg(                  /* return square of distance to line segment */
00059 FVECT p,             /* the point */
00060 FVECT ep1,
00061 FVECT ep2            /* the end points */
00062 )
00063 {
00064        double  d, d1, d2;
00065 
00066        d = dist2(ep1, ep2);
00067        d1 = dist2(ep1, p);
00068        d2 = dist2(ep2, p);
00069 
00070        if (d2 > d1) {                     /* check if past endpoints */
00071               if (d2 - d1 > d)
00072                      return(d1);
00073        } else {
00074               if (d1 - d2 > d)
00075                      return(d2);
00076        }
00077        d2 = d + d1 - d2;
00078 
00079        return(d1 - 0.25*d2*d2/d);  /* distance to line */
00080 }
00081 
00082 
00083 void
00084 fcross(                            /* vres = v1 X v2 */
00085 FVECT vres,
00086 FVECT v1,
00087 FVECT v2
00088 )
00089 {
00090        vres[0] = v1[1]*v2[2] - v1[2]*v2[1];
00091        vres[1] = v1[2]*v2[0] - v1[0]*v2[2];
00092        vres[2] = v1[0]*v2[1] - v1[1]*v2[0];
00093 }
00094 
00095 
00096 void
00097 fvsum(                      /* vres = v0 + f*v1 */
00098 FVECT vres,
00099 FVECT v0,
00100 FVECT v1,
00101 double f
00102 )
00103 {
00104        vres[0] = v0[0] + f*v1[0];
00105        vres[1] = v0[1] + f*v1[1];
00106        vres[2] = v0[2] + f*v1[2];
00107 }
00108 
00109 
00110 double
00111 normalize(                  /* normalize a vector, return old magnitude */
00112 FVECT  v
00113 )
00114 {
00115        double  len, d;
00116        
00117        d = DOT(v, v);
00118        
00119        if (d == 0.0)
00120               return(0.0);
00121        
00122        if (d <= 1.0+FTINY && d >= 1.0-FTINY)
00123               len = 0.5 + 0.5*d;   /* first order approximation */
00124        else
00125               len = sqrt(d);
00126 
00127        v[0] *= d = 1.0/len;
00128        v[1] *= d;
00129        v[2] *= d;
00130 
00131        return(len);
00132 }
00133 
00134 
00135 int
00136 closestapproach(                   /* closest approach of two rays */
00137 RREAL t[2],          /* returned distances along each ray */
00138 FVECT rorg0,         /* first origin */
00139 FVECT rdir0,         /* first direction (normalized) */
00140 FVECT rorg1,         /* second origin */
00141 FVECT rdir1          /* second direction (normalized) */
00142 )
00143 {
00144        double dotprod = DOT(rdir0, rdir1);
00145        double denom = 1. - dotprod*dotprod;
00146        double o1o2_d1;
00147        FVECT  o0o1;
00148 
00149        if (denom <= FTINY) {              /* check if lines are parallel */
00150               t[0] = t[1] = 0.0;
00151               return(0);
00152        }
00153        VSUB(o0o1, rorg0, rorg1);
00154        o1o2_d1 = DOT(o0o1, rdir1);
00155        t[0] = (o1o2_d1*dotprod - DOT(o0o1,rdir0)) / denom;
00156        t[1] = o1o2_d1 + t[0]*dotprod;
00157        return(1);
00158 }
00159 
00160 
00161 void
00162 spinvector(                        /* rotate vector around normal */
00163 FVECT vres,          /* returned vector */
00164 FVECT vorig,         /* original vector */
00165 FVECT vnorm,         /* normalized vector for rotation */
00166 double theta         /* left-hand radians */
00167 )
00168 {
00169        double  sint, cost, normprod;
00170        FVECT  vperp;
00171        int  i;
00172        
00173        if (theta == 0.0) {
00174               if (vres != vorig)
00175                      VCOPY(vres, vorig);
00176               return;
00177        }
00178        cost = cos(theta);
00179        sint = sin(theta);
00180        normprod = DOT(vorig, vnorm)*(1.-cost);
00181        fcross(vperp, vnorm, vorig);
00182        for (i = 0; i < 3; i++)
00183               vres[i] = vorig[i]*cost + vnorm[i]*normprod + vperp[i]*sint;
00184 }