src/common/fvect.c

#ifndef lint
static const char       RCSid[] = "$Id: fvect.c,v 2.7 2003/02/25 02:47:21 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 */
register FVECT v1,
register FVECT v2
)
{
        return(DOT(v1,v2));
}


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

        delta[0] = p2[0] - p1[0];
        delta[1] = p2[1] - p1[1];
        delta[2] = p2[2] - p1[2];

        return(DOT(delta, delta));
}


double
dist2line(                      /* return square of distance to line */
FVECT p,                /* the point */
FVECT ep1,
FVECT ep2               /* points on the line */
)
{
        register 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 */
FVECT p,                /* the point */
FVECT ep1,
FVECT ep2               /* the end points */
)
{
        register 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 */
register FVECT vres,
register FVECT v1,
register 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];
}


void
fvsum(                          /* vres = v0 + f*v1 */
register FVECT vres,
register FVECT v0,
register FVECT v1,
register double f
)
{
        vres[0] = v0[0] + f*v1[0];
        vres[1] = v0[1] + f*v1[1];
        vres[2] = v0[2] + f*v1[2];
}


double
normalize(                      /* normalize a vector, return old magnitude */
register FVECT  v
)
{
        register 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 */
        else
                len = sqrt(d);

        v[0] *= d = 1.0/len;
        v[1] *= d;
        v[2] *= d;

        return(len);
}


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) */
)
{
        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);
}


#if 0
int
closestapproach(                        /* closest approach of two rays */
RREAL t[2],             /* returned distances along each ray */
FVECT rorg0,            /* first origin */
FVECT rdir0,            /* first direction (unnormalized) */
FVECT rorg1,            /* second origin */
FVECT rdir1             /* second direction (unnormalized) */
)
{
        double  dotprod = DOT(rdir0, rdir1);
        double  d0n2 = DOT(rdir0, rdir0);
        double  d1n2 = DOT(rdir1, rdir1);
        double  denom = d0n2*d1n2 - 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)*d1n2) / denom;
        t[1] = (o1o2_d1 + t[0]*dotprod) / d1n2;
        return(1);
}
#endif


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 */
)
{
        double  sint, cost, normprod;
        FVECT  vperp;
        register 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);
        fcross(vperp, vnorm, vorig);
        for (i = 0; i < 3; i++)
                vres[i] = vorig[i]*cost + vnorm[i]*normprod + vperp[i]*sint;
}
Revision:	2.8
Committed:	Tue Sep 16 06:30:20 2003 UTC (22 years, 1 month ago) by greg
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rad3R6, rad3R6P1
Changes since 2.7:	+95 -21 lines
Log Message:	Wrote closestapproach() function to find closest point for two rays
#	User	Rev	Content
1	greg	1.1	#ifndef lint
2	greg	2.8	static const char RCSid[] = "$Id: fvect.c,v 2.7 2003/02/25 02:47:21 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.2	#include <math.h>
11	greg	1.1	#include "fvect.h"
12
13
14			double
15	greg	2.8	fdot( /* return the dot product of two vectors */
16			register FVECT v1,
17			register FVECT v2
18			)
19	greg	1.1	{
20			return(DOT(v1,v2));
21			}
22
23
24			double
25	greg	2.8	dist2( /* return square of distance between points */
26			register FVECT p1,
27			register FVECT p2
28			)
29	greg	1.1	{
30	gwlarson	2.4	FVECT delta;
31	greg	1.1
32			delta[0] = p2[0] - p1[0];
33			delta[1] = p2[1] - p1[1];
34			delta[2] = p2[2] - p1[2];
35	gwlarson	2.5
36	greg	1.1	return(DOT(delta, delta));
37			}
38
39
40			double
41	greg	2.8	dist2line( /* return square of distance to line */
42			FVECT p, /* the point */
43			FVECT ep1,
44			FVECT ep2 /* points on the line */
45			)
46	greg	1.1	{
47	gwlarson	2.4	register double d, d1, d2;
48	greg	1.1
49			d = dist2(ep1, ep2);
50			d1 = dist2(ep1, p);
51	gwlarson	2.5	d2 = d + d1 - dist2(ep2, p);
52	greg	1.1
53	gwlarson	2.5	return(d1 - 0.25d2d2/d);
54	greg	1.1	}
55
56
57			double
58	greg	2.8	dist2lseg( /* return square of distance to line segment */
59			FVECT p, /* the point */
60			FVECT ep1,
61			FVECT ep2 /* the end points */
62			)
63	greg	1.1	{
64	gwlarson	2.4	register double d, d1, d2;
65	greg	1.1
66			d = dist2(ep1, ep2);
67			d1 = dist2(ep1, p);
68			d2 = dist2(ep2, p);
69
70			if (d2 > d1) { /* check if past endpoints */
71			if (d2 - d1 > d)
72			return(d1);
73			} else {
74			if (d1 - d2 > d)
75			return(d2);
76			}
77	gwlarson	2.5	d2 = d + d1 - d2;
78	greg	1.1
79	gwlarson	2.5	return(d1 - 0.25d2d2/d); /* distance to line */
80	greg	1.1	}
81
82
83	greg	2.6	void
84	greg	2.8	fcross( /* vres = v1 X v2 */
85			register FVECT vres,
86			register FVECT v1,
87			register FVECT v2
88			)
89	greg	1.1	{
90			vres[0] = v1[1]v2[2] - v1[2]v2[1];
91			vres[1] = v1[2]v2[0] - v1[0]v2[2];
92			vres[2] = v1[0]v2[1] - v1[1]v2[0];
93			}
94
95
96	greg	2.6	void
97	greg	2.8	fvsum( /* vres = v0 + fv1 /
98			register FVECT vres,
99			register FVECT v0,
100			register FVECT v1,
101			register double f
102			)
103	greg	1.4	{
104			vres[0] = v0[0] + f*v1[0];
105			vres[1] = v0[1] + f*v1[1];
106			vres[2] = v0[2] + f*v1[2];
107			}
108
109
110	greg	1.1	double
111	greg	2.8	normalize( /* normalize a vector, return old magnitude */
112			register FVECT v
113			)
114	greg	1.1	{
115	gwlarson	2.5	register double len, d;
116	greg	1.1
117	gwlarson	2.5	d = DOT(v, v);
118	greg	1.1
119	gwlarson	2.5	if (d <= 0.0)
120	greg	1.1	return(0.0);
121
122	gwlarson	2.5	if (d <= 1.0+FTINY && d >= 1.0-FTINY)
123			len = 0.5 + 0.5d; / first order approximation */
124	greg	2.3	else
125	gwlarson	2.5	len = sqrt(d);
126	greg	1.1
127	gwlarson	2.5	v[0] *= d = 1.0/len;
128			v[1] *= d;
129			v[2] *= d;
130	greg	2.3
131	greg	1.1	return(len);
132			}
133	greg	1.5
134
135	greg	2.8	int
136			closestapproach( /* closest approach of two rays */
137			RREAL t[2], /* returned distances along each ray */
138			FVECT rorg0, /* first origin */
139			FVECT rdir0, /* first direction (normalized) */
140			FVECT rorg1, /* second origin */
141			FVECT rdir1 /* second direction (normalized) */
142			)
143			{
144			double dotprod = DOT(rdir0, rdir1);
145			double denom = 1. - dotprod*dotprod;
146			double o1o2_d1;
147			FVECT o0o1;
148
149			if (denom <= FTINY) { /* check if lines are parallel */
150			t[0] = t[1] = 0.0;
151			return(0);
152			}
153			VSUB(o0o1, rorg0, rorg1);
154			o1o2_d1 = DOT(o0o1, rdir1);
155			t[0] = (o1o2_d1*dotprod - DOT(o0o1,rdir0)) / denom;
156			t[1] = o1o2_d1 + t[0]*dotprod;
157			return(1);
158			}
159
160
161			#if 0
162			int
163			closestapproach( /* closest approach of two rays */
164			RREAL t[2], /* returned distances along each ray */
165			FVECT rorg0, /* first origin */
166			FVECT rdir0, /* first direction (unnormalized) */
167			FVECT rorg1, /* second origin */
168			FVECT rdir1 /* second direction (unnormalized) */
169			)
170			{
171			double dotprod = DOT(rdir0, rdir1);
172			double d0n2 = DOT(rdir0, rdir0);
173			double d1n2 = DOT(rdir1, rdir1);
174			double denom = d0n2d1n2 - dotproddotprod;
175			double o1o2_d1;
176			FVECT o0o1;
177
178			if (denom <= FTINY) { /* check if lines are parallel */
179			t[0] = t[1] = 0.0;
180			return(0);
181			}
182			VSUB(o0o1, rorg0, rorg1);
183			o1o2_d1 = DOT(o0o1, rdir1);
184			t[0] = (o1o2_d1dotprod - DOT(o0o1,rdir0)d1n2) / denom;
185			t[1] = (o1o2_d1 + t[0]*dotprod) / d1n2;
186			return(1);
187			}
188			#endif
189
190
191	greg	2.6	void
192	greg	2.8	spinvector( /* rotate vector around normal */
193			FVECT vres, /* returned vector */
194			FVECT vorig, /* original vector */
195			FVECT vnorm, /* normalized vector for rotation */
196			double theta /* left-hand radians */
197			)
198	greg	1.5	{
199	greg	1.6	double sint, cost, normprod;
200	greg	1.5	FVECT vperp;
201			register int i;
202
203			if (theta == 0.0) {
204	greg	1.6	if (vres != vorig)
205			VCOPY(vres, vorig);
206	greg	1.5	return;
207			}
208	greg	1.6	cost = cos(theta);
209	greg	1.5	sint = sin(theta);
210	greg	1.6	normprod = DOT(vorig, vnorm)*(1.-cost);
211	greg	1.5	fcross(vperp, vnorm, vorig);
212			for (i = 0; i < 3; i++)
213	greg	1.6	vres[i] = vorig[i]cost + vnorm[i]normprod + vperp[i]*sint;
214	greg	1.5	}