src/common/fvect.c

#ifndef lint
static const char       RCSid[] = "$Id$";
#endif
/*
 *  fvect.c - routines for floating-point vector calculations
 */

#include "copyright.h"

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


double
fdot(v1, v2)                    /* return the dot product of two vectors */
register FVECT  v1, v2;
{
        return(DOT(v1,v2));
}


double
dist2(p1, p2)                   /* return square of distance between points */
register FVECT  p1, 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(p, ep1, ep2)          /* return square of distance to line */
FVECT  p;               /* the point */
FVECT  ep1, 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(p, ep1, ep2)          /* return square of distance to line segment */
FVECT  p;               /* the point */
FVECT  ep1, 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, v2)            /* vres = v1 X v2 */
register FVECT  vres, v1, 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, v1, f)          /* vres = v0 + f*v1 */
register FVECT  vres, v0, 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(v)                    /* 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);
}


void
spinvector(vres, vorig, vnorm, theta)   /* rotate vector around normal */
FVECT  vres, vorig, vnorm;
double  theta;
{
        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.7
Committed:	Tue Feb 25 02:47:21 2003 UTC (21 years, 2 months ago) by greg
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rad3R5
Changes since 2.6:	+1 -56 lines
Log Message:	Replaced inline copyright notice with #include "copyright.h"
#	User	Rev	Content
1	greg	1.1	#ifndef lint
2	greg	2.6	static const char RCSid[] = "$Id$";
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			fdot(v1, v2) /* return the dot product of two vectors */
16			register FVECT v1, v2;
17			{
18			return(DOT(v1,v2));
19			}
20
21
22			double
23			dist2(p1, p2) /* return square of distance between points */
24			register FVECT p1, p2;
25			{
26	gwlarson	2.4	FVECT delta;
27	greg	1.1
28			delta[0] = p2[0] - p1[0];
29			delta[1] = p2[1] - p1[1];
30			delta[2] = p2[2] - p1[2];
31	gwlarson	2.5
32	greg	1.1	return(DOT(delta, delta));
33			}
34
35
36			double
37			dist2line(p, ep1, ep2) /* return square of distance to line */
38			FVECT p; /* the point */
39			FVECT ep1, ep2; /* points on the line */
40			{
41	gwlarson	2.4	register double d, d1, d2;
42	greg	1.1
43			d = dist2(ep1, ep2);
44			d1 = dist2(ep1, p);
45	gwlarson	2.5	d2 = d + d1 - dist2(ep2, p);
46	greg	1.1
47	gwlarson	2.5	return(d1 - 0.25d2d2/d);
48	greg	1.1	}
49
50
51			double
52			dist2lseg(p, ep1, ep2) /* return square of distance to line segment */
53			FVECT p; /* the point */
54			FVECT ep1, ep2; /* the end points */
55			{
56	gwlarson	2.4	register double d, d1, d2;
57	greg	1.1
58			d = dist2(ep1, ep2);
59			d1 = dist2(ep1, p);
60			d2 = dist2(ep2, p);
61
62			if (d2 > d1) { /* check if past endpoints */
63			if (d2 - d1 > d)
64			return(d1);
65			} else {
66			if (d1 - d2 > d)
67			return(d2);
68			}
69	gwlarson	2.5	d2 = d + d1 - d2;
70	greg	1.1
71	gwlarson	2.5	return(d1 - 0.25d2d2/d); /* distance to line */
72	greg	1.1	}
73
74
75	greg	2.6	void
76	greg	1.1	fcross(vres, v1, v2) /* vres = v1 X v2 */
77			register FVECT vres, v1, v2;
78			{
79			vres[0] = v1[1]v2[2] - v1[2]v2[1];
80			vres[1] = v1[2]v2[0] - v1[0]v2[2];
81			vres[2] = v1[0]v2[1] - v1[1]v2[0];
82			}
83
84
85	greg	2.6	void
86	greg	1.4	fvsum(vres, v0, v1, f) /* vres = v0 + fv1 /
87	gwlarson	2.5	register FVECT vres, v0, v1;
88			register double f;
89	greg	1.4	{
90			vres[0] = v0[0] + f*v1[0];
91			vres[1] = v0[1] + f*v1[1];
92			vres[2] = v0[2] + f*v1[2];
93			}
94
95
96	greg	1.1	double
97			normalize(v) /* normalize a vector, return old magnitude */
98			register FVECT v;
99			{
100	gwlarson	2.5	register double len, d;
101	greg	1.1
102	gwlarson	2.5	d = DOT(v, v);
103	greg	1.1
104	gwlarson	2.5	if (d <= 0.0)
105	greg	1.1	return(0.0);
106
107	gwlarson	2.5	if (d <= 1.0+FTINY && d >= 1.0-FTINY)
108			len = 0.5 + 0.5d; / first order approximation */
109	greg	2.3	else
110	gwlarson	2.5	len = sqrt(d);
111	greg	1.1
112	gwlarson	2.5	v[0] *= d = 1.0/len;
113			v[1] *= d;
114			v[2] *= d;
115	greg	2.3
116	greg	1.1	return(len);
117			}
118	greg	1.5
119
120	greg	2.6	void
121	greg	1.5	spinvector(vres, vorig, vnorm, theta) /* rotate vector around normal */
122			FVECT vres, vorig, vnorm;
123			double theta;
124			{
125	greg	1.6	double sint, cost, normprod;
126	greg	1.5	FVECT vperp;
127			register int i;
128
129			if (theta == 0.0) {
130	greg	1.6	if (vres != vorig)
131			VCOPY(vres, vorig);
132	greg	1.5	return;
133			}
134	greg	1.6	cost = cos(theta);
135	greg	1.5	sint = sin(theta);
136	greg	1.6	normprod = DOT(vorig, vnorm)*(1.-cost);
137	greg	1.5	fcross(vperp, vnorm, vorig);
138			for (i = 0; i < 3; i++)
139	greg	1.6	vres[i] = vorig[i]cost + vnorm[i]normprod + vperp[i]*sint;
140	greg	1.5	}