src/common/fvect.c

#ifndef lint
static const char       RCSid[] = "$Id: fvect.c,v 2.23 2015/05/21 07:02:23 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
)
{
        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,
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 perpedicular direction */
FVECT vp,                               /* returns normalized */
const FVECT v,                          /* input vector must be normalized */
int randomize                           /* randomize orientation */
)
{
        int     ord[3];
        FVECT   v1;
        int     i;

        if (randomize) {                /* randomize coordinates? */
                v1[0] = 0.5 - frandom();
                v1[1] = 0.5 - frandom();
                v1[2] = 0.5 - frandom();
                switch ((int)(frandom()*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;
        }

        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);
}


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.24
Committed:	Thu Jul 23 18:22:26 2015 UTC (10 years, 3 months ago) by greg
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rad5R3, rad5R2, rad5R1, rad5R0
Changes since 2.23:	+3 -3 lines
Log Message:	Made it so we'll never generate another segfault from this code
#	Content
1	#ifndef lint
2	static const char RCSid[] = "$Id: fvect.c,v 2.23 2015/05/21 07:02:23 greg Exp $";
3	#endif
4	/*
5	* fvect.c - routines for floating-point vector calculations
6	*/
7
8	#include "copyright.h"
9
10	#define _USE_MATH_DEFINES
11	#include <math.h>
12	#include "fvect.h"
13	#include "random.h"
14
15	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
35	double
36	fdot( /* return the dot product of two vectors */
37	const FVECT v1,
38	const FVECT v2
39	)
40	{
41	return(DOT(v1,v2));
42	}
43
44
45	double
46	dist2( /* return square of distance between points */
47	const FVECT p1,
48	const FVECT p2
49	)
50	{
51	FVECT delta;
52
53	VSUB(delta, p2, p1);
54
55	return(DOT(delta, delta));
56	}
57
58
59	double
60	dist2line( /* return square of distance to line */
61	const FVECT p, /* the point */
62	const FVECT ep1,
63	const FVECT ep2 /* points on the line */
64	)
65	{
66	double d, d1, d2;
67
68	d = dist2(ep1, ep2);
69	d1 = dist2(ep1, p);
70	d2 = d + d1 - dist2(ep2, p);
71
72	return(d1 - 0.25d2d2/d);
73	}
74
75
76	double
77	dist2lseg( /* return square of distance to line segment */
78	const FVECT p, /* the point */
79	const FVECT ep1,
80	const FVECT ep2 /* the end points */
81	)
82	{
83	double d, d1, d2;
84
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	d2 = d + d1 - d2;
97
98	return(d1 - 0.25d2d2/d); /* distance to line */
99	}
100
101
102	void
103	fcross( /* vres = v1 X v2 */
104	FVECT vres,
105	const FVECT v1,
106	const FVECT v2
107	)
108	{
109	if ((vres == v1) \| (vres == v2)) {
110	FVECT vtmp;
111	VCROSS(vtmp, v1, v2);
112	VCOPY(vres, vtmp);
113	return;
114	}
115	VCROSS(vres, v1, v2);
116	}
117
118
119	void
120	fvsum( /* vres = v0 + fv1 /
121	FVECT vres,
122	const FVECT v0,
123	const FVECT v1,
124	double f
125	)
126	{
127	VSUM(vres, v0, v1, f);
128	}
129
130
131	double
132	normalize( /* normalize a vector, return old magnitude */
133	FVECT v
134	)
135	{
136	double len, d;
137
138	d = DOT(v, v);
139
140	if (d == 0.0)
141	return(0.0);
142
143	if ((d <= 1.0+FTINY) & (d >= 1.0-FTINY)) {
144	len = 0.5 + 0.5d; / first order approximation */
145	d = 2.0 - len;
146	} else {
147	len = sqrt(d);
148	d = 1.0/len;
149	}
150	v[0] *= d;
151	v[1] *= d;
152	v[2] *= d;
153
154	return(len);
155	}
156
157
158	int
159	getperpendicular( /* choose perpedicular direction */
160	FVECT vp, /* returns normalized */
161	const FVECT v, /* input vector must be normalized */
162	int randomize /* randomize orientation */
163	)
164	{
165	int ord[3];
166	FVECT v1;
167	int i;
168
169	if (randomize) { /* randomize coordinates? */
170	v1[0] = 0.5 - frandom();
171	v1[1] = 0.5 - frandom();
172	v1[2] = 0.5 - frandom();
173	switch ((int)(frandom()*6.)) {
174	case 0: ord[0] = 0; ord[1] = 1; ord[2] = 2; break;
175	case 1: ord[0] = 0; ord[1] = 2; ord[2] = 1; break;
176	case 2: ord[0] = 1; ord[1] = 0; ord[2] = 2; break;
177	case 3: ord[0] = 1; ord[1] = 2; ord[2] = 0; break;
178	case 4: ord[0] = 2; ord[1] = 0; ord[2] = 1; break;
179	default: ord[0] = 2; ord[1] = 1; ord[2] = 0; break;
180	}
181	} else {
182	v1[0] = v1[1] = v1[2] = 0.0;
183	ord[0] = 0; ord[1] = 1; ord[2] = 2;
184	}
185
186	for (i = 3; i--; )
187	if ((-0.6 < v[ord[i]]) & (v[ord[i]] < 0.6))
188	break;
189	if (i < 0)
190	return(0);
191
192	v1[ord[i]] = 1.0;
193	fcross(vp, v1, v);
194
195	return(normalize(vp) > 0.0);
196	}
197
198
199	int
200	closestapproach( /* closest approach of two rays */
201	RREAL t[2], /* returned distances along each ray */
202	const FVECT rorg0, /* first origin */
203	const FVECT rdir0, /* first direction (normalized) */
204	const FVECT rorg1, /* second origin */
205	const FVECT rdir1 /* second direction (normalized) */
206	)
207	{
208	double dotprod = DOT(rdir0, rdir1);
209	double denom = 1. - dotprod*dotprod;
210	double o1o2_d1;
211	FVECT o0o1;
212
213	if (denom <= FTINY) { /* check if lines are parallel */
214	t[0] = t[1] = 0.0;
215	return(0);
216	}
217	VSUB(o0o1, rorg0, rorg1);
218	o1o2_d1 = DOT(o0o1, rdir1);
219	t[0] = (o1o2_d1*dotprod - DOT(o0o1,rdir0)) / denom;
220	t[1] = o1o2_d1 + t[0]*dotprod;
221	return(1);
222	}
223
224
225	void
226	spinvector( /* rotate vector around normal */
227	FVECT vres, /* returned vector (same magnitude as vorig) */
228	const FVECT vorig, /* original vector */
229	const FVECT vnorm, /* normalized vector for rotation */
230	double theta /* right-hand radians */
231	)
232	{
233	double sint, cost, normprod;
234	FVECT vperp;
235	int i;
236
237	if (theta == 0.0) {
238	if (vres != vorig)
239	VCOPY(vres, vorig);
240	return;
241	}
242	cost = cos(theta);
243	sint = sin(theta);
244	normprod = DOT(vorig, vnorm)*(1.-cost);
245	VCROSS(vperp, vnorm, vorig);
246	for (i = 0; i < 3; i++)
247	vres[i] = vorig[i]cost + vnorm[i]normprod + vperp[i]*sint;
248	}
249
250	double
251	geodesic( /* rotate vector on great circle towards target */
252	FVECT vres, /* returned vector (same magnitude as vorig) */
253	const FVECT vorig, /* original vector */
254	const FVECT vtarg, /* vector we are rotating towards */
255	double t, /* amount along arc directed towards vtarg */
256	int meas /* distance measure (radians, absolute, relative) */
257	)
258	{
259	FVECT normtarg;
260	double volen, dotprod, sintr, cost;
261	int i;
262
263	VCOPY(normtarg, vtarg); /* in case vtarg==vres */
264	if (vres != vorig)
265	VCOPY(vres, vorig);
266	if (t == 0.0)
267	return(VLEN(vres)); /* no rotation requested */
268	if ((volen = normalize(vres)) == 0.0)
269	return(0.0);
270	if (normalize(normtarg) == 0.0)
271	return(0.0); /* target vector is zero */
272	dotprod = DOT(vres, normtarg);
273	/* check for colinear */
274	if (dotprod >= 1.0-FTINY*FTINY) {
275	if (meas != GEOD_REL)
276	return(0.0);
277	vres[0] = volen; vres[1] = volen; vres[2] *= volen;
278	return(volen);
279	}
280	if (dotprod <= -1.0+FTINY*FTINY)
281	return(0.0);
282	if (meas == GEOD_ABS)
283	t /= volen;
284	else if (meas == GEOD_REL)
285	t *= acos(dotprod);
286	cost = cos(t);
287	sintr = sin(t) / sqrt(1. - dotprod*dotprod);
288	for (i = 0; i < 3; i++)
289	vres[i] = volen( costvres[i] +
290	sintr(normtarg[i] - dotprodvres[i]) );
291
292	return(volen); /* return vector length */
293	}