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  "fvect.h"

#define  FTINY          1e-7


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;
{
        extern double  sin(), cos();
        double  sint, cost, dotp;
        FVECT  vperp;
        register int  i;
        
        if (theta == 0.0) {
                VCOPY(vres, vorig);
                return;
        }
        sint = sin(theta);
        cost = cos(theta);
        dotp = DOT(vorig, vnorm);
        fcross(vperp, vnorm, vorig);
        for (i = 0; i < 3; i++)
                vres[i] = vnorm[i]*dotp*(1.-cost) +
                                vorig[i]*cost + vperp[i]*sint;
}
Revision:	1.5
Committed:	Tue Mar 19 13:14:19 1991 UTC (33 years, 1 month ago) by greg
Content type:	text/plain
Branch:	MAIN
Changes since 1.4:	+23 -0 lines
Log Message:	added spinvector()
#	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			#include "fvect.h"
14
15			#define FTINY 1e-7
16
17
18			double
19			fdot(v1, v2) /* return the dot product of two vectors */
20			register FVECT v1, v2;
21			{
22			return(DOT(v1,v2));
23			}
24
25
26			double
27			dist2(p1, p2) /* return square of distance between points */
28			register FVECT p1, p2;
29			{
30			static FVECT delta;
31
32			delta[0] = p2[0] - p1[0];
33			delta[1] = p2[1] - p1[1];
34			delta[2] = p2[2] - p1[2];
35			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			static double d, d1, d2;
45
46			d = dist2(ep1, ep2);
47			d1 = dist2(ep1, p);
48			d2 = dist2(ep2, p);
49
50			return(d1 - (d+d1-d2)*(d+d1-d2)/d/4);
51			}
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			static double d, d1, d2;
60
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
73			return(d1 - (d+d1-d2)(d+d1-d2)/d/4); / distance to line */
74			}
75
76
77			fcross(vres, v1, v2) /* vres = v1 X v2 */
78			register FVECT vres, v1, v2;
79			{
80			vres[0] = v1[1]v2[2] - v1[2]v2[1];
81			vres[1] = v1[2]v2[0] - v1[0]v2[2];
82			vres[2] = v1[0]v2[1] - v1[1]v2[0];
83			}
84
85
86	greg	1.4	fvsum(vres, v0, v1, f) /* vres = v0 + fv1 /
87			FVECT vres, v0, v1;
88			double f;
89			{
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			static double len;
101
102			len = DOT(v, v);
103
104	greg	1.3	if (len <= 0.0)
105	greg	1.1	return(0.0);
106
107	greg	1.2	/****** problematic
108	greg	1.1	if (len >= (1.0-FTINY)*(1.0-FTINY) &&
109			len <= (1.0+FTINY)*(1.0+FTINY))
110			return(1.0);
111	greg	1.2	******/
112	greg	1.1
113			len = sqrt(len);
114			v[0] /= len;
115			v[1] /= len;
116			v[2] /= len;
117			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			extern double sin(), cos();
126			double sint, cost, dotp;
127			FVECT vperp;
128			register int i;
129
130			if (theta == 0.0) {
131			VCOPY(vres, vorig);
132			return;
133			}
134			sint = sin(theta);
135			cost = cos(theta);
136			dotp = DOT(vorig, vnorm);
137			fcross(vperp, vnorm, vorig);
138			for (i = 0; i < 3; i++)
139			vres[i] = vnorm[i]dotp(1.-cost) +
140			vorig[i]cost + vperp[i]sint;
141			}