src/common/fvect.c

#ifndef lint
static const char       RCSid[] = "$Id: fvect.c,v 2.19 2013/06/29 21:03:44 greg Exp $";
#endif
/*
 *  fvect.c - routines for floating-point vector calculations
 */

#include "copyright.h"

#define _USE_MATH_DEFINES
#include  <math.h>
#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 */
const FVECT v1,
const FVECT v2
)
{
        return(DOT(v1,v2));
}


double
dist2(                          /* return square of distance between points */
const FVECT p1,
const FVECT p2
)
{
        FVECT  delta;

        VSUB(delta, p2, p1);

        return(DOT(delta, delta));
}


double
dist2line(                      /* return square of distance to line */
const FVECT p,          /* the point */
const FVECT ep1,
const FVECT ep2         /* points on the line */
)
{
        double  d, d1, d2;

        d = dist2(ep1, ep2);
        d1 = dist2(ep1, p);
        d2 = d + d1 - dist2(ep2, p);

        return(d1 - 0.25*d2*d2/d);
}


double
dist2lseg(                      /* return square of distance to line segment */
const FVECT p,          /* the point */
const FVECT ep1,
const FVECT ep2         /* the end points */
)
{
        double  d, d1, d2;

        d = dist2(ep1, ep2);
        d1 = dist2(ep1, p);
        d2 = dist2(ep2, p);

        if (d2 > d1) {                  /* check if past endpoints */
                if (d2 - d1 > d)
                        return(d1);
        } else {
                if (d1 - d2 > d)
                        return(d2);
        }
        d2 = d + d1 - d2;

        return(d1 - 0.25*d2*d2/d);      /* distance to line */
}


void
fcross(                         /* vres = v1 X v2 */
FVECT vres,
const FVECT v1,
const FVECT v2
)
{
        VCROSS(vres, v1, v2);
}


void
fvsum(                          /* vres = v0 + f*v1 */
FVECT vres,
const FVECT v0,
const FVECT v1,
double f
)
{
        VSUM(vres, v0, v1, f);
}


double
normalize(                      /* normalize a vector, return old magnitude */
FVECT  v
)
{
        double  len, d;
        
        d = DOT(v, v);
        
        if (d == 0.0)
                return(0.0);
        
        if ((d <= 1.0+FTINY) & (d >= 1.0-FTINY)) {
                len = 0.5 + 0.5*d;      /* first order approximation */
                d = 2.0 - len;
        } else {
                len = sqrt(d);
                d = 1.0/len;
        }
        v[0] *= d;
        v[1] *= d;
        v[2] *= d;

        return(len);
}


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;
        VCROSS(vp, v1, v);
        return(normalize(vp) > 0.0);
}

int
closestapproach(                        /* closest approach of two rays */
RREAL t[2],             /* returned distances along each ray */
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  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);
}


void
spinvector(                             /* rotate vector around normal */
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;
        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);
        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 */
}
Revision:	2.20
Committed:	Thu Dec 4 05:26:27 2014 UTC (10 years, 11 months ago) by greg
Content type:	text/plain
Branch:	MAIN
Changes since 2.19:	+24 -1 lines
Log Message:	Improved behavior of anisotropic reflections
#	User	Rev	Content
1	greg	1.1	#ifndef lint
2	greg	2.20	static const char RCSid[] = "$Id: fvect.c,v 2.19 2013/06/29 21:03:44 greg Exp $";
3	greg	1.1	#endif
4	greg	2.6	/*
5			* fvect.c - routines for floating-point vector calculations
6			*/
7	greg	1.1
8	greg	2.7	#include "copyright.h"
9	greg	1.1
10	greg	2.19	#define _USE_MATH_DEFINES
11	greg	2.2	#include <math.h>
12	greg	1.1	#include "fvect.h"
13	greg	2.20	#include "random.h"
14	greg	1.1
15	greg	2.19	double
16			Acos(double x) /* insurance for touchy math library */
17			{
18			if (x <= -1.+FTINY*FTINY)
19			return(M_PI);
20			if (x >= 1.-FTINY*FTINY)
21			return(.0);
22			return(acos(x));
23			}
24
25			double
26			Asin(double x) /* insurance for touchy math library */
27			{
28			if (x <= -1.+FTINY*FTINY)
29			return(-M_PI/2.);
30			if (x >= 1.-FTINY*FTINY)
31			return(M_PI/2);
32			return(asin(x));
33			}
34	greg	1.1
35			double
36	greg	2.8	fdot( /* return the dot product of two vectors */
37	greg	2.13	const FVECT v1,
38			const FVECT v2
39	greg	2.8	)
40	greg	1.1	{
41			return(DOT(v1,v2));
42			}
43
44
45			double
46	greg	2.8	dist2( /* return square of distance between points */
47	greg	2.13	const FVECT p1,
48			const FVECT p2
49	greg	2.8	)
50	greg	1.1	{
51	gwlarson	2.4	FVECT delta;
52	greg	1.1
53	greg	2.18	VSUB(delta, p2, p1);
54	gwlarson	2.5
55	greg	1.1	return(DOT(delta, delta));
56			}
57
58
59			double
60	greg	2.8	dist2line( /* return square of distance to line */
61	greg	2.13	const FVECT p, /* the point */
62			const FVECT ep1,
63			const FVECT ep2 /* points on the line */
64	greg	2.8	)
65	greg	1.1	{
66	greg	2.11	double d, d1, d2;
67	greg	1.1
68			d = dist2(ep1, ep2);
69			d1 = dist2(ep1, p);
70	gwlarson	2.5	d2 = d + d1 - dist2(ep2, p);
71	greg	1.1
72	gwlarson	2.5	return(d1 - 0.25d2d2/d);
73	greg	1.1	}
74
75
76			double
77	greg	2.8	dist2lseg( /* return square of distance to line segment */
78	greg	2.13	const FVECT p, /* the point */
79			const FVECT ep1,
80			const FVECT ep2 /* the end points */
81	greg	2.8	)
82	greg	1.1	{
83	greg	2.11	double d, d1, d2;
84	greg	1.1
85			d = dist2(ep1, ep2);
86			d1 = dist2(ep1, p);
87			d2 = dist2(ep2, p);
88
89			if (d2 > d1) { /* check if past endpoints */
90			if (d2 - d1 > d)
91			return(d1);
92			} else {
93			if (d1 - d2 > d)
94			return(d2);
95			}
96	gwlarson	2.5	d2 = d + d1 - d2;
97	greg	1.1
98	gwlarson	2.5	return(d1 - 0.25d2d2/d); /* distance to line */
99	greg	1.1	}
100
101
102	greg	2.6	void
103	greg	2.8	fcross( /* vres = v1 X v2 */
104	greg	2.11	FVECT vres,
105	greg	2.13	const FVECT v1,
106			const FVECT v2
107	greg	2.8	)
108	greg	1.1	{
109	greg	2.18	VCROSS(vres, v1, v2);
110	greg	1.1	}
111
112
113	greg	2.6	void
114	greg	2.8	fvsum( /* vres = v0 + fv1 /
115	greg	2.11	FVECT vres,
116	greg	2.13	const FVECT v0,
117			const FVECT v1,
118	greg	2.11	double f
119	greg	2.8	)
120	greg	1.4	{
121	greg	2.18	VSUM(vres, v0, v1, f);
122	greg	1.4	}
123
124
125	greg	1.1	double
126	greg	2.8	normalize( /* normalize a vector, return old magnitude */
127	greg	2.11	FVECT v
128	greg	2.8	)
129	greg	1.1	{
130	greg	2.11	double len, d;
131	greg	1.1
132	gwlarson	2.5	d = DOT(v, v);
133	greg	1.1
134	greg	2.10	if (d == 0.0)
135	greg	1.1	return(0.0);
136
137	greg	2.15	if ((d <= 1.0+FTINY) & (d >= 1.0-FTINY)) {
138	gwlarson	2.5	len = 0.5 + 0.5d; / first order approximation */
139	greg	2.12	d = 2.0 - len;
140			} else {
141	gwlarson	2.5	len = sqrt(d);
142	greg	2.12	d = 1.0/len;
143			}
144			v[0] *= d;
145	gwlarson	2.5	v[1] *= d;
146			v[2] *= d;
147	greg	2.3
148	greg	1.1	return(len);
149			}
150	greg	1.5
151
152	greg	2.8	int
153	greg	2.20	getperpendicular( /* choose random perpedicular direction */
154			FVECT vp, /* returns normalized */
155			const FVECT v /* input vector must be normalized */
156			)
157			{
158			FVECT v1;
159			int i;
160			/* randomize other coordinates */
161			v1[0] = 0.5 - frandom();
162			v1[1] = 0.5 - frandom();
163			v1[2] = 0.5 - frandom();
164			for (i = 3; i--; )
165			if ((-0.6 < v[i]) & (v[i] < 0.6))
166			break;
167			if (i < 0)
168			return(0);
169			v1[i] = 1.0;
170			VCROSS(vp, v1, v);
171			return(normalize(vp) > 0.0);
172			}
173
174			int
175	greg	2.8	closestapproach( /* closest approach of two rays */
176			RREAL t[2], /* returned distances along each ray */
177	greg	2.13	const FVECT rorg0, /* first origin */
178			const FVECT rdir0, /* first direction (normalized) */
179			const FVECT rorg1, /* second origin */
180			const FVECT rdir1 /* second direction (normalized) */
181	greg	2.8	)
182			{
183			double dotprod = DOT(rdir0, rdir1);
184			double denom = 1. - dotprod*dotprod;
185			double o1o2_d1;
186			FVECT o0o1;
187
188			if (denom <= FTINY) { /* check if lines are parallel */
189			t[0] = t[1] = 0.0;
190			return(0);
191			}
192			VSUB(o0o1, rorg0, rorg1);
193			o1o2_d1 = DOT(o0o1, rdir1);
194			t[0] = (o1o2_d1*dotprod - DOT(o0o1,rdir0)) / denom;
195			t[1] = o1o2_d1 + t[0]*dotprod;
196			return(1);
197			}
198
199
200	greg	2.6	void
201	greg	2.8	spinvector( /* rotate vector around normal */
202	greg	2.15	FVECT vres, /* returned vector (same magnitude as vorig) */
203	greg	2.13	const FVECT vorig, /* original vector */
204			const FVECT vnorm, /* normalized vector for rotation */
205	greg	2.14	double theta /* right-hand radians */
206	greg	2.8	)
207	greg	1.5	{
208	greg	1.6	double sint, cost, normprod;
209	greg	1.5	FVECT vperp;
210	greg	2.11	int i;
211	greg	1.5
212			if (theta == 0.0) {
213	greg	1.6	if (vres != vorig)
214			VCOPY(vres, vorig);
215	greg	1.5	return;
216			}
217	greg	1.6	cost = cos(theta);
218	greg	1.5	sint = sin(theta);
219	greg	1.6	normprod = DOT(vorig, vnorm)*(1.-cost);
220	greg	2.18	VCROSS(vperp, vnorm, vorig);
221	greg	1.5	for (i = 0; i < 3; i++)
222	greg	1.6	vres[i] = vorig[i]cost + vnorm[i]normprod + vperp[i]*sint;
223	greg	1.5	}
224	greg	2.15
225			double
226			geodesic( /* rotate vector on great circle towards target */
227			FVECT vres, /* returned vector (same magnitude as vorig) */
228			const FVECT vorig, /* original vector */
229			const FVECT vtarg, /* vector we are rotating towards */
230			double t, /* amount along arc directed towards vtarg */
231			int meas /* distance measure (radians, absolute, relative) */
232			)
233			{
234			FVECT normtarg;
235	greg	2.17	double volen, dotprod, sintr, cost;
236	greg	2.15	int i;
237
238	greg	2.16	VCOPY(normtarg, vtarg); /* in case vtarg==vres */
239	greg	2.15	if (vres != vorig)
240			VCOPY(vres, vorig);
241			if (t == 0.0)
242			return(VLEN(vres)); /* no rotation requested */
243			if ((volen = normalize(vres)) == 0.0)
244			return(0.0);
245			if (normalize(normtarg) == 0.0)
246			return(0.0); /* target vector is zero */
247			dotprod = DOT(vres, normtarg);
248			/* check for colinear */
249			if (dotprod >= 1.0-FTINY*FTINY) {
250			if (meas != GEOD_REL)
251			return(0.0);
252			vres[0] = volen; vres[1] = volen; vres[2] *= volen;
253			return(volen);
254			}
255			if (dotprod <= -1.0+FTINY*FTINY)
256			return(0.0);
257			if (meas == GEOD_ABS)
258			t /= volen;
259			else if (meas == GEOD_REL)
260			t *= acos(dotprod);
261			cost = cos(t);
262	greg	2.17	sintr = sin(t) / sqrt(1. - dotprod*dotprod);
263	greg	2.15	for (i = 0; i < 3; i++)
264			vres[i] = volen( costvres[i] +
265	greg	2.17	sintr(normtarg[i] - dotprodvres[i]) );
266	greg	2.15
267			return(volen); /* return vector length */
268			}