src/common/fvect.c

#ifndef lint
static const char       RCSid[] = "$Id: fvect.c,v 2.17 2012/11/22 06:07:17 greg Exp $";
#endif
/*
 *  fvect.c - routines for floating-point vector calculations
 */

#include "copyright.h"

#include  <math.h>
#include  "fvect.h"


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
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.18
Committed:	Wed Apr 3 00:22:12 2013 UTC (11 years, 1 month ago) by greg
Content type:	text/plain
Branch:	MAIN
Changes since 2.17:	+5 -11 lines
Log Message:	Added more use of macros
#	Content
1	#ifndef lint
2	static const char RCSid[] = "$Id: fvect.c,v 2.17 2012/11/22 06:07:17 greg Exp $";
3	#endif
4	/*
5	* fvect.c - routines for floating-point vector calculations
6	*/
7
8	#include "copyright.h"
9
10	#include <math.h>
11	#include "fvect.h"
12
13
14	double
15	fdot( /* return the dot product of two vectors */
16	const FVECT v1,
17	const FVECT v2
18	)
19	{
20	return(DOT(v1,v2));
21	}
22
23
24	double
25	dist2( /* return square of distance between points */
26	const FVECT p1,
27	const FVECT p2
28	)
29	{
30	FVECT delta;
31
32	VSUB(delta, p2, p1);
33
34	return(DOT(delta, delta));
35	}
36
37
38	double
39	dist2line( /* return square of distance to line */
40	const FVECT p, /* the point */
41	const FVECT ep1,
42	const FVECT ep2 /* points on the line */
43	)
44	{
45	double d, d1, d2;
46
47	d = dist2(ep1, ep2);
48	d1 = dist2(ep1, p);
49	d2 = d + d1 - dist2(ep2, p);
50
51	return(d1 - 0.25d2d2/d);
52	}
53
54
55	double
56	dist2lseg( /* return square of distance to line segment */
57	const FVECT p, /* the point */
58	const FVECT ep1,
59	const FVECT ep2 /* the end points */
60	)
61	{
62	double d, d1, d2;
63
64	d = dist2(ep1, ep2);
65	d1 = dist2(ep1, p);
66	d2 = dist2(ep2, p);
67
68	if (d2 > d1) { /* check if past endpoints */
69	if (d2 - d1 > d)
70	return(d1);
71	} else {
72	if (d1 - d2 > d)
73	return(d2);
74	}
75	d2 = d + d1 - d2;
76
77	return(d1 - 0.25d2d2/d); /* distance to line */
78	}
79
80
81	void
82	fcross( /* vres = v1 X v2 */
83	FVECT vres,
84	const FVECT v1,
85	const FVECT v2
86	)
87	{
88	VCROSS(vres, v1, v2);
89	}
90
91
92	void
93	fvsum( /* vres = v0 + fv1 /
94	FVECT vres,
95	const FVECT v0,
96	const FVECT v1,
97	double f
98	)
99	{
100	VSUM(vres, v0, v1, f);
101	}
102
103
104	double
105	normalize( /* normalize a vector, return old magnitude */
106	FVECT v
107	)
108	{
109	double len, d;
110
111	d = DOT(v, v);
112
113	if (d == 0.0)
114	return(0.0);
115
116	if ((d <= 1.0+FTINY) & (d >= 1.0-FTINY)) {
117	len = 0.5 + 0.5d; / first order approximation */
118	d = 2.0 - len;
119	} else {
120	len = sqrt(d);
121	d = 1.0/len;
122	}
123	v[0] *= d;
124	v[1] *= d;
125	v[2] *= d;
126
127	return(len);
128	}
129
130
131	int
132	closestapproach( /* closest approach of two rays */
133	RREAL t[2], /* returned distances along each ray */
134	const FVECT rorg0, /* first origin */
135	const FVECT rdir0, /* first direction (normalized) */
136	const FVECT rorg1, /* second origin */
137	const FVECT rdir1 /* second direction (normalized) */
138	)
139	{
140	double dotprod = DOT(rdir0, rdir1);
141	double denom = 1. - dotprod*dotprod;
142	double o1o2_d1;
143	FVECT o0o1;
144
145	if (denom <= FTINY) { /* check if lines are parallel */
146	t[0] = t[1] = 0.0;
147	return(0);
148	}
149	VSUB(o0o1, rorg0, rorg1);
150	o1o2_d1 = DOT(o0o1, rdir1);
151	t[0] = (o1o2_d1*dotprod - DOT(o0o1,rdir0)) / denom;
152	t[1] = o1o2_d1 + t[0]*dotprod;
153	return(1);
154	}
155
156
157	void
158	spinvector( /* rotate vector around normal */
159	FVECT vres, /* returned vector (same magnitude as vorig) */
160	const FVECT vorig, /* original vector */
161	const FVECT vnorm, /* normalized vector for rotation */
162	double theta /* right-hand radians */
163	)
164	{
165	double sint, cost, normprod;
166	FVECT vperp;
167	int i;
168
169	if (theta == 0.0) {
170	if (vres != vorig)
171	VCOPY(vres, vorig);
172	return;
173	}
174	cost = cos(theta);
175	sint = sin(theta);
176	normprod = DOT(vorig, vnorm)*(1.-cost);
177	VCROSS(vperp, vnorm, vorig);
178	for (i = 0; i < 3; i++)
179	vres[i] = vorig[i]cost + vnorm[i]normprod + vperp[i]*sint;
180	}
181
182	double
183	geodesic( /* rotate vector on great circle towards target */
184	FVECT vres, /* returned vector (same magnitude as vorig) */
185	const FVECT vorig, /* original vector */
186	const FVECT vtarg, /* vector we are rotating towards */
187	double t, /* amount along arc directed towards vtarg */
188	int meas /* distance measure (radians, absolute, relative) */
189	)
190	{
191	FVECT normtarg;
192	double volen, dotprod, sintr, cost;
193	int i;
194
195	VCOPY(normtarg, vtarg); /* in case vtarg==vres */
196	if (vres != vorig)
197	VCOPY(vres, vorig);
198	if (t == 0.0)
199	return(VLEN(vres)); /* no rotation requested */
200	if ((volen = normalize(vres)) == 0.0)
201	return(0.0);
202	if (normalize(normtarg) == 0.0)
203	return(0.0); /* target vector is zero */
204	dotprod = DOT(vres, normtarg);
205	/* check for colinear */
206	if (dotprod >= 1.0-FTINY*FTINY) {
207	if (meas != GEOD_REL)
208	return(0.0);
209	vres[0] = volen; vres[1] = volen; vres[2] *= volen;
210	return(volen);
211	}
212	if (dotprod <= -1.0+FTINY*FTINY)
213	return(0.0);
214	if (meas == GEOD_ABS)
215	t /= volen;
216	else if (meas == GEOD_REL)
217	t *= acos(dotprod);
218	cost = cos(t);
219	sintr = sin(t) / sqrt(1. - dotprod*dotprod);
220	for (i = 0; i < 3; i++)
221	vres[i] = volen( costvres[i] +
222	sintr(normtarg[i] - dotprodvres[i]) );
223
224	return(volen); /* return vector length */
225	}