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;
{
        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->prev = xf_context;        /* push new transform onto stack */
        xf_context = spec;
        return(MG_OK);
}


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.1
Committed:	Tue Jun 21 14:45:48 1994 UTC (29 years, 10 months ago) by greg
Content type:	text/plain
Branch:	MAIN
Log Message:	Initial revision
#	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			register int i;
37			register XF_SPEC *spec;
38			XF thisxf;
39
40			if (ac == 1) { /* pop top transform */
41			if ((spec = xf_context) == NULL)
42			return(MG_OK); /* should be error? */
43			while (xf_argc > spec->xav0)
44			free((MEM_PTR)xf_argv[--xf_argc]);
45			xf_argv[xf_argc] = NULL;
46			xf_context = spec->prev;
47			free((MEM_PTR)spec);
48			return(MG_OK);
49			}
50			/* translate new specification */
51			if (xf(&thisxf, ac-1, av+1) != ac-1)
52			return(MG_ETYPE);
53			/* allocate space for new transform */
54			spec = (XF_SPEC *)malloc(sizeof(XF_SPEC));
55			if (spec == NULL)
56			return(MG_EMEM);
57			spec->xav0 = xf_argc;
58			spec->xac = ac-1;
59			/* and store new xf arguments */
60			if (xf_argc+ac > xf_maxarg) {
61			if (!xf_maxarg)
62			xf_argv = (char **)malloc(
63			(xf_maxarg=ac)sizeof(char ));
64			else
65			xf_argv = (char **)realloc((MEM_PTR)xf_argv,
66			(xf_maxarg+=ac)sizeof(char ));
67			if (xf_argv == NULL)
68			return(MG_EMEM);
69			}
70			for (i = 0; i < ac-1; i++) {
71			xf_argv[xf_argc] = (char *)malloc(strlen(av[i+1])+1);
72			if (xf_argv[xf_argc] == NULL)
73			return(MG_EMEM);
74			strcpy(xf_argv[xf_argc++], av[i+1]);
75			}
76			xf_argv[xf_argc] = NULL;
77			/* compute total transformation */
78			if (xf_context != NULL) {
79			multmat4(spec->xf.xfm, xf_context->xf.xfm, thisxf.xfm);
80			spec->xf.sca = xf_context->xf.sca * thisxf.sca;
81			} else
82			spec->xf = thisxf;
83			spec->prev = xf_context; /* push new transform onto stack */
84			xf_context = spec;
85			return(MG_OK);
86			}
87
88
89			void
90			xf_xfmpoint(v1, v2) /* transform a point by the current matrix */
91			FVECT v1, v2;
92			{
93			if (xf_context == NULL) {
94			v1[0] = v2[0];
95			v1[1] = v2[1];
96			v1[2] = v2[2];
97			return;
98			}
99			multp3(v1, v2, xf_context->xf.xfm);
100			}
101
102
103			void
104			xf_xfmvect(v1, v2) /* transform a vector using current matrix */
105			FVECT v1, v2;
106			{
107			if (xf_context == NULL) {
108			v1[0] = v2[0];
109			v1[1] = v2[1];
110			v1[2] = v2[2];
111			return;
112			}
113			multv3(v1, v2, xf_context->xf.xfm);
114			}
115
116
117			void
118			xf_rotvect(v1, v2) /* rotate a vector using current matrix */
119			FVECT v1, v2;
120			{
121			xf_xfmvect(v1, v2);
122			if (xf_context == NULL)
123			return;
124			v1[0] /= xf_context->xf.sca;
125			v1[1] /= xf_context->xf.sca;
126			v1[2] /= xf_context->xf.sca;
127			}
128
129
130			double
131			xf_scale(d) /* scale a number by the current transform */
132			double d;
133			{
134			if (xf_context == NULL)
135			return(d);
136			return(d*xf_context->xf.sca);
137			}
138
139
140			void
141			multmat4(m4a, m4b, m4c) /* multiply m4b X m4c and put into m4a */
142			MAT4 m4a;
143			register MAT4 m4b, m4c;
144			{
145			register int i, j;
146
147			for (i = 4; i--; )
148			for (j = 4; j--; )
149			m4tmp[i][j] = m4b[i][0]*m4c[0][j] +
150			m4b[i][1]*m4c[1][j] +
151			m4b[i][2]*m4c[2][j] +
152			m4b[i][3]*m4c[3][j];
153
154			copymat4(m4a, m4tmp);
155			}
156
157
158			void
159			multv3(v3a, v3b, m4) /* transform vector v3b by m4 and put into v3a */
160			FVECT v3a;
161			register FVECT v3b;
162			register MAT4 m4;
163			{
164			m4tmp[0][0] = v3b[0]m4[0][0] + v3b[1]m4[1][0] + v3b[2]*m4[2][0];
165			m4tmp[0][1] = v3b[0]m4[0][1] + v3b[1]m4[1][1] + v3b[2]*m4[2][1];
166			m4tmp[0][2] = v3b[0]m4[0][2] + v3b[1]m4[1][2] + v3b[2]*m4[2][2];
167
168			v3a[0] = m4tmp[0][0];
169			v3a[1] = m4tmp[0][1];
170			v3a[2] = m4tmp[0][2];
171			}
172
173
174			void
175			multp3(p3a, p3b, m4) /* transform p3b by m4 and put into p3a */
176			register FVECT p3a;
177			FVECT p3b;
178			register MAT4 m4;
179			{
180			multv3(p3a, p3b, m4); /* transform as vector */
181			p3a[0] += m4[3][0]; /* translate */
182			p3a[1] += m4[3][1];
183			p3a[2] += m4[3][2];
184			}
185
186
187			int
188			xf(ret, ac, av) /* get transform specification */
189			register XF *ret;
190			int ac;
191			char *av[];
192			{
193			MAT4 xfmat, m4;
194			double xfsca, dtmp;
195			int i, icnt;
196
197			setident4(ret->xfm);
198			ret->sca = 1.0;
199
200			icnt = 1;
201			setident4(xfmat);
202			xfsca = 1.0;
203
204			for (i = 0; i < ac && av[i][0] == '-'; i++) {
205
206			setident4(m4);
207
208			switch (av[i][1]) {
209
210			case 't': /* translate */
211			checkarg(2,"fff");
212			m4[3][0] = atof(av[++i]);
213			m4[3][1] = atof(av[++i]);
214			m4[3][2] = atof(av[++i]);
215			break;
216
217			case 'r': /* rotate */
218			switch (av[i][2]) {
219			case 'x':
220			checkarg(3,"f");
221			dtmp = d2r(atof(av[++i]));
222			m4[1][1] = m4[2][2] = cos(dtmp);
223			m4[2][1] = -(m4[1][2] = sin(dtmp));
224			break;
225			case 'y':
226			checkarg(3,"f");
227			dtmp = d2r(atof(av[++i]));
228			m4[0][0] = m4[2][2] = cos(dtmp);
229			m4[0][2] = -(m4[2][0] = sin(dtmp));
230			break;
231			case 'z':
232			checkarg(3,"f");
233			dtmp = d2r(atof(av[++i]));
234			m4[0][0] = m4[1][1] = cos(dtmp);
235			m4[1][0] = -(m4[0][1] = sin(dtmp));
236			break;
237			default:
238			goto done;
239			}
240			break;
241
242			case 's': /* scale */
243			checkarg(2,"f");
244			dtmp = atof(av[i+1]);
245			if (dtmp == 0.0) goto done;
246			i++;
247			xfsca *=
248			m4[0][0] =
249			m4[1][1] =
250			m4[2][2] = dtmp;
251			break;
252
253			case 'm': /* mirror */
254			switch (av[i][2]) {
255			case 'x':
256			checkarg(3,"");
257			xfsca *=
258			m4[0][0] = -1.0;
259			break;
260			case 'y':
261			checkarg(3,"");
262			xfsca *=
263			m4[1][1] = -1.0;
264			break;
265			case 'z':
266			checkarg(3,"");
267			xfsca *=
268			m4[2][2] = -1.0;
269			break;
270			default:
271			goto done;
272			}
273			break;
274
275			case 'i': /* iterate */
276			checkarg(2,"i");
277			while (icnt-- > 0) {
278			multmat4(ret->xfm, ret->xfm, xfmat);
279			ret->sca *= xfsca;
280			}
281			icnt = atoi(av[++i]);
282			setident4(xfmat);
283			xfsca = 1.0;
284			continue;
285
286			default:
287			goto done;
288
289			}
290			multmat4(xfmat, xfmat, m4);
291			}
292			done:
293			while (icnt-- > 0) {
294			multmat4(ret->xfm, ret->xfm, xfmat);
295			ret->sca *= xfsca;
296			}
297			return(i);
298			}