src/common/fvect.c

/* Copyright (c) 1998 Silicon Graphics, Inc. */

#ifndef lint
static char SCCSid[] = "$SunId$ SGI";
#endif

/*
 *  fvect.c - routines for float vector calculations
 *
 *     8/14/85
 */

#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 */
}


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


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


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.5
Committed:	Wed Nov 25 16:56:33 1998 UTC (25 years, 5 months ago) by gwlarson
Content type:	text/plain
Branch:	MAIN
Changes since 2.4:	+16 -14 lines
Log Message:	modest efficiency improvements
#	User	Rev	Content
1	gwlarson	2.4	/* Copyright (c) 1998 Silicon Graphics, Inc. */
2	greg	1.1
3			#ifndef lint
4	gwlarson	2.4	static char SCCSid[] = "$SunId$ SGI";
5	greg	1.1	#endif
6
7			/*
8			* fvect.c - routines for float vector calculations
9			*
10			* 8/14/85
11			*/
12
13	greg	2.2	#include <math.h>
14	greg	1.1	#include "fvect.h"
15
16
17			double
18			fdot(v1, v2) /* return the dot product of two vectors */
19			register FVECT v1, v2;
20			{
21			return(DOT(v1,v2));
22			}
23
24
25			double
26			dist2(p1, p2) /* return square of distance between points */
27			register FVECT p1, p2;
28			{
29	gwlarson	2.4	FVECT delta;
30	greg	1.1
31			delta[0] = p2[0] - p1[0];
32			delta[1] = p2[1] - p1[1];
33			delta[2] = p2[2] - p1[2];
34	gwlarson	2.5
35	greg	1.1	return(DOT(delta, delta));
36			}
37
38
39			double
40			dist2line(p, ep1, ep2) /* return square of distance to line */
41			FVECT p; /* the point */
42			FVECT ep1, ep2; /* points on the line */
43			{
44	gwlarson	2.4	register double d, d1, d2;
45	greg	1.1
46			d = dist2(ep1, ep2);
47			d1 = dist2(ep1, p);
48	gwlarson	2.5	d2 = d + d1 - dist2(ep2, p);
49	greg	1.1
50	gwlarson	2.5	return(d1 - 0.25d2d2/d);
51	greg	1.1	}
52
53
54			double
55			dist2lseg(p, ep1, ep2) /* return square of distance to line segment */
56			FVECT p; /* the point */
57			FVECT ep1, ep2; /* the end points */
58			{
59	gwlarson	2.4	register double d, d1, d2;
60	greg	1.1
61			d = dist2(ep1, ep2);
62			d1 = dist2(ep1, p);
63			d2 = dist2(ep2, p);
64
65			if (d2 > d1) { /* check if past endpoints */
66			if (d2 - d1 > d)
67			return(d1);
68			} else {
69			if (d1 - d2 > d)
70			return(d2);
71			}
72	gwlarson	2.5	d2 = d + d1 - d2;
73	greg	1.1
74	gwlarson	2.5	return(d1 - 0.25d2d2/d); /* distance to line */
75	greg	1.1	}
76
77
78			fcross(vres, v1, v2) /* vres = v1 X v2 */
79			register FVECT vres, v1, v2;
80			{
81			vres[0] = v1[1]v2[2] - v1[2]v2[1];
82			vres[1] = v1[2]v2[0] - v1[0]v2[2];
83			vres[2] = v1[0]v2[1] - v1[1]v2[0];
84			}
85
86
87	greg	1.4	fvsum(vres, v0, v1, f) /* vres = v0 + fv1 /
88	gwlarson	2.5	register FVECT vres, v0, v1;
89			register double f;
90	greg	1.4	{
91			vres[0] = v0[0] + f*v1[0];
92			vres[1] = v0[1] + f*v1[1];
93			vres[2] = v0[2] + f*v1[2];
94			}
95
96
97	greg	1.1	double
98			normalize(v) /* normalize a vector, return old magnitude */
99			register FVECT v;
100			{
101	gwlarson	2.5	register double len, d;
102	greg	1.1
103	gwlarson	2.5	d = DOT(v, v);
104	greg	1.1
105	gwlarson	2.5	if (d <= 0.0)
106	greg	1.1	return(0.0);
107
108	gwlarson	2.5	if (d <= 1.0+FTINY && d >= 1.0-FTINY)
109			len = 0.5 + 0.5d; / first order approximation */
110	greg	2.3	else
111	gwlarson	2.5	len = sqrt(d);
112	greg	1.1
113	gwlarson	2.5	v[0] *= d = 1.0/len;
114			v[1] *= d;
115			v[2] *= d;
116	greg	2.3
117	greg	1.1	return(len);
118			}
119	greg	1.5
120
121			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	}