src/common/fvect.c

/* Copyright (c) 1986 Regents of the University of California */

#ifndef lint
static char SCCSid[] = "$SunId$ LBL";
#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;
{
        static 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 */
{
        static double  d, d1, d2;

        d = dist2(ep1, ep2);
        d1 = dist2(ep1, p);
        d2 = dist2(ep2, p);

        return(d1 - (d+d1-d2)*(d+d1-d2)/d/4);
}


double
dist2lseg(p, ep1, ep2)          /* return square of distance to line segment */
FVECT  p;               /* the point */
FVECT  ep1, ep2;        /* the end points */
{
        static 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);
        }

        return(d1 - (d+d1-d2)*(d+d1-d2)/d/4);   /* 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 */
FVECT  vres, v0, v1;
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;
{
        static double  len;
        
        len = DOT(v, v);
        
        if (len <= 0.0)
                return(0.0);
        
        /****** problematic
        if (len >= (1.0-FTINY)*(1.0-FTINY) &&
                        len <= (1.0+FTINY)*(1.0+FTINY))
                return(1.0);
        ******/

        len = sqrt(len);
        v[0] /= len;
        v[1] /= len;
        v[2] /= len;
        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.2
Committed:	Fri Oct 2 16:02:43 1992 UTC (31 years, 7 months ago) by greg
Content type:	text/plain
Branch:	MAIN
Changes since 2.1:	+1 -1 lines
Log Message:	Removed problematic math function declarations
#	User	Rev	Content
1	greg	1.1	/* Copyright (c) 1986 Regents of the University of California */
2
3			#ifndef lint
4			static char SCCSid[] = "$SunId$ LBL";
5			#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			static FVECT delta;
30
31			delta[0] = p2[0] - p1[0];
32			delta[1] = p2[1] - p1[1];
33			delta[2] = p2[2] - p1[2];
34			return(DOT(delta, delta));
35			}
36
37
38			double
39			dist2line(p, ep1, ep2) /* return square of distance to line */
40			FVECT p; /* the point */
41			FVECT ep1, ep2; /* points on the line */
42			{
43			static double d, d1, d2;
44
45			d = dist2(ep1, ep2);
46			d1 = dist2(ep1, p);
47			d2 = dist2(ep2, p);
48
49			return(d1 - (d+d1-d2)*(d+d1-d2)/d/4);
50			}
51
52
53			double
54			dist2lseg(p, ep1, ep2) /* return square of distance to line segment */
55			FVECT p; /* the point */
56			FVECT ep1, ep2; /* the end points */
57			{
58			static double d, d1, d2;
59
60			d = dist2(ep1, ep2);
61			d1 = dist2(ep1, p);
62			d2 = dist2(ep2, p);
63
64			if (d2 > d1) { /* check if past endpoints */
65			if (d2 - d1 > d)
66			return(d1);
67			} else {
68			if (d1 - d2 > d)
69			return(d2);
70			}
71
72			return(d1 - (d+d1-d2)(d+d1-d2)/d/4); / distance to line */
73			}
74
75
76			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	1.4	fvsum(vres, v0, v1, f) /* vres = v0 + fv1 /
86			FVECT vres, v0, v1;
87			double f;
88			{
89			vres[0] = v0[0] + f*v1[0];
90			vres[1] = v0[1] + f*v1[1];
91			vres[2] = v0[2] + f*v1[2];
92			}
93
94
95	greg	1.1	double
96			normalize(v) /* normalize a vector, return old magnitude */
97			register FVECT v;
98			{
99			static double len;
100
101			len = DOT(v, v);
102
103	greg	1.3	if (len <= 0.0)
104	greg	1.1	return(0.0);
105
106	greg	1.2	/****** problematic
107	greg	1.1	if (len >= (1.0-FTINY)*(1.0-FTINY) &&
108			len <= (1.0+FTINY)*(1.0+FTINY))
109			return(1.0);
110	greg	1.2	******/
111	greg	1.1
112			len = sqrt(len);
113			v[0] /= len;
114			v[1] /= len;
115			v[2] /= len;
116			return(len);
117			}
118	greg	1.5
119
120			spinvector(vres, vorig, vnorm, theta) /* rotate vector around normal */
121			FVECT vres, vorig, vnorm;
122			double theta;
123			{
124	greg	1.6	double sint, cost, normprod;
125	greg	1.5	FVECT vperp;
126			register int i;
127
128			if (theta == 0.0) {
129	greg	1.6	if (vres != vorig)
130			VCOPY(vres, vorig);
131	greg	1.5	return;
132			}
133	greg	1.6	cost = cos(theta);
134	greg	1.5	sint = sin(theta);
135	greg	1.6	normprod = DOT(vorig, vnorm)*(1.-cost);
136	greg	1.5	fcross(vperp, vnorm, vorig);
137			for (i = 0; i < 3; i++)
138	greg	1.6	vres[i] = vorig[i]cost + vnorm[i]normprod + vperp[i]*sint;
139	greg	1.5	}