src/common/g3vector.c

#ifndef lint
static const char RCSid[] = "$Id$";
#endif
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "g3vector.h"


g3Vec   g3v_create()
{
        return ((g3Vec) calloc(3,sizeof(g3Float)));
}

void    g3v_free(g3Vec v)
{
        if (v) {
                free(v);
                v = NULL;
        }
}

g3Vec   g3v_copy(g3Vec dest,g3Vec src)
{
        if (dest == src)
                return dest;
        memcpy(dest,src,(3*sizeof(g3Float)));
        return dest;
}

g3Vec   g3v_zero(g3Vec v)
{
        memset(v,0,3*sizeof(g3Float));
        return v;
}

g3Vec   g3v_set(g3Vec v,g3Float x,g3Float y,g3Float z)
{
        v[0] = x;
        v[1] = y;
        v[2] = z;
        return v;
}

g3Vec   g3v_sane(g3Vec v)
{
        int i;
        for(i=0;i<3;i++) 
                if (gb_epseq(v[i],0.0))
                        v[i] = 0.0;
        return v;
}

g3Vec   g3v_fromrad(g3Vec v,FVECT rv)
{
        int i;
        
        for(i=0;i<3;i++) 
                v[i] = rv[i];
        return v;
}

void    g3v_torad(FVECT rv,g3Vec v)
{
        int i;
        
        for(i=0;i<3;i++) 
                rv[i] = v[i];
}

int             g3v_epszero(g3Vec v)
{
        int i;

        for(i=0;i<3;i++) 
                if (!gb_epseq(v[i],0))
                        return 0;
        return 1;
}

int             g3v_epsorder(g3Vec v1,g3Vec v2)
{
        int i;
        int res;

        for(i=0;i<3;i++) 
                if ((res = gb_epsorder(v1[i],v2[i]))) 
                        return res;
        return 0;
}

int             g3v_epseq(g3Vec v1,g3Vec v2)
{
        return (g3v_epsorder(v1,v2) == 0);
}

g3Float g3v_dot(g3Vec v1,g3Vec v2)
{
        int i;
        g3Float res = 0.0;

        for(i=0;i<3;i++) 
                res += v1[i]*v2[i];
        return res;
}


g3Float g3v_length(g3Vec v)
{
        return sqrt(g3v_dot(v,v));
}

g3Vec   g3v_normalize(g3Vec v)
{
        int i;
        g3Float len = g3v_length(v);

        if (gb_epseq(len,0))
                return NULL;
        for(i=0;i<3;i++) 
                v[i] /= len;
        return v;
}

g3Vec   g3v_cross(g3Vec res,g3Vec v1,g3Vec v2)
{
        g3Vec h = NULL;
        if ((res == v1) || (res == v2))
                h = g3v_create();
        else
                h = res;
        h[0] = v1[1]*v2[2] - v1[2]*v2[1];
    h[1] = v1[2]*v2[0] - v1[0]*v2[2];
    h[2] = v1[0]*v2[1] - v1[1]*v2[0];
        if (h != res) {
                g3v_copy(res,h);
                g3v_free(h);
        }
        return res;
}

g3Vec   g3v_sub(g3Vec res,g3Vec v1,g3Vec v2)
{
        int i;

        for(i=0;i<3;i++) 
                res[i] = v1[i] - v2[i];
        return res;
}

g3Vec   g3v_add(g3Vec res,g3Vec v1,g3Vec v2)
{
        int i;

        for(i=0;i<3;i++) 
                res[i] = v1[i] + v2[i];
        return res;
}

g3Vec   g3v_scale(g3Vec res,g3Vec v,g3Float sc)
{
        int i;

        for(i=0;i<3;i++) 
                res[i] = v[i] * sc;
        return res;
}

g3Vec   g3v_rotate(g3Vec res,g3Vec v,g3Vec rnorm,g3Float ang)
{
        g3Vec c1;
        int copy = 0;

        if (res == v) {
                res = g3v_create();
                copy = 1;
        }
        ang = -ang;
        c1 = g3v_create();
        g3v_scale(res,v,cos(ang));
        g3v_add(res,res,g3v_scale(c1,rnorm,g3v_dot(rnorm,v)*(1 - cos(ang))));
        g3v_add(res,res,g3v_scale(c1,g3v_cross(c1,v,rnorm),sin(ang)));
        g3v_free(c1);
        if (copy) {
                g3v_copy(v,res);
                g3v_free(res);
                res = v;
        }
        return res;
}

g3Vec   g3v_print(g3Vec v,FILE* fp)
{
        int i;
        
        for(i=0;i<3;i++) 
                fprintf(fp,"%f ",v[i]);
        return v;
}

int             g3v_read(g3Vec v,FILE* fp)
{
        char st[100];

        if (!fgets(st,99,fp))
                return 0;
#if USE_DOUBLE
                if (sscanf(st,"%lf %lf %lf",&(v[0]),&(v[1]),&(v[2])) != 3)
                        return 0;
#else
                if (sscanf(st,"%f %f %f",&(v[0]),&(v[1]),&(v[2])) != 3)
                        return 0;
#endif
        return 1;
}

#ifdef VEC_TEST

int     main(int argc,char** argv)
{
        g3Vec a,b;
        char st[100];

        a = g3v_create();
        b = g3v_create();
        g3v_set(b,atof(argv[1]),atof(argv[2]),atof(argv[3]));
        while (fgets(st,99,stdin)) {
                sscanf(st,"%lf %lf %lf",a,(a+1),(a+2));
                g3v_rotate(a,a,b,DEG2RAD(atof(argv[4])));
                g3v_print(a,stdout);
        }
        return EXIT_SUCCESS;
}

#endif
Revision:	2.2
Committed:	Tue Aug 18 15:02:53 2015 UTC (10 years, 2 months ago) by greg
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rad6R0, rad5R4, rad5R3, rad5R2, rad5R1, rad5R0, HEAD
Changes since 2.1:	+3 -0 lines
Log Message:	Added RCCS id's to new source files (forgotten during initial check-in)
#	User	Rev	Content
1	greg	2.2	#ifndef lint
2			static const char RCSid[] = "$Id$";
3			#endif
4	greg	2.1	#include <stdlib.h>
5			#include <stdio.h>
6			#include <string.h>
7
8			#include "g3vector.h"
9
10
11
12			g3Vec g3v_create()
13			{
14			return ((g3Vec) calloc(3,sizeof(g3Float)));
15			}
16
17			void g3v_free(g3Vec v)
18			{
19			if (v) {
20			free(v);
21			v = NULL;
22			}
23			}
24
25			g3Vec g3v_copy(g3Vec dest,g3Vec src)
26			{
27			if (dest == src)
28			return dest;
29			memcpy(dest,src,(3*sizeof(g3Float)));
30			return dest;
31			}
32
33			g3Vec g3v_zero(g3Vec v)
34			{
35			memset(v,0,3*sizeof(g3Float));
36			return v;
37			}
38
39			g3Vec g3v_set(g3Vec v,g3Float x,g3Float y,g3Float z)
40			{
41			v[0] = x;
42			v[1] = y;
43			v[2] = z;
44			return v;
45			}
46
47			g3Vec g3v_sane(g3Vec v)
48			{
49			int i;
50			for(i=0;i<3;i++)
51			if (gb_epseq(v[i],0.0))
52			v[i] = 0.0;
53			return v;
54			}
55
56			g3Vec g3v_fromrad(g3Vec v,FVECT rv)
57			{
58			int i;
59
60			for(i=0;i<3;i++)
61			v[i] = rv[i];
62			return v;
63			}
64
65			void g3v_torad(FVECT rv,g3Vec v)
66			{
67			int i;
68
69			for(i=0;i<3;i++)
70			rv[i] = v[i];
71			}
72
73			int g3v_epszero(g3Vec v)
74			{
75			int i;
76
77			for(i=0;i<3;i++)
78			if (!gb_epseq(v[i],0))
79			return 0;
80			return 1;
81			}
82
83			int g3v_epsorder(g3Vec v1,g3Vec v2)
84			{
85			int i;
86			int res;
87
88			for(i=0;i<3;i++)
89			if ((res = gb_epsorder(v1[i],v2[i])))
90			return res;
91			return 0;
92			}
93
94			int g3v_epseq(g3Vec v1,g3Vec v2)
95			{
96			return (g3v_epsorder(v1,v2) == 0);
97			}
98
99			g3Float g3v_dot(g3Vec v1,g3Vec v2)
100			{
101			int i;
102			g3Float res = 0.0;
103
104			for(i=0;i<3;i++)
105			res += v1[i]*v2[i];
106			return res;
107			}
108
109
110			g3Float g3v_length(g3Vec v)
111			{
112			return sqrt(g3v_dot(v,v));
113			}
114
115			g3Vec g3v_normalize(g3Vec v)
116			{
117			int i;
118			g3Float len = g3v_length(v);
119
120			if (gb_epseq(len,0))
121			return NULL;
122			for(i=0;i<3;i++)
123			v[i] /= len;
124			return v;
125			}
126
127			g3Vec g3v_cross(g3Vec res,g3Vec v1,g3Vec v2)
128			{
129			g3Vec h = NULL;
130			if ((res == v1) \|\| (res == v2))
131			h = g3v_create();
132			else
133			h = res;
134			h[0] = v1[1]v2[2] - v1[2]v2[1];
135			h[1] = v1[2]v2[0] - v1[0]v2[2];
136			h[2] = v1[0]v2[1] - v1[1]v2[0];
137			if (h != res) {
138			g3v_copy(res,h);
139			g3v_free(h);
140			}
141			return res;
142			}
143
144			g3Vec g3v_sub(g3Vec res,g3Vec v1,g3Vec v2)
145			{
146			int i;
147
148			for(i=0;i<3;i++)
149			res[i] = v1[i] - v2[i];
150			return res;
151			}
152
153			g3Vec g3v_add(g3Vec res,g3Vec v1,g3Vec v2)
154			{
155			int i;
156
157			for(i=0;i<3;i++)
158			res[i] = v1[i] + v2[i];
159			return res;
160			}
161
162			g3Vec g3v_scale(g3Vec res,g3Vec v,g3Float sc)
163			{
164			int i;
165
166			for(i=0;i<3;i++)
167			res[i] = v[i] * sc;
168			return res;
169			}
170
171			g3Vec g3v_rotate(g3Vec res,g3Vec v,g3Vec rnorm,g3Float ang)
172			{
173			g3Vec c1;
174			int copy = 0;
175
176			if (res == v) {
177			res = g3v_create();
178			copy = 1;
179			}
180			ang = -ang;
181			c1 = g3v_create();
182			g3v_scale(res,v,cos(ang));
183			g3v_add(res,res,g3v_scale(c1,rnorm,g3v_dot(rnorm,v)*(1 - cos(ang))));
184			g3v_add(res,res,g3v_scale(c1,g3v_cross(c1,v,rnorm),sin(ang)));
185			g3v_free(c1);
186			if (copy) {
187			g3v_copy(v,res);
188			g3v_free(res);
189			res = v;
190			}
191			return res;
192			}
193
194			g3Vec g3v_print(g3Vec v,FILE* fp)
195			{
196			int i;
197
198			for(i=0;i<3;i++)
199			fprintf(fp,"%f ",v[i]);
200			return v;
201			}
202
203			int g3v_read(g3Vec v,FILE* fp)
204			{
205			char st[100];
206
207			if (!fgets(st,99,fp))
208			return 0;
209			#if USE_DOUBLE
210			if (sscanf(st,"%lf %lf %lf",&(v[0]),&(v[1]),&(v[2])) != 3)
211			return 0;
212			#else
213			if (sscanf(st,"%f %f %f",&(v[0]),&(v[1]),&(v[2])) != 3)
214			return 0;
215			#endif
216			return 1;
217			}
218
219			#ifdef VEC_TEST
220
221			int main(int argc,char** argv)
222			{
223			g3Vec a,b;
224			char st[100];
225
226			a = g3v_create();
227			b = g3v_create();
228			g3v_set(b,atof(argv[1]),atof(argv[2]),atof(argv[3]));
229			while (fgets(st,99,stdin)) {
230			sscanf(st,"%lf %lf %lf",a,(a+1),(a+2));
231			g3v_rotate(a,a,b,DEG2RAD(atof(argv[4])));
232			g3v_print(a,stdout);
233			}
234			return EXIT_SUCCESS;
235			}
236
237			#endif