cv/mgflib/xf.c

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

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

/*
 * Routines for 4x4 homogeneous, rigid-body transformations
 */

#include <stdio.h>
#include <math.h>
#include <string.h>
#include "parser.h"

#define  d2r(a)         ((PI/180.)*(a))

#define  checkarg(a,l)  if (av[i][a] || badarg(ac-i-1,av+i+1,l)) goto done

MAT4  m4ident = MAT4IDENT;

static MAT4  m4tmp;             /* for efficiency */

int     xf_argc;                        /* total # transform args. */
char    **xf_argv;                      /* transform arguments */
XF_SPEC *xf_context;                    /* current context */

static int      xf_maxarg;              /* # allocated arguments */


int
xf_handler(ac, av)              /* handle xf entity */
int     ac;
char    **av;
{
#define randshift(x,n)  ((long)(x) << shifttab[(n)&63])
        static char     shifttab[64] = { 15, 5, 11, 5, 6, 3,
                                9, 15, 13, 2, 13, 5, 2, 12, 14, 11,
                                11, 12, 12, 3, 2, 11, 8, 12, 1, 12,
                                5, 4, 15, 9, 14, 5, 13, 14, 2, 10,
                                10, 14, 12, 3, 5, 5, 14, 6, 12, 11,
                                13, 9, 12, 8, 1, 6, 5, 12, 7, 13,
                                15, 8, 9, 2, 6, 11, 9, 11 };
        register int    i;
        register XF_SPEC        *spec;
        XF      thisxf;

        if (ac == 1) {                  /* pop top transform */
                if ((spec = xf_context) == NULL)
                        return(MG_OK);          /* should be error? */
                while (xf_argc > spec->xav0)
                        free((MEM_PTR)xf_argv[--xf_argc]);
                xf_argv[xf_argc] = NULL;
                xf_context = spec->prev;
                free((MEM_PTR)spec);
                return(MG_OK);
        }
                                        /* translate new specification */
        if (xf(&thisxf, ac-1, av+1) != ac-1)
                return(MG_ETYPE);
                                        /* allocate space for new transform */
        spec = (XF_SPEC *)malloc(sizeof(XF_SPEC));
        if (spec == NULL)
                return(MG_EMEM);
        spec->xav0 = xf_argc;
        spec->xac = ac-1;
                                        /* and store new xf arguments */
        if (xf_argc+ac > xf_maxarg) {
                if (!xf_maxarg)
                        xf_argv = (char **)malloc(
                                        (xf_maxarg=ac)*sizeof(char *));
                else
                        xf_argv = (char **)realloc((MEM_PTR)xf_argv,
                                        (xf_maxarg+=ac)*sizeof(char *));
                if (xf_argv == NULL)
                        return(MG_EMEM);
        }
        for (i = 0; i < ac-1; i++) {
                xf_argv[xf_argc] = (char *)malloc(strlen(av[i+1])+1);
                if (xf_argv[xf_argc] == NULL)
                        return(MG_EMEM);
                strcpy(xf_argv[xf_argc++], av[i+1]);
        }
        xf_argv[xf_argc] = NULL;
                                        /* compute total transformation */
        if (xf_context != NULL) {
                multmat4(spec->xf.xfm, xf_context->xf.xfm, thisxf.xfm);
                spec->xf.sca = xf_context->xf.sca * thisxf.sca;
        } else
                spec->xf = thisxf;
        spec->xid = 0;                  /* compute unique transform id */
        for (i = 0; i < sizeof(MAT4)/sizeof(unsigned short); i++)
                spec->xid ^= randshift(((unsigned short *)&spec->xf.xfm)[i],i);

        spec->prev = xf_context;        /* push new transform onto stack */
        xf_context = spec;
        return(MG_OK);
#undef randshift
}


void
xf_clear()                      /* clear transform stack */
{
        register XF_SPEC        *spec;

        while (xf_argc)
                free((MEM_PTR)xf_argv[--xf_argc]);
        if (xf_maxarg) {
                free((MEM_PTR)xf_argv);
                xf_argv = NULL;
                xf_maxarg = 0;
        }
        while ((spec = xf_context) != NULL) {
                xf_context = spec->prev;
                free((MEM_PTR)spec);
        }
}


void
xf_xfmpoint(v1, v2)             /* transform a point by the current matrix */
FVECT   v1, v2;
{
        if (xf_context == NULL) {
                v1[0] = v2[0];
                v1[1] = v2[1];
                v1[2] = v2[2];
                return;
        }
        multp3(v1, v2, xf_context->xf.xfm);
}


void
xf_xfmvect(v1, v2)              /* transform a vector using current matrix */
FVECT   v1, v2;
{
        if (xf_context == NULL) {
                v1[0] = v2[0];
                v1[1] = v2[1];
                v1[2] = v2[2];
                return;
        }
        multv3(v1, v2, xf_context->xf.xfm);
}


void
xf_rotvect(v1, v2)              /* rotate a vector using current matrix */
FVECT   v1, v2;
{
        xf_xfmvect(v1, v2);
        if (xf_context == NULL)
                return;
        v1[0] /= xf_context->xf.sca;
        v1[1] /= xf_context->xf.sca;
        v1[2] /= xf_context->xf.sca;
}


double
xf_scale(d)                     /* scale a number by the current transform */
double  d;
{
        if (xf_context == NULL)
                return(d);
        return(d*xf_context->xf.sca);
}


void
multmat4(m4a, m4b, m4c)         /* multiply m4b X m4c and put into m4a */
MAT4  m4a;
register MAT4  m4b, m4c;
{
        register int  i, j;
        
        for (i = 4; i--; )
                for (j = 4; j--; )
                        m4tmp[i][j] = m4b[i][0]*m4c[0][j] +
                                      m4b[i][1]*m4c[1][j] +
                                      m4b[i][2]*m4c[2][j] +
                                      m4b[i][3]*m4c[3][j];
        
        copymat4(m4a, m4tmp);
}


void
multv3(v3a, v3b, m4)    /* transform vector v3b by m4 and put into v3a */
FVECT  v3a;
register FVECT  v3b;
register MAT4  m4;
{
        m4tmp[0][0] = v3b[0]*m4[0][0] + v3b[1]*m4[1][0] + v3b[2]*m4[2][0];
        m4tmp[0][1] = v3b[0]*m4[0][1] + v3b[1]*m4[1][1] + v3b[2]*m4[2][1];
        m4tmp[0][2] = v3b[0]*m4[0][2] + v3b[1]*m4[1][2] + v3b[2]*m4[2][2];
        
        v3a[0] = m4tmp[0][0];
        v3a[1] = m4tmp[0][1];
        v3a[2] = m4tmp[0][2];
}


void
multp3(p3a, p3b, m4)            /* transform p3b by m4 and put into p3a */
register FVECT  p3a;
FVECT  p3b;
register MAT4  m4;
{
        multv3(p3a, p3b, m4);   /* transform as vector */
        p3a[0] += m4[3][0];     /* translate */
        p3a[1] += m4[3][1];
        p3a[2] += m4[3][2];
}


int
xf(ret, ac, av)                 /* get transform specification */
register XF  *ret;
int  ac;
char  **av;
{
        MAT4  xfmat, m4;
        double  xfsca, dtmp;
        int  i, icnt;

        setident4(ret->xfm);
        ret->sca = 1.0;

        icnt = 1;
        setident4(xfmat);
        xfsca = 1.0;

        for (i = 0; i < ac && av[i][0] == '-'; i++) {
        
                setident4(m4);
                
                switch (av[i][1]) {
        
                case 't':                       /* translate */
                        checkarg(2,"fff");
                        m4[3][0] = atof(av[++i]);
                        m4[3][1] = atof(av[++i]);
                        m4[3][2] = atof(av[++i]);
                        break;

                case 'r':                       /* rotate */
                        switch (av[i][2]) {
                        case 'x':
                                checkarg(3,"f");
                                dtmp = d2r(atof(av[++i]));
                                m4[1][1] = m4[2][2] = cos(dtmp);
                                m4[2][1] = -(m4[1][2] = sin(dtmp));
                                break;
                        case 'y':
                                checkarg(3,"f");
                                dtmp = d2r(atof(av[++i]));
                                m4[0][0] = m4[2][2] = cos(dtmp);
                                m4[0][2] = -(m4[2][0] = sin(dtmp));
                                break;
                        case 'z':
                                checkarg(3,"f");
                                dtmp = d2r(atof(av[++i]));
                                m4[0][0] = m4[1][1] = cos(dtmp);
                                m4[1][0] = -(m4[0][1] = sin(dtmp));
                                break;
                        default:
                                goto done;
                        }
                        break;

                case 's':                       /* scale */
                        checkarg(2,"f");
                        dtmp = atof(av[i+1]);
                        if (dtmp == 0.0) goto done;
                        i++;
                        xfsca *=
                        m4[0][0] = 
                        m4[1][1] = 
                        m4[2][2] = dtmp;
                        break;

                case 'm':                       /* mirror */
                        switch (av[i][2]) {
                        case 'x':
                                checkarg(3,"");
                                xfsca *=
                                m4[0][0] = -1.0;
                                break;
                        case 'y':
                                checkarg(3,"");
                                xfsca *=
                                m4[1][1] = -1.0;
                                break;
                        case 'z':
                                checkarg(3,"");
                                xfsca *=
                                m4[2][2] = -1.0;
                                break;
                        default:
                                goto done;
                        }
                        break;

                case 'i':                       /* iterate */
                        checkarg(2,"i");
                        while (icnt-- > 0) {
                                multmat4(ret->xfm, ret->xfm, xfmat);
                                ret->sca *= xfsca;
                        }
                        icnt = atoi(av[++i]);
                        setident4(xfmat);
                        xfsca = 1.0;
                        continue;

                default:
                        goto done;

                }
                multmat4(xfmat, xfmat, m4);
        }
done:
        while (icnt-- > 0) {
                multmat4(ret->xfm, ret->xfm, xfmat);
                ret->sca *= xfsca;
        }
        return(i);
}
Revision:	1.4
Committed:	Sat Jun 25 16:33:19 1994 UTC (29 years, 10 months ago) by greg
Content type:	text/plain
Branch:	MAIN
Changes since 1.3:	+13 -0 lines
Log Message:	bug fixes and enhancements
#	User	Rev	Content
1	greg	1.1	/* Copyright (c) 1994 Regents of the University of California */
2
3			#ifndef lint
4			static char SCCSid[] = "$SunId$ LBL";
5			#endif
6
7			/*
8			* Routines for 4x4 homogeneous, rigid-body transformations
9			*/
10
11			#include <stdio.h>
12			#include <math.h>
13			#include <string.h>
14			#include "parser.h"
15
16			#define d2r(a) ((PI/180.)*(a))
17
18			#define checkarg(a,l) if (av[i][a] \|\| badarg(ac-i-1,av+i+1,l)) goto done
19
20			MAT4 m4ident = MAT4IDENT;
21
22			static MAT4 m4tmp; /* for efficiency */
23
24			int xf_argc; /* total # transform args. */
25			char *xf_argv; / transform arguments */
26			XF_SPEC xf_context; / current context */
27
28			static int xf_maxarg; /* # allocated arguments */
29
30
31			int
32			xf_handler(ac, av) /* handle xf entity */
33			int ac;
34			char **av;
35			{
36	greg	1.4	#define randshift(x,n) ((long)(x) << shifttab[(n)&63])
37			static char shifttab[64] = { 15, 5, 11, 5, 6, 3,
38			9, 15, 13, 2, 13, 5, 2, 12, 14, 11,
39			11, 12, 12, 3, 2, 11, 8, 12, 1, 12,
40			5, 4, 15, 9, 14, 5, 13, 14, 2, 10,
41			10, 14, 12, 3, 5, 5, 14, 6, 12, 11,
42			13, 9, 12, 8, 1, 6, 5, 12, 7, 13,
43			15, 8, 9, 2, 6, 11, 9, 11 };
44	greg	1.1	register int i;
45			register XF_SPEC *spec;
46			XF thisxf;
47
48			if (ac == 1) { /* pop top transform */
49			if ((spec = xf_context) == NULL)
50			return(MG_OK); /* should be error? */
51			while (xf_argc > spec->xav0)
52			free((MEM_PTR)xf_argv[--xf_argc]);
53			xf_argv[xf_argc] = NULL;
54			xf_context = spec->prev;
55			free((MEM_PTR)spec);
56			return(MG_OK);
57			}
58			/* translate new specification */
59			if (xf(&thisxf, ac-1, av+1) != ac-1)
60			return(MG_ETYPE);
61			/* allocate space for new transform */
62			spec = (XF_SPEC *)malloc(sizeof(XF_SPEC));
63			if (spec == NULL)
64			return(MG_EMEM);
65			spec->xav0 = xf_argc;
66			spec->xac = ac-1;
67			/* and store new xf arguments */
68			if (xf_argc+ac > xf_maxarg) {
69			if (!xf_maxarg)
70			xf_argv = (char **)malloc(
71			(xf_maxarg=ac)sizeof(char ));
72			else
73			xf_argv = (char **)realloc((MEM_PTR)xf_argv,
74			(xf_maxarg+=ac)sizeof(char ));
75			if (xf_argv == NULL)
76			return(MG_EMEM);
77			}
78			for (i = 0; i < ac-1; i++) {
79			xf_argv[xf_argc] = (char *)malloc(strlen(av[i+1])+1);
80			if (xf_argv[xf_argc] == NULL)
81			return(MG_EMEM);
82			strcpy(xf_argv[xf_argc++], av[i+1]);
83			}
84			xf_argv[xf_argc] = NULL;
85			/* compute total transformation */
86			if (xf_context != NULL) {
87			multmat4(spec->xf.xfm, xf_context->xf.xfm, thisxf.xfm);
88			spec->xf.sca = xf_context->xf.sca * thisxf.sca;
89			} else
90			spec->xf = thisxf;
91	greg	1.4	spec->xid = 0; /* compute unique transform id */
92			for (i = 0; i < sizeof(MAT4)/sizeof(unsigned short); i++)
93			spec->xid ^= randshift(((unsigned short *)&spec->xf.xfm)[i],i);
94
95	greg	1.1	spec->prev = xf_context; /* push new transform onto stack */
96			xf_context = spec;
97			return(MG_OK);
98	greg	1.4	#undef randshift
99	greg	1.1	}
100
101
102			void
103	greg	1.2	xf_clear() /* clear transform stack */
104			{
105			register XF_SPEC *spec;
106
107			while (xf_argc)
108			free((MEM_PTR)xf_argv[--xf_argc]);
109			if (xf_maxarg) {
110			free((MEM_PTR)xf_argv);
111			xf_argv = NULL;
112			xf_maxarg = 0;
113			}
114			while ((spec = xf_context) != NULL) {
115			xf_context = spec->prev;
116			free((MEM_PTR)spec);
117			}
118			}
119
120
121			void
122	greg	1.1	xf_xfmpoint(v1, v2) /* transform a point by the current matrix */
123			FVECT v1, v2;
124			{
125			if (xf_context == NULL) {
126			v1[0] = v2[0];
127			v1[1] = v2[1];
128			v1[2] = v2[2];
129			return;
130			}
131			multp3(v1, v2, xf_context->xf.xfm);
132			}
133
134
135			void
136			xf_xfmvect(v1, v2) /* transform a vector using current matrix */
137			FVECT v1, v2;
138			{
139			if (xf_context == NULL) {
140			v1[0] = v2[0];
141			v1[1] = v2[1];
142			v1[2] = v2[2];
143			return;
144			}
145			multv3(v1, v2, xf_context->xf.xfm);
146			}
147
148
149			void
150			xf_rotvect(v1, v2) /* rotate a vector using current matrix */
151			FVECT v1, v2;
152			{
153			xf_xfmvect(v1, v2);
154			if (xf_context == NULL)
155			return;
156			v1[0] /= xf_context->xf.sca;
157			v1[1] /= xf_context->xf.sca;
158			v1[2] /= xf_context->xf.sca;
159			}
160
161
162			double
163			xf_scale(d) /* scale a number by the current transform */
164			double d;
165			{
166			if (xf_context == NULL)
167			return(d);
168			return(d*xf_context->xf.sca);
169			}
170
171
172			void
173			multmat4(m4a, m4b, m4c) /* multiply m4b X m4c and put into m4a */
174			MAT4 m4a;
175			register MAT4 m4b, m4c;
176			{
177			register int i, j;
178
179			for (i = 4; i--; )
180			for (j = 4; j--; )
181			m4tmp[i][j] = m4b[i][0]*m4c[0][j] +
182			m4b[i][1]*m4c[1][j] +
183			m4b[i][2]*m4c[2][j] +
184			m4b[i][3]*m4c[3][j];
185
186			copymat4(m4a, m4tmp);
187			}
188
189
190			void
191			multv3(v3a, v3b, m4) /* transform vector v3b by m4 and put into v3a */
192			FVECT v3a;
193			register FVECT v3b;
194			register MAT4 m4;
195			{
196			m4tmp[0][0] = v3b[0]m4[0][0] + v3b[1]m4[1][0] + v3b[2]*m4[2][0];
197			m4tmp[0][1] = v3b[0]m4[0][1] + v3b[1]m4[1][1] + v3b[2]*m4[2][1];
198			m4tmp[0][2] = v3b[0]m4[0][2] + v3b[1]m4[1][2] + v3b[2]*m4[2][2];
199
200			v3a[0] = m4tmp[0][0];
201			v3a[1] = m4tmp[0][1];
202			v3a[2] = m4tmp[0][2];
203			}
204
205
206			void
207			multp3(p3a, p3b, m4) /* transform p3b by m4 and put into p3a */
208			register FVECT p3a;
209			FVECT p3b;
210			register MAT4 m4;
211			{
212			multv3(p3a, p3b, m4); /* transform as vector */
213			p3a[0] += m4[3][0]; /* translate */
214			p3a[1] += m4[3][1];
215			p3a[2] += m4[3][2];
216			}
217
218
219			int
220			xf(ret, ac, av) /* get transform specification */
221			register XF *ret;
222			int ac;
223	greg	1.3	char **av;
224	greg	1.1	{
225			MAT4 xfmat, m4;
226			double xfsca, dtmp;
227			int i, icnt;
228
229			setident4(ret->xfm);
230			ret->sca = 1.0;
231
232			icnt = 1;
233			setident4(xfmat);
234			xfsca = 1.0;
235
236			for (i = 0; i < ac && av[i][0] == '-'; i++) {
237
238			setident4(m4);
239
240			switch (av[i][1]) {
241
242			case 't': /* translate */
243			checkarg(2,"fff");
244			m4[3][0] = atof(av[++i]);
245			m4[3][1] = atof(av[++i]);
246			m4[3][2] = atof(av[++i]);
247			break;
248
249			case 'r': /* rotate */
250			switch (av[i][2]) {
251			case 'x':
252			checkarg(3,"f");
253			dtmp = d2r(atof(av[++i]));
254			m4[1][1] = m4[2][2] = cos(dtmp);
255			m4[2][1] = -(m4[1][2] = sin(dtmp));
256			break;
257			case 'y':
258			checkarg(3,"f");
259			dtmp = d2r(atof(av[++i]));
260			m4[0][0] = m4[2][2] = cos(dtmp);
261			m4[0][2] = -(m4[2][0] = sin(dtmp));
262			break;
263			case 'z':
264			checkarg(3,"f");
265			dtmp = d2r(atof(av[++i]));
266			m4[0][0] = m4[1][1] = cos(dtmp);
267			m4[1][0] = -(m4[0][1] = sin(dtmp));
268			break;
269			default:
270			goto done;
271			}
272			break;
273
274			case 's': /* scale */
275			checkarg(2,"f");
276			dtmp = atof(av[i+1]);
277			if (dtmp == 0.0) goto done;
278			i++;
279			xfsca *=
280			m4[0][0] =
281			m4[1][1] =
282			m4[2][2] = dtmp;
283			break;
284
285			case 'm': /* mirror */
286			switch (av[i][2]) {
287			case 'x':
288			checkarg(3,"");
289			xfsca *=
290			m4[0][0] = -1.0;
291			break;
292			case 'y':
293			checkarg(3,"");
294			xfsca *=
295			m4[1][1] = -1.0;
296			break;
297			case 'z':
298			checkarg(3,"");
299			xfsca *=
300			m4[2][2] = -1.0;
301			break;
302			default:
303			goto done;
304			}
305			break;
306
307			case 'i': /* iterate */
308			checkarg(2,"i");
309			while (icnt-- > 0) {
310			multmat4(ret->xfm, ret->xfm, xfmat);
311			ret->sca *= xfsca;
312			}
313			icnt = atoi(av[++i]);
314			setident4(xfmat);
315			xfsca = 1.0;
316			continue;
317
318			default:
319			goto done;
320
321			}
322			multmat4(xfmat, xfmat, m4);
323			}
324			done:
325			while (icnt-- > 0) {
326			multmat4(ret->xfm, ret->xfm, xfmat);
327			ret->sca *= xfsca;
328			}
329			return(i);
330			}