matrix.c
Go to the documentation of this file.
1 /***************************************************/
2 /* float vector and matrix operations
3 /* Copyright(c) Toshihiro Matsui
4 /* Electrotechnical Laboratory
5 /*
6 /* 1986-Nov
7 /* 1987-Feb complete floatvector
8 /* 1987-Mar modify rotation
9 /* 1987-Nov simultaneous equations
10 /* 1988-Jan matrix is represented by 2D array,
11 /* not by an vector of vectors
12 /**************************************************************/
13 static char *rcsid="@(#)$Id$";
14 
15 #include "math.h"
16 #include "eus.h"
17 
18 extern double sin(), cos(), sqrt(), atan2();
19 extern pointer makefvector(),defkeyword();
20 extern pointer makematrix();
21 
23 
24 /* #define PI 3.1415926536 */
25 
26 #define ckvsize(a,b) ((a->c.vec.size==b->c.vec.size)?vecsize(a):(int)(eusinteger_t)error(E_VECINDEX))
27 
28 pointer VPLUS(ctx,n,argv)
29 register context *ctx;
30 int n;
31 register pointer *argv;
32 { register int i,s;
33  register pointer result;
34  register int isf,isi,iss;
35 
36  ckarg2(2,3);
37  if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
38  !((isi=isintvector(argv[0])) && isintvector(argv[1])) &&
39  !((iss=isstring(argv[0])) && isstring(argv[1])))
41  s=ckvsize(argv[0],argv[1]);
42  if (n==3) {
43  result=argv[2];
44  if (s!=vecsize(result)) error(E_FLOATVECTOR);}
45  else result=makevector(classof(argv[0]), s);
46  if (isf) {
47  register eusfloat_t *a,*b,*r;
48  if (!isfltvector(result)) error(E_FLOATVECTOR);
49  a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
50  r=result->c.fvec.fv;
51  for(i=0; i<s; i++) r[i]= a[i] + b[i];
52  return(result);}
53  else if (isi) {
54  register eusinteger_t *ia, *ib, *ir;
55  if (!isintvector(result)) error(E_NOINTVECTOR);
56  ia=argv[0]->c.ivec.iv; ib=argv[1]->c.ivec.iv;
57  ir=result->c.ivec.iv;
58  for(i=0; i<s; i++) ir[i]= ia[i] + ib[i];
59  return(result);}
60  else if (iss) {
61  register unsigned char *ba, *bb, *br;
62  ba=argv[0]->c.str.chars;
63  bb=argv[1]->c.str.chars;
64  br=result->c.str.chars;
65  for(i=0; i<s; i++) br[i]=ba[i] + bb[i];
66  return(result);}
67  else error(E_NOSEQ);
68  }
69 
70 pointer VPLUSPLUS(ctx,n,argv)
71 register context *ctx;
72 int n;
73 register pointer *argv;
74 { register int i,s;
75  register pointer p,result;
76  eusfloat_t *a,*r;
77 
78  if (n==1) return(argv[0]);
79  if (!isfltvector(argv[0])) error(E_FLOATVECTOR);
80  s=vecsize(argv[0]);
81  result=makefvector(s);
82  r=result->c.fvec.fv; a=argv[0]->c.fvec.fv;
83  for (i=0; i<s; i++) r[i]=a[i];
84  while (--n>0) {
85  p=argv[n];
86  if (!isfltvector(p)) error(E_FLOATVECTOR);
87  if (vecsize(p) != s) error(E_VECINDEX);
88  a=p->c.fvec.fv;
89  for (i=0; i<s; i++) r[i]+=a[i];}
90  return(result);}
91 
92 pointer VMINUS(ctx,n,argv)
93 register context *ctx;
94 int n;
95 register pointer *argv;
96 { register int i,s;
97  register eusinteger_t *ia, *ib, *ir;
98  register pointer result;
99  register eusfloat_t *a,*b,*r;
100  register int isi,isf,iss;
101 
102  ckarg2(1,3);
103  isi=isintvector(argv[0]);
104  isf=isfltvector(argv[0]);
105  iss=isstring (argv[0]);
106  if (!isi && !isf && !iss)
107  error(E_NOVECTOR);
108  s=vecsize(argv[0]);
109  if (n==1) { /*negate float vector*/
110  result=makevector(classof(argv[0]),s);
111  if (isi) {
112  ia=argv[0]->c.ivec.iv;
113  for (i=0; i<s; i++) result->c.ivec.iv[i]= -ia[i];
114  return(result);}
115  else if (isf) {
116  a=argv[0]->c.fvec.fv;
117  for (i=0; i<s; i++) result->c.fvec.fv[i]= -a[i];
118  return(result);}
119  else error(E_NOVECTOR);}
120 
121  /* argc>=2 */
122  if (!(isintvector(argv[1])&&isi) &&
123  !(isfltvector(argv[1])&&isf) &&
124  !(isstring(argv[1]) &&iss))
126  s=ckvsize(argv[0],argv[1]);
127  if (n==3) {
128  result=argv[2];
129  if (!(isf&&isfltvector(result)) && !(isi&&isintvector(result)) &&
130  !(iss&&isstring(result)) )
132  if (vecsize(result)!=s) error(E_VECINDEX);}
133  else result=makevector(classof(argv[0]),s);
134 
135  if (isf) {
136  a=argv[0]->c.fvec.fv;
137  b=argv[1]->c.fvec.fv;
138  r=result->c.fvec.fv;
139  for(i=0; i<s; i++) r[i]=a[i]-b[i];
140  return(result);}
141  else if (isi) {
142  ia=argv[0]->c.ivec.iv;
143  ib=argv[1]->c.ivec.iv;
144  ir=result->c.ivec.iv;
145  for(i=0; i<s; i++) ir[i]=ia[i]-ib[i];
146  return(result);}
147  else if (iss) {
148  register unsigned char *ba, *bb, *br;
149  ba=argv[0]->c.str.chars;
150  bb=argv[1]->c.str.chars;
151  br=result->c.str.chars;
152  for(i=0; i<s; i++) br[i]=ba[i]-bb[i];
153  return(result);}
154  else error(E_NOSEQ);}
155 
157 register context *ctx;
158 int n;
159 register pointer *argv;
160 { register int i,s;
161  register eusinteger_t *ia, *ib, *ir;
162  register pointer result;
163  register eusfloat_t *a,*b,*r;
164  register eusfloat_t x;
165  register int isi,isf,iss;
166 
167  register int ix;
168 
169  ckarg2(2,3);
170  if (!(isi=isintvector(argv[0])) && !(isf=isfltvector(argv[0])) &&
171  !(iss=isstring (argv[0])))
172  error(E_NOVECTOR);
173  s=vecsize(argv[0]);
174 
175  /* argc>=2 */
176  if (!(isintvector(argv[1])&&isi) &&
177  !(isfltvector(argv[1])&&isf) &&
178  !(isstring(argv[1]) &&iss))
180  s=ckvsize(argv[0],argv[1]);
181  if (n==3) {
182  result=argv[2];
183  if (!(isf&&isfltvector(result)) && !(isi&&isintvector(result)) &&
184  !(iss&&isstring(result)) )
186  if (vecsize(result)!=s) error(E_VECINDEX);}
187  else result=makevector(classof(argv[0]),s);
188 
189  if (isf) {
190  a=argv[0]->c.fvec.fv;
191  b=argv[1]->c.fvec.fv;
192  r=result->c.fvec.fv;
193  for(i=0; i<s; i++) {
194  x=a[i]-b[i];
195  if (x<0) x= -x;
196  r[i]=x;}
197  return(result);}
198  else if (isi) {
199  ia=argv[0]->c.ivec.iv;
200  ib=argv[1]->c.ivec.iv;
201  ir=result->c.ivec.iv;
202  for(i=0; i<s; i++) {
203  ix=ia[i]-ib[i];
204  if (ix<0) ix= -ix;
205  ir[i]=ix;}
206  return(result);}
207  else if (iss) {
208  register unsigned char *ba, *bb, *br;
209  ba=argv[0]->c.str.chars;
210  bb=argv[1]->c.str.chars;
211  br=result->c.str.chars;
212  for(i=0; i<s; i++) {
213  ix=ba[i]-bb[i];
214  if (ix<0) ix= -ix;
215  br[i]=ix;}
216  return(result);}
217  else error(E_NOSEQ);}
218 
220 register context *ctx;
221 int n;
222 register pointer *argv;
223 { register eusfloat_t *a,*b;
224  register long i,s;
225  register eusinteger_t *ia, *ib; register long isum=0;
226  eusfloat_t sum=0.0;
227  register int isf,isi;
228  numunion nu;
229  ckarg(2);
230  if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
231  !((isi=isintvector(argv[0])) && isintvector(argv[1])))
233  s=ckvsize(argv[0],argv[1]);
234  if (isf) {
235  a= (argv[0]->c.fvec.fv); b= (argv[1]->c.fvec.fv);
236  for (i=0; i<s; i++) sum+= a[i] * b[i];
237  return(makeflt(sum));}
238  if (isi) {
239  ia= (argv[0]->c.ivec.iv); ib= (argv[1]->c.ivec.iv);
240  for (i=0; i<s; i++) isum+= ia[i] * ib[i];
241  return(makeint(isum));}
242  else error(E_NOSEQ);}
243 
244 pointer VNORM(ctx,n,argv)
245 register context *ctx;
246 int n;
247 pointer *argv;
248 { register int i,s;
249  register eusfloat_t *a,sum=0.0;
250  numunion nu;
251  ckarg(1);
252  if (!isvector(argv[0])) error(E_NOVECTOR);
253  s=vecsize(argv[0]);
254  if (elmtypeof(argv[0])==ELM_FLOAT) {
255  a=argv[0]->c.fvec.fv;
256  for (i=0; i<s; i++) sum+=a[i]*a[i];
257  sum=sqrt(sum);
258  return(makeflt(sum));}
259  else if (elmtypeof(argv[0])==ELM_INT) {
260  eusinteger_t *ia;
261  ia=argv[0]->c.ivec.iv;
262  for (i=0; i<s; i++) sum+=ia[i]*ia[i];
263  sum=sqrt(sum);
264  return(makeflt(sum));}
265  else error(E_FLOATVECTOR);
266  }
267 
268 pointer VNORM2(ctx,n,argv)
269 register context *ctx;
270 int n;
271 pointer *argv;
272 { register int i,s;
273  register eusfloat_t *a,sum=0.0;
274  numunion nu;
275  ckarg(1);
276  if (!isvector(argv[0])) error(E_NOVECTOR);
277  s=vecsize(argv[0]);
278  if (elmtypeof(argv[0])==ELM_FLOAT) {
279  a=argv[0]->c.fvec.fv;
280  for (i=0; i<s; i++) sum+=a[i]*a[i];
281  /* sum=sqrt(sum); no square root */
282  return(makeflt(sum));}
283  else if (elmtypeof(argv[0])==ELM_INT) {
284  eusinteger_t *ia;
285  ia=argv[0]->c.ivec.iv;
286  for (i=0; i<s; i++) sum+=ia[i]*ia[i];
287  return(makeflt(sum));}
288  else error(E_FLOATVECTOR);}
289 
291 register context *ctx;
292 int n;
293 register pointer argv[];
294 { register pointer result;
295  register eusfloat_t *a,*r,sum=0.0;
296  register int i,s;
297 
298  ckarg2(1,2);
299  if (!isfltvector(argv[0])) error(E_FLOATVECTOR);
300  s=vecsize(argv[0]);
301  if (n==2) {
302  result=argv[1];
303  if (!isfltvector(result)) error(E_FLOATVECTOR);
304  if (s!=vecsize(result)) error(E_VECINDEX);}
305  else result=makefvector(s);
306  a=argv[0]->c.fvec.fv;
307  r=result->c.fvec.fv;
308  for (i=0; i<s; i++) sum+= a[i]*a[i];
309  sum=sqrt(sum);
310  for (i=0; i<s; i++) r[i]=a[i]/sum;
311  return(result);}
312 
313 pointer VDISTANCE(ctx,n,argv)
314 register context *ctx;
315 int n;
316 pointer argv[];
317 {
318  register eusfloat_t *a, *b;
319  register int s;
320  eusfloat_t d,dist=0.0;
321  register int isf,isi;
322  numunion nu;
323  ckarg(2);
324  if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
325  !((isi=isintvector(argv[0])) && isintvector(argv[1])))
327  s=ckvsize(argv[0],argv[1]);
328  if (isf) {
329  a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
330  while (--s>=0) { d=a[s]-b[s]; dist+= d*d;}
331  return(makeflt(sqrt(dist)));}
332  else if (isi) {
333  register eusinteger_t *ia, *ib;
334  register long id, idist=0;
335  ia=argv[0]->c.ivec.iv; ib=argv[1]->c.ivec.iv;
336  while (--s>=0) { id= ia[s]-ib[s]; idist+= id * id;}
337  dist=idist;
338  return(makeflt(sqrt(dist)));}
339  else error(E_FLOATVECTOR);}
340 
342 register context *ctx;
343 int n;
344 pointer argv[];
345 {
346  register eusfloat_t *a, *b;
347  register int s;
348  eusfloat_t d,dist=0.0;
349  register int isf,isi;
350  numunion nu;
351  ckarg(2);
352  if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
353  !((isi=isintvector(argv[0])) && isintvector(argv[1])))
355  s=ckvsize(argv[0],argv[1]);
356  if (isf) {
357  a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
358  while (--s>=0) { d=a[s]-b[s]; dist+= d*d;}
359  return(makeflt(dist));}
360  else if (isi) {
361  register eusinteger_t *ia, *ib;
362  register long id, idist=0;
363  ia=argv[0]->c.ivec.iv; ib=argv[1]->c.ivec.iv;
364  while (--s>=0) { id= ia[s]-ib[s]; idist+= id * id;}
365  dist=idist;
366  return(makeflt(dist));}
367  else error(E_FLOATVECTOR);}
368 
370 register context *ctx;
371 int n;
372 pointer argv[];
373 {
374  register eusfloat_t *a, *b, *r;
375  register int i,s;
376  pointer result;
377  eusfloat_t norm=0.0;
378  ckarg2(2,3);
379  if (!isfltvector(argv[0]) || !isfltvector(argv[1])) error(E_FLOATVECTOR);
380  s=ckvsize(argv[0],argv[1]);
381  if (n==3) {
382  result=argv[2];
383  if (!isfltvector(result)) error(E_FLOATVECTOR);
384  if (vecsize(result)!=s) error(E_VECINDEX);
385  }
386  else result=makefvector(s);
387  r=result->c.fvec.fv; a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
388  for (i=0; i<s; i++) { r[i]=b[i]-a[i]; norm+= r[i]*r[i];}
389  norm=sqrt(norm);
390  for (i=0; i<s; i++) { r[i]=r[i]/norm;}
391  return(result);}
392 
394 register context *ctx;
395 int n;
396 register pointer *argv;
397 {
398  register eusfloat_t *fv1,*fv2,*rfv;
399  register pointer result;
400  register int s;
401 
402  ckarg2(2,3);
403  if (!isfltvector(argv[0]) || !isfltvector(argv[1])) error(E_FLOATVECTOR);
404  s=ckvsize(argv[0],argv[1]);
405  if (s!=3) error(E_VECINDEX);
406  if (n==3) {
407  result=argv[2];
408  if (!isfltvector(result)) error(E_FLOATVECTOR);
409  if (vecsize(result)!=3) error(E_VECINDEX);
410  }
411  else result=makefvector(3);
412  rfv=result->c.fvec.fv;
413  fv1= argv[0]->c.fvec.fv; fv2= argv[1]->c.fvec.fv;
414  rfv[0]=fv1[1] * fv2[2] - fv1[2] * fv2[1];
415  rfv[1]=fv1[2] * fv2[0] - fv1[0] * fv2[2];
416  rfv[2]=fv1[0] * fv2[1] - fv1[1] * fv2[0];
417  return(result);}
418 
419 pointer SCA3PROD(ctx,n,argv) /*scalar tripple product [A,B,C] */
420 register context *ctx;
421 int n;
422 pointer argv[];
423 {
424  register eusfloat_t *va,*vb,*vc;
425  eusfloat_t val;
426  numunion nu;
427  ckarg(3);
428  if (!isfltvector(argv[0]) || !isfltvector(argv[1]) || !isfltvector(argv[2]))
430  if (vecsize(argv[0])!=3 || vecsize(argv[1])!=3 || vecsize(argv[2])!=3) error(E_VECINDEX);
431  va=argv[0]->c.fvec.fv; vb=argv[1]->c.fvec.fv; vc=argv[2]->c.fvec.fv;
432  val =va[0] * vb[1] * vc[2];
433  val+=va[2] * vb[0] * vc[1];
434  val+=va[1] * vb[2] * vc[0];
435  val-=va[2] * vb[1] * vc[0];
436  val-=va[0] * vb[2] * vc[1];
437  val-=va[1] * vb[0] * vc[2];
438  return(makeflt(val));}
439 
440 pointer SCALEVEC(ctx,n,argv)
441 register context *ctx;
442 int n;
443 pointer argv[];
444 {
445  eusfloat_t scale;
446  pointer result;
447  register eusfloat_t *a,*r;
448  register int s,i;
449  register int isf,isi;
450  numunion nu;
451 
452  ckarg2(2,3);
453  scale=ckfltval(argv[0]);
454  if (!(isf=isfltvector(argv[1])) && !(isi=isintvector(argv[1])))
456  s=vecsize(argv[1]);
457  if (n==3) {
458  result=argv[2];
459  if (!isvector(result)) error(E_NOVECTOR);
460  if (elmtypeof(result)!=elmtypeof(argv[1])) error(E_TYPEMISMATCH);
461  if (s!=vecsize(result)) error(E_VECINDEX);}
462  else result=makevector(classof(argv[1]), s);
463  if (isf) {
464  a=argv[1]->c.fvec.fv;
465  r=result->c.fvec.fv;
466  for (i=0; i<s; i++) r[i]=scale*(a[i]);
467  return(result);}
468  else if (isi) {
469  register eusinteger_t *ia, *ir;
470  ia=argv[1]->c.ivec.iv;
471  ir=result->c.ivec.iv;
472  for (i=0; i<s; i++) ir[i]=scale*(ia[i]);
473  return(result);}
474  else error(E_NOSEQ);}
475 
476 pointer MIDPOINT(ctx,n,argv)
477 register context *ctx;
478 int n;
479 register pointer argv[];
480 { int i,vsize;
481  double ratio, ratio2;
482  pointer p1,p2,result;
483  register int isf,isi;
484  numunion nu;
485 
486  ckarg2(3,4);
487  ratio=ckfltval(argv[0]); ratio2=1.0-ratio;
488  p1=argv[1]; p2=argv[2];
489  if (!((isf=isfltvector(p1))&&isfltvector(p2)) &&
490  !((isi=isintvector(p1))&&isintvector(p2)))
492  vsize=ckvsize(p1,p2);
493  if (n==4) {
494  result=argv[3];
495  if (!(isf&&isfltvector(result))&&!(isi&&isintvector(result)))
497  if (vsize!=vecsize(result)) error(E_VECINDEX);}
498  else result=makevector(classof(p1), vsize);
499  if (isf) {
500  register eusfloat_t *r,*pp1,*pp2;
501  r=result->c.fvec.fv; pp1=p1->c.fvec.fv; pp2=p2->c.fvec.fv;
502  for (i=0; i<vsize; i++)
503  r[i]=pp1[i]*ratio2 + pp2[i]*ratio;
504  return(result);}
505  else if (isi) {
506  register eusinteger_t *r,*pp1,*pp2;
507  r=result->c.ivec.iv; pp1=p1->c.ivec.iv; pp2=p2->c.ivec.iv;
508  for (i=0; i<vsize; i++)
509  r[i]=pp1[i]*ratio2 + pp2[i]*ratio;
510  return(result);}
511  else error(E_FLOATVECTOR);}
512 
513 /*
514 pointer TRIANGLE(ctx,n,argv)
515 register context *ctx;
516 int n;
517 register pointer argv[];
518 { register float *v1,*v2,*v3,tr;
519  ckarg(3);
520  if (!isfltvector(argv[0])) error(E_FLOATVECTOR);
521  if (!isfltvector(argv[1])) error(E_FLOATVECTOR);
522  if (!isfltvector(argv[2])) error(E_FLOATVECTOR);
523  v1=argv[0]->c.fvec.fv;
524  v2=argv[1]->c.fvec.fv;
525  v3=argv[2]->c.fvec.fv;
526  tr=v1[0]*v2[1] + v2[0]*v3[1] + v3[0]*v1[1]
527  - v3[0]*v2[1] - v2[0]*v1[1] - v1[0]*v3[1];
528  return(makeflt(tr));}
529 */
530 
531 pointer MKFLTVEC(ctx,n,argv)
532 register context *ctx;
533 int n;
534 pointer argv[];
535 { register pointer r;
536  register int i;
537  register eusfloat_t *fv;
538  numunion nu;
539  r=makefvector(n);
540  fv=r->c.fvec.fv;
541  for (i=0; i<n; i++) {
542  //fv[i]=ckfltval(argv[i]);
543  if ( isflt(argv[i]) ) fv[i] = fltval(argv[i]);
544  else if ( isint(argv[i]) ) fv[i] = intval(argv[i]);
545  else if ( pisbignum(argv[i]) ) fv[i] = big_to_float(argv[i]);
546  else if ( pisratio(argv[i]) ) fv[i] = ratio2flt(argv[i]);
547  else if ( issymbol(argv[i]) ) {
548  char *sym = get_string(argv[i]);
549  if ( strcmp(sym, "NAN") == 0 ) fv[i] = NAN;
550  else if ( strcmp(sym, "-NAN") == 0 ) fv[i] = -NAN;
551  else if ( strcmp(sym, "INF") == 0 ) fv[i] = INFINITY;
552  else if ( strcmp(sym, "-INF") == 0 ) fv[i] = -INFINITY;
553  else { (eusinteger_t)error(E_NONUMBER); }
554  }
555  else { (eusinteger_t)error(E_NONUMBER); }
556  }
557 
558  return(r);
559  }
560 
561 /* comparation of vectors */
562 
563 pointer VLESSP(ctx,n,argv)
564 register context *ctx;
565 int n;
566 pointer argv[];
567 { register int i,s;
568  register pointer a,b;
569  register int isf,isi;
570  ckarg(2);
571  a=argv[0]; b=argv[1];
572  if (!((isf=isfltvector(a))&&isfltvector(b)) &&
573  !((isi=isintvector(a))&&isintvector(b))) error(E_FLOATVECTOR);
574  s=ckvsize(a,b);
575  if (isf) {
576  register eusfloat_t *av=a->c.fvec.fv, *bv=b->c.fvec.fv;
577  for (i=0; i<s; i++) if (av[i] > bv[i]) return(NIL);
578  return(T);}
579  else if (isi) {
580  register eusinteger_t *av=a->c.ivec.iv, *bv=b->c.ivec.iv;
581  for (i=0; i<s; i++) if (av[i] > bv[i]) return(NIL);
582  return(T);}
583  else error(E_FLOATVECTOR);}
584 
585 pointer VGREATERP(ctx,n,argv)
586 register context *ctx;
587 int n;
588 pointer argv[];
589 /*every element of argv[0] is greater than corresponding elem of arg[1]*/
590 { register int i,s;
591  register pointer a,b;
592  register int isf,isi;
593  ckarg(2);
594  a=argv[0]; b=argv[1];
595  if (!((isf=isfltvector(a))&&isfltvector(b)) &&
596  !((isi=isintvector(a))&&isintvector(b))) error(E_FLOATVECTOR);
597  s=ckvsize(a,b);
598  if (isf) {
599  register eusfloat_t *av=a->c.fvec.fv, *bv=b->c.fvec.fv;
600  for (i=0; i<s; i++) if (av[i] < bv[i]) return(NIL);
601  return(T);}
602  else if (isi) {
603  register eusinteger_t *av=a->c.ivec.iv, *bv=b->c.ivec.iv;
604  for (i=0; i<s; i++) if (av[i] < bv[i]) return(NIL);
605  return(T);}
606  else error(E_FLOATVECTOR);}
607 
609 register context *ctx;
610 int n;
611 pointer argv[];
612 {
613  eusfloat_t dx,dy,dz,err=0.0,diameter;
614  register int i;
615  register pointer a;
616  pointer v;
617  register eusfloat_t *f;
618  register pointer vmin,vmax;
619  numunion nu;
620 
621  ckarg2(3,4);
622  a=argv[0];
623  if (!islist(a)) error(E_NOLIST);
624  v=ccar(a); a=ccdr(a);
625  vmin=argv[1];
626  if (!isfltvector(vmin)) error(E_FLOATVECTOR);
627  vmax=argv[2];
628  if (!isfltvector(vmax)) error(E_FLOATVECTOR);
629  if (n==4) err=ckfltval(argv[3]);
630  for (i=0; i<3; i++) vmin->c.fvec.fv[i]=vmax->c.fvec.fv[i]=v->c.fvec.fv[i];
631  while (islist(a)) {
632  v=ccar(a); a=ccdr(a);
633  if (!isfltvector(v)) error(E_FLOATVECTOR);
634  f=v->c.fvec.fv;
635  if (f[0]<vmin->c.fvec.fv[0]) vmin->c.fvec.fv[0]=f[0];
636  else if (f[0]>vmax->c.fvec.fv[0]) vmax->c.fvec.fv[0]=f[0];
637  if (f[1]<vmin->c.fvec.fv[1]) vmin->c.fvec.fv[1]=f[1];
638  else if (f[1]>vmax->c.fvec.fv[1]) vmax->c.fvec.fv[1]=f[1];
639  if (f[2]<vmin->c.fvec.fv[2]) vmin->c.fvec.fv[2]=f[2];
640  else if (f[2]>vmax->c.fvec.fv[2]) vmax->c.fvec.fv[2]=f[2];}
641  dx= vmax->c.fvec.fv[0] - vmin->c.fvec.fv[0];
642  dy= vmax->c.fvec.fv[1] - vmin->c.fvec.fv[1];
643  dz= vmax->c.fvec.fv[2] - vmin->c.fvec.fv[2];
644  diameter=sqrt(dx*dx + dy*dy + dz*dz);
645  if (err!=0.0) {
646  vmin->c.fvec.fv[0]-= diameter*err;
647  vmin->c.fvec.fv[1]-= diameter*err;
648  vmin->c.fvec.fv[2]-= diameter*err;
649  vmax->c.fvec.fv[0]+= diameter*err;
650  vmax->c.fvec.fv[1]+= diameter*err;
651  vmax->c.fvec.fv[2]+= diameter*err; }
652  return(makeflt(diameter));}
653 
654 pointer VMIN(ctx,n,argv)
655 register context *ctx;
656 int n;
657 pointer argv[];
658 { register int i,j,s;
659  pointer r;
660  register pointer v;
661  register eusfloat_t *vf,*rf;
662  register eusinteger_t *irf, *ivf;
663  register int isf,isi;
664 
665  v=argv[0];
666  if (!(isf=isfltvector(v)) && !(isi=isintvector(v))) error(E_FLOATVECTOR);
667  s=vecsize(v);
668  r=makevector(classof(v),s);
669  rf=r->c.fvec.fv;
670  for (i=0; i<s; i++) r->c.fvec.fv[i]=v->c.fvec.fv[i];
671  if (isf) {
672  for (i=1; i<n; i++) {
673  v=argv[i];
674  if (!isfltvector(v)) error(E_FLOATVECTOR);
675  if (vecsize(v)!=s) error(E_VECINDEX);
676  vf=v->c.fvec.fv;
677  for (j=0; j<s; j++) if (vf[j]<rf[j]) rf[j]=vf[j]; } }
678  else if (isi) {
679  irf=r->c.ivec.iv;
680  for (i=1; i<n; i++) {
681  v=argv[i];
682  if (!isintvector(v)) error(E_NOINTVECTOR);
683  if (vecsize(v)!=s) error(E_VECINDEX);
684  ivf=v->c.ivec.iv;
685  for (j=0; j<s; j++) if (ivf[j]<irf[j]) irf[j]=ivf[j]; } }
686  else error(E_NOSEQ);
687  return(r);}
688 
689 pointer VMAX(ctx,n,argv)
690 register context *ctx;
691 int n;
692 pointer argv[];
693 { register int i,j,s;
694  pointer r;
695  register pointer v;
696  register eusfloat_t *vf,*rf;
697  register eusinteger_t *irf, *ivf;
698  register int isf,isi;
699 
700  v=argv[0];
701  if (!(isf=isfltvector(v)) && !(isi=isintvector(v))) error(E_FLOATVECTOR);
702  s=vecsize(v);
703  r=makevector(classof(v),s);
704  rf=r->c.fvec.fv;
705  for (i=0; i<s; i++) r->c.fvec.fv[i]=v->c.fvec.fv[i];
706  if (isf) {
707  for (i=1; i<n; i++) {
708  v=argv[i];
709  if (!isfltvector(v)) error(E_FLOATVECTOR);
710  if (vecsize(v)!=s) error(E_VECINDEX);
711  vf=v->c.fvec.fv;
712  for (j=0; j<s; j++) if (vf[j]>rf[j]) rf[j]=vf[j]; } }
713  else if (isi) {
714  irf=r->c.ivec.iv;
715  for (i=1; i<n; i++) {
716  v=argv[i];
717  if (!isintvector(v)) error(E_NOINTVECTOR);
718  if (vecsize(v)!=s) error(E_VECINDEX);
719  ivf=v->c.ivec.iv;
720  for (j=0; j<s; j++) if (ivf[j]>irf[j]) irf[j]=ivf[j]; } }
721  else error(E_NOSEQ);
722  return(r);}
723 
724 
725 /****************************************************************/
726 /* (m* m1 m2 [result])
727 /* (transform mat vector [result]) ; mat*vec
728 /* (transform vector mat [result]) ; vec*mat
729 /****************************************************************/
730 
731 #define ismatrix(p) ((isarray(p) && \
732  p->c.ary.rank==makeint(2) && \
733  elmtypeof(p->c.ary.entity)==ELM_FLOAT))
734 #define rowsize(p) (intval(p->c.ary.dim[0]))
735 #define colsize(p) (intval(p->c.ary.dim[1]))
736 
737 /*
738 float scaprod(v1,v2,s)
739 pointer v1,v2;
740 registerint s;
741 { float sum=0;
742  register int i;
743  for (i=0; i<s; i++) sum+= v1->c.fvec.fv[i] * v2->c.fvec.fv[i];
744  return(sum);}
745 
746 float vtrans(mat,n,vec,s)
747 pointer mat,vec;
748 int n,s;
749 { pointer m;
750  register int i;
751  float f=0.0;
752  for (i=0; i<s; i++)
753  f+ = mat->c.vec.v[i]->c.fvec.fv[n] * vec->c.fvec.fv[i];
754  return(f);}
755 */
756 
757 pointer MATTIMES(ctx,n,argv)
758 register context *ctx;
759 int n;
760 register pointer argv[];
761 { pointer rm;
762  register int k,i,j,ii,jj,row1,column1,row2,column2;
763  register eusfloat_t *fm1,*fm2,*fm;
764  eusfloat_t *fv,x,fvv[256];
765  fv = fvv;
766 
767  ckarg2(2,3);
768  if (!ismatrix(argv[0]) || !ismatrix(argv[1])) error(E_NOVECTOR);
769  fm1=argv[0]->c.ary.entity->c.fvec.fv;
770  fm2=argv[1]->c.ary.entity->c.fvec.fv;
771  row1=rowsize(argv[0]); row2=rowsize(argv[1]);
772  column1=colsize(argv[0]); column2=colsize(argv[1]);
773  if (column1!=row2) error(E_VECINDEX);
774  if (n==3) {
775  rm=argv[2];
776  if (!ismatrix(rm)) error(E_NOVECTOR);
777  if (row1!=rowsize(rm) || column2!=colsize(rm)) error(E_VECINDEX);
778  }
779  else rm=makematrix(ctx,row1,column2);
780  if (row1>256 || column2>256){
781  fv = (eusfloat_t *)malloc(sizeof(eusfloat_t) * ((row1>column2)?row1:column2));
782  //error(E_VECINDEX);
783  }
784  fm=rm->c.ary.entity->c.fvec.fv;
785  if (fm2!=fm)
786  for (i=0; i<row1; i++) {
787  ii=i*column1;
788  for (j=0; j<column2; j++) {
789  x=0.0; jj=j;
790  for (k=0; k<column1; k++) {
791  x += fm1[ii+k] * fm2[jj];
792  jj+=column2;}
793  fv[j]=x;}
794  ii=i*column2;
795  for (j=0; j<column2; j++) fm[ii+j]=fv[j];}
796  else
797  for (i=0; i<column2; i++) {
798  for (j=0; j<row1; j++) {
799  x=0.0; jj=j*column1; ii=0;
800  for (k=0; k<column1; k++) {
801  x += fm1[jj+k] * fm2[i+ii];
802  ii+=column2;}
803  fv[j]=x;}
804  jj=0;
805  for (j=0; j<row1; j++, jj+=column2) fm[i+jj]=fv[j];}
806  if (fv!=fvv) free(fv);
807  return(rm);}
808 
809 pointer TRANSFORM(ctx,n,argv)
810 register context *ctx;
811 int n;
812 register pointer argv[];
813 { register pointer result;
814  register eusfloat_t *m,*v,x;
815  eusfloat_t *fv,fvv[256];
816  register int i,j,ii,s,s2;
817  fv = fvv;
818 
819  ckarg2(2,3);
820  if (ismatrix(argv[0]) && isfltvector(argv[1])) {
821  m=argv[0]->c.ary.entity->c.fvec.fv;
822  v=argv[1]->c.fvec.fv;
823  s=colsize(argv[0]); s2=rowsize(argv[0]);
824  if (s!=vecsize(argv[1])) error(E_VECINDEX); }
825  else if (isfltvector(argv[0]) && ismatrix(argv[1])) {
826  v=argv[0]->c.fvec.fv;
827  m=argv[1]->c.ary.entity->c.fvec.fv;
828  s2=colsize(argv[1]); s=rowsize(argv[1]);
829  if (s!=vecsize(argv[0])) error(E_VECINDEX); }
830  else error(E_NOVECTOR);
831 
832  if (n==3) {
833  result=argv[2];
834  if (!isfltvector(result)) error(E_NOVECTOR);
835  if (s2!=vecsize(result)) error(E_VECINDEX);}
836  else result=makefvector(s2);
837  if (s2>256){
838  fv = (eusfloat_t *)malloc(sizeof(eusfloat_t) * s2);
839  // error(E_VECINDEX);
840  }
841 
842  if (isfltvector(argv[0])) { /* vec*mat */
843  for (i=0; i<s2; i++) {
844  x=0.0; ii=i;
845  for (j=0; j<s; j++) { x+= m[ii] * v[j]; ii+=s2;}
846  fv[i]=x;}}
847  else { /* mat*vec */
848  for (i=0; i<s2; i++) {
849  x=0.0; ii=i*s;
850  for (j=0; j<s; j++) x+=m[ii+j]*v[j];
851  fv[i]=x;} }
852  for (i=0; i<s2; i++) result->c.fvec.fv[i]=fv[i];
853  if (s2>256) free(fv);
854  return(result);
855  }
856 
857 /****************************************************************/
858 /* rotate vector
859 /* (rotate-vector fltvec theta :axis [result])
860 /* theta : radian
861 /* axis : one of :x,:y or :z or 0,1,2
862 /****************************************************************/
863 
864 pointer ROTVEC(ctx,n,argv)
865 register context *ctx;
866 int n;
867 pointer argv[];
868 { register pointer vec,result,a;
869  double theta,c,s1,s2;
870  eusfloat_t f1,f2;
871  register int k1,k2,k3,size;
872  numunion nu;
873 
874  ckarg2(3,4);
875  vec=argv[0];
876  if (!isfltvector(vec)) error(E_FLOATVECTOR);
877  size=vecsize(vec);
878  theta=ckfltval(argv[1]);
879  c=cos(theta); s1=sin(theta);
880  a=argv[2];
881  if (a==K_X || a==makeint(0)) { k1=1; k2=2; k3=0; s2=s1; s1= -s1;}
882  else if (a==K_Y || a==makeint(1)) { k1=0; k2=2; k3=1; s2= -s1;}
883  else if (a==K_Z || a==makeint(2)) { k1=0; k2=1; k3=2; s2=s1; s1= -s1;}
884  else if (a==NIL) { k1=0; k2=1; s2=s1; s1= -s1;}
885  else error(E_ROTAXIS);
886  if (n==4) {
887  result=argv[3];
888  if (!isfltvector(result)) error(E_FLOATVECTOR); }
889  else result=makefvector(size);
890  f1=vec->c.fvec.fv[k1];
891  f2=vec->c.fvec.fv[k2];
892  result->c.fvec.fv[k1]=c*f1+s1*f2;
893  result->c.fvec.fv[k2]=s2*f1+c*f2;
894  if (a!=NIL) result->c.fvec.fv[k3]=vec->c.fvec.fv[k3];
895  return(result);}
896 
897 /****************************************************************/
898 /* rotate matrix
899 /* (rotate-matrix mat theta :axis [worldp] [result])
900 /* theta : radian
901 /* axis : one of :x,:y or :z or 0,1,2
902 /* worldp: nil--local(mult rot mat from the right)
903 /* t----world(mult rot mat from the left)
904 /****************************************************************/
905 static void copymat(dest,src,size)
906 pointer dest,src;
907 register int size;
908 { register int i;
909  register eusfloat_t *rv=dest->c.ary.entity->c.fvec.fv;
910  register eusfloat_t *mv=src->c.ary.entity->c.fvec.fv;
911  size=size*size;
912  for (i=0; i<size; i++) rv[i]=mv[i]; }
913 
914 pointer ROTMAT(ctx,n,argv)
915 register context *ctx;
916 int n; /* rot theata axis worldp result */
917 pointer argv[];
918 { register pointer mat,result,a;
919  double theta,c,s1,s2;
920  eusfloat_t *m1,*m2,f1,f2;
921  register eusfloat_t *rm,*m;
922  register int i,size,k1,k2,worldp=0,ss;
923  numunion nu;
924 
925  ckarg2(3,5);
926  mat=argv[0];
927  if (!ismatrix(mat)) error(E_NOVECTOR);
928  size=colsize(mat);
929  theta=ckfltval(argv[1]);
930  c=cos(theta); s1=sin(theta);
931 
932  a=argv[2];
933  if (n>=4) worldp=(argv[3]!=NIL); /*0:local 1:world*/
934 
935  if (n==5) {
936  result=argv[4];
937  if (!ismatrix(result)) error(E_NOVECTOR); }
938  else result=makematrix(ctx,size,size);
939  rm=result->c.ary.entity->c.fvec.fv;
940  m=mat->c.ary.entity->c.fvec.fv;
941 
942  if (a==K_X || a==makeint(0)) {
943  k1=1; k2=2;
944  if (worldp) { s2=s1; s1= -s1;}
945  else s2= -s1;}
946  else if (a==MK_X) {
947  k1=1; k2=2;
948  if (worldp) s2= -s1;
949  else { s2=s1; s1= -s1;}}
950  else if (a==K_Y || a==makeint(1)) {
951  k1=0; k2=2;
952  if (worldp) s2= -s1;
953  else { s2=s1; s1= -s1;}}
954  else if (a==MK_Y) {
955  k1=0; k2=2;
956  if (worldp) { s2=s1; s1= -s1;}
957  else s2= -s1;}
958  else if (a==K_Z || a==makeint(2)) {
959  k1=0; k2=1;
960  if (worldp) { s2=s1; s1= -s1;}
961  else s2= -s1;}
962  else if (a==MK_Z) {
963  k1=0; k2=1;
964  if (worldp) s2= -s1;
965  else { s2=s1; s1= -s1;}}
966  else if (a==NIL) { /* 2dimensional m.i */
967  eusfloat_t m0,m1,m2,m3;
968  m0=c*m[0]-s1*m[2]; m1=c*m[1]-s1*m[3];
969  m2=s1*m[0]+c*m[2]; m3=s1*m[1]+c*m[3];
970  rm[0]=m0; rm[1]=m1; rm[2]=m2; rm[3]=m3;
971  return(result); }
972  else error(E_ROTAXIS);
973 
974  ss=size*size;
975  if (mat!=result) for (i=0; i<ss; i++) rm[i]=m[i];
976  for (i=0; i<size; i++) {
977  if (worldp) {
978  m1= &rm[k1*size+i];
979  m2= &rm[k2*size+i];}
980  else {
981  m1= &rm[i*size+k1];
982  m2= &rm[i*size+k2];}
983  f1= (*m1)*c + (*m2)*s1;
984  f2= (*m1)*s2 + (*m2)*c;
985  *m1=f1; *m2=f2;}
986  return(result);}
987 
988 /* (rotation theta floatvector [result])
989 /* make rotation matrix along arbitrary axis
990 /* Axis is given by floatvector, it need not to be normalized.
991 */
993 register context *ctx;
994 int n;
995 pointer *argv;
996 { register pointer a,result;
997  register eusfloat_t *rm;
998  register int size;
999  double x, y, z, s;
1000  double cs, sn, vers, xv, yv,zv, xyv, yzv, zxv, xs, ys, zs, norm;
1001  numunion nu;
1002 
1003  ckarg2(1,3);
1004  s=ckfltval(argv[0]);
1005  cs = cos(s); sn = sin(s); vers = 1.0 - cs;
1006  a=argv[1];
1007  if (n==1 || (n==2 && ismatrix(a))) { /*2D rot. matrix*/
1008  if (n==2) result=a;
1009  else result=makematrix(ctx,2,2);
1010  rm=result->c.ary.entity->c.fvec.fv;
1011  rm[0]=rm[3]=cs; rm[1]=-sn; rm[2]=sn;
1012  return(result);}
1013  if (a==NIL) size=2; else size=3;
1014  if (n==3) { result=argv[2]; if (!ismatrix(result)) error(E_NOVECTOR);}
1015  else result=makematrix(ctx,size,size);
1016  rm=result->c.ary.entity->c.fvec.fv;
1017  if (isfltvector(a)) {
1018  if (size<=2) error(E_VECINDEX);
1019  x=a->c.fvec.fv[0]; y=a->c.fvec.fv[1]; z=a->c.fvec.fv[2];
1020  norm = sqrt(x*x + y*y + z*z);
1021  if (fabs(norm)<0.00001) return(NIL); /*error(E_USER,(pointer)"too small axis vector");*/
1022  x = x/norm; y = y/norm; z= z/norm;
1023  xv = x*x*vers; yv = y*y*vers; zv = z*z*vers;
1024  xyv = x*y*vers; yzv = y*z*vers; zxv = z*x*vers;
1025  xs = x*sn; ys = y*sn; zs = z*sn;
1026  rm[0] = xv + cs;
1027  rm[1] = xyv - zs;
1028  rm[2] = zxv + ys;
1029  rm[size] = xyv + zs;
1030  rm[size+1] = yv + cs;
1031  rm[size+2] = yzv - xs;
1032  rm[size+size] = zxv - ys;
1033  rm[size+size+1] = yzv + xs;
1034  rm[size+size+2] = zv + cs;
1035  return(result);}
1036  else {
1037  if (size==2) {
1038  rm[0]=rm[3]=cs; rm[1]=-sn; rm[2]=sn;
1039  return(result);}
1040  for (size=0; size<9; size++) rm[size]=0.0;
1041  if (a==makeint(0) || a==K_X) {
1042  rm[0]=1.0; rm[4]=cs; rm[5]= -sn; rm[7]=sn; rm[8]=cs;}
1043  else if (a==MK_X) {
1044  rm[0]=1.0; rm[4]=cs; rm[5]= sn; rm[7]=-sn; rm[8]=cs;}
1045  else if (a==makeint(1) || a==K_Y) {
1046  rm[0]=cs; rm[2]=sn; rm[4]=1.0; rm[6]= -sn; rm[8]=cs;}
1047  else if (a==MK_Y) {
1048  rm[0]=cs; rm[2]=-sn; rm[4]=1.0; rm[6]= sn; rm[8]=cs;}
1049  else if (a==makeint(2) || a==K_Z) {
1050  rm[0]=cs; rm[1]= -sn; rm[3]=sn; rm[4]=cs; rm[8]=1.0;}
1051  else if (a==MK_Z) {
1052  rm[0]=cs; rm[1]= sn; rm[3]=-sn; rm[4]=cs; rm[8]=1.0;}
1053  else error(E_NOVECTOR);
1054  return(result);} }
1055 
1056 pointer ROTANGLE(ctx,n,argv) /*equivalent rotation axis and angle*/
1057 register context *ctx;
1058 int n;
1059 pointer argv[];
1060 { pointer r=argv[0];
1061  int size;
1062  eusfloat_t *m,*krv,kx,ky,kz;
1063  eusfloat_t th,t1,t2,t3;
1064  eusfloat_t cs,cs2,vers,sn2,norm;
1065  pointer kr;
1066  numunion nu;
1067 
1068  ckarg(1);
1069  if (!ismatrix(r)) error(E_NOVECTOR);
1070  size=colsize(r);
1071  m=r->c.ary.entity->c.fvec.fv;
1072 
1073  if (size==2) { /*2D rotation*/
1074  th=atan2(m[2],m[0]);
1075  return(makeflt(th));}
1076 
1077  kr=makefvector(3); /*axis vector*/
1078  krv=kr->c.fvec.fv;
1079  t1=m[size+size+1]-m[size+2];
1080  t2=m[2] -m[size+size];
1081  t3=m[size] -m[1];
1082  cs2=m[0]+m[size+1]+m[size+size+2]-1.0;
1083  cs=cs2/2;
1084  vers=1-cs;
1085  sn2=sqrt(t1*t1 + t2*t2 + t3*t3);
1086  th=atan2(sn2,cs2);
1087  if (th<1e-10||vers<1e-10) return(NIL);
1088  else if (th<2.6) {
1089  kx=(m[size+size+1]-m[size+2])/sn2;
1090  ky=(m[2]-m[size+size])/sn2;
1091  kz=(m[size]-m[1])/sn2;
1092  }
1093  else {
1094  kx=sqrt((m[0]-cs)/vers);
1095  if (m[size+size+1]-m[size+2]<0) kx= -kx;
1096  ky=sqrt((m[size+1]-cs)/vers);
1097  if (m[2]-m[size+size]<0) ky= -ky;
1098  kz=sqrt((m[size+size+2]-cs)/vers);
1099  if (m[size]-m[1]<0) kz= -kz;
1100  }
1101 
1102  if (debug)
1103  printf("rotation-angle1: %f %f %f\n",kx,ky,kz);
1104  if ((fabs(kx) > fabs(ky)) && (fabs(kx) > fabs(kz))) {
1105  ky=(m[size]+m[1])/(2*kx*vers); kz=(m[2]+m[size+size])/(2*kx*vers);
1106  norm=sqrt((ky*ky+kz*kz)/(1.0-kx*kx)); if (!isnan(norm)) {ky/=norm; kz/=norm;}}
1107  else if ((fabs(ky) > fabs(kx)) && (fabs(ky) > fabs(kz))) {
1108  kx=(m[size]+m[1])/(2*ky*vers); kz=(m[size+2]+m[size+size+1])/(2*ky*vers);
1109  norm=sqrt((kx*kx+kz*kz)/(1.0-ky*ky)); if (!isnan(norm)) {kx/=norm; kz/=norm;}}
1110  else {
1111  kx=(m[2]+m[size+size])/(2*kz*vers);
1112  ky=(m[size+2]+m[size+size+1])/(2*kz*vers);
1113  norm=sqrt((kx*kx+ky*ky)/(1.0-kz*kz)); if (!isnan(norm)) {kx/=norm; ky/=norm;}}
1114 
1115  norm=sqrt(kx*kx + ky*ky + kz*kz);
1116  if (debug)
1117  printf("rotation-angle2: %f %f %f norm=%f\n",kx,ky,kz,norm);
1118  krv[0]=kx/norm; krv[1]=ky/norm; krv[2]=kz/norm;
1119  kx=kx/norm; ky=ky/norm; kz=kz/norm;
1120  norm=sqrt(kx*kx + ky*ky + kz*kz);
1121  return(cons(ctx,makeflt(th),cons(ctx,kr,NIL)));}
1122 
1123 
1124 /* (transpose mat [result])
1125 */
1127 register context *ctx;
1128 int n;
1129 pointer argv[];
1130 { register eusfloat_t *rm,*m,x;
1131  pointer mat,result;
1132  register int i,j,row,column;
1133 
1134  ckarg2(1,2);
1135  mat=argv[0];
1136  if (!ismatrix(mat)) error(E_NOVECTOR);
1137  column=colsize(mat); row=rowsize(mat);
1138  if (n==2) {
1139  result=argv[1];
1140  if (!ismatrix(result)) error(E_NOVECTOR);
1141  if (rowsize(result)!=column || colsize(result)!=row) error(E_VECINDEX);}
1142  else result=makematrix(ctx,column,row);
1143  rm=result->c.ary.entity->c.fvec.fv;
1144  m=mat->c.ary.entity->c.fvec.fv;
1145  if (result==mat) /*square matrix*/
1146  for (i=0; i<row; i++)
1147  for (j=0; j<=i; j++) {
1148  x=m[j*column+i];
1149  rm[j*column+i]=m[column*i+j];
1150  rm[column*i+j]=x;}
1151  else
1152  for (i=0; i<row; i++)
1153  for (j=0; j<column; j++) rm[j*row+i]=m[i*column+j];
1154  return(result);}
1155 
1156 /* get roll, pitch yaw angles out of rotation matrix*/
1157 pointer INV_RPY(ctx,n,argv)
1158 register context *ctx;
1159 int n;
1160 pointer argv[];
1161 { pointer mat;
1162  eusfloat_t a, b, c, sa, ca;
1163  register eusfloat_t *mv;
1164  pointer result,result2;
1165  numunion nu;
1166 
1167  ckarg(1);
1168  mat=argv[0];
1169  if (!ismatrix(mat)) error(E_NOVECTOR);
1170  if (colsize(mat)!=3) error(E_VECINDEX);
1171  mv=mat->c.ary.entity->c.fvec.fv;
1172  a = atan2(mv[3],mv[0]);
1173  sa = sin(a); ca = cos(a);
1174  b = atan2(-mv[6], ca*mv[0] + sa*mv[3]);
1175  c = atan2(sa*mv[2] - ca*mv[5], -sa*mv[1] + ca*mv[4]);
1176  result = cons(ctx,makeflt(c), NIL);
1177  result = cons(ctx,makeflt(b), result);
1178  result = cons(ctx,makeflt(a), result);
1179  a=a + M_PI;
1180  sa = sin(a); ca = cos(a);
1181  b = atan2(-mv[6], ca*mv[0] + sa*mv[3]);
1182  c = atan2(sa*mv[2] - ca*mv[5], -sa*mv[1] + ca*mv[4]);
1183  result2 = cons(ctx,makeflt(c), NIL);
1184  result2 = cons(ctx,makeflt(b), result2);
1185  result2 = cons(ctx,makeflt(a), result2);
1186  result2 = cons(ctx,result2,NIL);
1187  return(cons(ctx,result,result2));}
1188 
1190 register context *ctx;
1191 int n;
1192 pointer argv[];
1193 { pointer mat;
1194  eusfloat_t a, b, c, sa, ca;
1195  register eusfloat_t *mv;
1196  pointer result,result2;
1197  numunion nu;
1198 
1199  ckarg(1);
1200  mat=argv[0];
1201  if (!ismatrix(mat)) error(E_NOVECTOR);
1202  if (colsize(mat)!=3) error(E_VECINDEX);
1203  mv=mat->c.ary.entity->c.fvec.fv;
1204  if (-0.00001<mv[5] && mv[5]<0.00001 && -0.00001<mv[2] && mv[2]<0.00001) a=0.0;
1205  else a = atan2(mv[5],mv[2]);
1206  sa = sin(a); ca = cos(a);
1207  b = atan2(ca*mv[2]+sa*mv[5],mv[8]);
1208  c = atan2(-sa*mv[0]+ca*mv[3], -sa*mv[1] + ca*mv[4]);
1209  result = cons(ctx,makeflt(c), NIL);
1210  result = cons(ctx,makeflt(b), result);
1211  result = cons(ctx,makeflt(a), result);
1212  a=a + M_PI;
1213  sa = sin(a); ca = cos(a);
1214  b = atan2(ca*mv[2]+sa*mv[5],mv[8]);
1215  c = atan2(-sa*mv[0]+ca*mv[3], -sa*mv[1] + ca*mv[4]);
1216  result2 = cons(ctx,makeflt(c), NIL);
1217  result2 = cons(ctx,makeflt(b), result2);
1218  result2 = cons(ctx,makeflt(a), result2);
1219  result2 = cons(ctx,result2,NIL);
1220  return(cons(ctx,result,result2));}
1221 
1222 /****************************************************************/
1223 /*
1224 /* simultaneous equation solver by LU decomposition method
1225 /*
1226 /* 1987-July-1
1227 /* Copyright Toshihiro MATSUI
1228 /*
1229 /****************************************************************/
1230 
1231 #define elm(a,i,j) (a->c.vec.v[j]->c.fvec.fv[i])
1232 #define EPS (1.0E-10)
1233 
1234 static int decompose(a,s,p)
1235 register eusfloat_t *a;
1236 register int s;
1237 eusinteger_t p[];
1238 {
1239  register int i,j,k,l;
1240  eusfloat_t al,bl;
1241 
1242  for (i=0; i<s; i++) p[i]=i;
1243  for (k=0; k<s; k++) {
1244  /*select pivot*/
1245  l=k;
1246  al=fabs(a[p[l]*s+k]);
1247  for (i=k+1; i<s; i++)
1248  if ((bl=fabs(a[p[i]*s+k])) > al) { l=i; al=bl;}
1249  if (l!=k) {
1250  /*change rows*/
1251  j=p[k]; p[k]=p[l]; p[l]=j; }
1252  if (al<EPS) return(-1); /* singular*/
1253  /* gauss elimination */
1254  a[p[k]*s+k]= 1.0/a[p[k]*s+k];
1255  for (i=k+1; i<s; i++) {
1256  al=a[p[i]*s+k]=a[p[i]*s+k]*a[p[k]*s+k];
1257  for (j=k+1; j<s; j++) a[p[i]*s+j] -= al*a[p[k]*s+j];
1258  }
1259  }
1260  return(0);
1261  }
1262 
1264 register context *ctx;
1265 int n;
1266 pointer argv[];
1267 /* (LU-DECOMPOSE mat [result]) */
1268 { pointer a,result,pv;
1269  int s,stat;
1270 
1271  ckarg2(1,2);
1272  a=argv[0];
1273  if (!ismatrix(a)) error(E_NOVECTOR);
1274  s=colsize(a);
1275  if (s!=rowsize(a)) error(E_VECSIZE);
1276  if (n==1) result=a;
1277  else {
1278  result=argv[1];
1279  if (!ismatrix(result)) error(E_NOVECTOR);
1280  if (s!=colsize(result)) error(E_VECSIZE);
1281  copymat(result,a,s); }
1282  pv=makevector(C_VECTOR,s);
1283  stat=decompose(result->c.ary.entity->c.fvec.fv,s,(eusinteger_t *)pv->c.vec.v);
1284  while (--s>=0) pv->c.vec.v[s]=makeint((eusinteger_t)pv->c.vec.v[s]);
1285  if (stat<0) return(NIL);
1286  else return(pv);}
1287 
1288 static void solve(a,pv,s,b,x)
1289 register eusfloat_t *a;
1290 pointer b,x,pv;
1291 int s;
1292 { register int i,j;
1293  eusfloat_t t;
1294  register pointer *p;
1295  p=pv->c.vec.v;
1296 
1297  /*forward substitution*/
1298  for (i=0; i<s; i++) {
1299  t=b->c.fvec.fv[intval(p[i])];
1300  for (j=0; j<i; j++) t-= a[intval(p[i])*s+j]*x->c.fvec.fv[j];
1301  x->c.fvec.fv[i]=t;}
1302  /*backward substitution*/
1303  for (i=s-1; i>=0; i--) {
1304  t=x->c.fvec.fv[i];
1305  for (j=i+1; j<s; j++) t-= a[intval(p[i])*s+j]*x->c.fvec.fv[j];
1306  x->c.fvec.fv[i]=t*a[intval(p[i])*s+i];}
1307 }
1308 
1309 pointer LU_SOLVE(ctx,n,argv)
1310 register context *ctx;
1311 int n;
1312 pointer argv[];
1313 { pointer a,p,b,x;
1314  int s;
1315 
1316  ckarg2(3,4);
1317  a=argv[0]; p=argv[1]; b=argv[2];
1318  if (!ismatrix(a)) error(E_NOVECTOR);
1319  s=colsize(a);
1320  if (!isvector(p) || !isfltvector(b)) error(E_NOVECTOR);
1321  if (s!=vecsize(p) || s!=vecsize(b)) error(E_VECSIZE);
1322  if (n==4) {
1323  x=argv[3];
1324  if (!isvector(x)) error(E_NOVECTOR);
1325  if (s!=vecsize(x)) error(E_VECSIZE); }
1326  else x=(pointer)makefvector(s);
1327  solve(a->c.ary.entity->c.fvec.fv,p,s,b,x);
1328  return(x);}
1329 
1331 register context *ctx;
1332 int n;
1333 pointer argv[];
1334 { register pointer a,p;
1335  register eusfloat_t *av;
1336  register int i,s;
1337  eusfloat_t val=1.0;
1338  numunion nu;
1339  ckarg(2);
1340  a=argv[0]; p=argv[1];
1341  if (!isvector(p) || !ismatrix(a)) error(E_NOVECTOR);
1342  s=vecsize(p);
1343  if (colsize(a)!=s || rowsize(a)!=s) error(E_VECSIZE);
1344  av=a->c.ary.entity->c.fvec.fv;
1345  for (i=0; i<s; i++) val*=av[intval(p->c.vec.v[i])*s+i];
1346  return(makeflt(1.0/val));}
1347 
1348 void matrix(ctx,mod)
1349 register context *ctx;
1350 register pointer mod;
1351 {
1352  K_X=defkeyword(ctx,"X");
1353  K_Y=defkeyword(ctx,"Y");
1354  K_Z=defkeyword(ctx,"Z");
1355  MK_X=defkeyword(ctx,"-X");
1356  MK_Y=defkeyword(ctx,"-Y");
1357  MK_Z=defkeyword(ctx,"-Z");
1358 
1359  defun(ctx,"V+",mod,VPLUS,NULL);
1360  defun(ctx,"V++",mod,VPLUSPLUS,NULL);
1361  defun(ctx,"V-",mod,VMINUS,NULL);
1362  defun(ctx,"V-ABS",mod,VMINUS_ABS,NULL);
1363  defun(ctx,"V.",mod,VINNERPRODUCT,NULL);
1364  defun(ctx,"V*",mod,VCROSSPRODUCT,NULL);
1365  defun(ctx,"V.*",mod,SCA3PROD,NULL);
1366  defun(ctx,"V<",mod,VLESSP,NULL);
1367  defun(ctx,"V>",mod,VGREATERP,NULL);
1368  defun(ctx,"VMIN",mod,VMIN,NULL);
1369  defun(ctx,"VMAX",mod,VMAX,NULL);
1370  defun(ctx,"MINIMAL-BOX",mod,MINIMALBOX,NULL);
1371  defun(ctx,"SCALE",mod,SCALEVEC,NULL);
1372  defun(ctx,"NORM",mod,VNORM,NULL);
1373  defun(ctx,"NORM2",mod,VNORM2,NULL);
1374  defun(ctx,"NORMALIZE-VECTOR",mod,VNORMALIZE,NULL);
1375  defun(ctx,"DISTANCE",mod,VDISTANCE,NULL);
1376  defun(ctx,"DISTANCE2",mod,VDISTANCE2,NULL);
1377  defun(ctx,"DIRECTION",mod,VDIRECTION,NULL);
1378  defun(ctx,"MIDPOINT",mod,MIDPOINT,NULL);
1379 /* defun(ctx,"TRIANGLE",mod,TRIANGLE,NULL); */
1380  defun(ctx,"FLOATVECTOR",mod,MKFLTVEC,NULL);
1381  defun(ctx,"FLOAT-VECTOR",mod,MKFLTVEC,NULL);
1382  defun(ctx,"TRANSFORM",mod,TRANSFORM,NULL);
1383  defun(ctx,"M*",mod,MATTIMES,NULL);
1384  defun(ctx,"ROTATE-VECTOR",mod,ROTVEC,NULL);
1385  defun(ctx,"ROTATE-MATRIX",mod,ROTMAT,NULL);
1386  defun(ctx,"ROTATION-MATRIX",mod,ROTATION_MATRIX,NULL);
1387  defun(ctx,"ROTATION-ANGLE",mod,ROTANGLE,NULL);
1388  defun(ctx,"TRANSPOSE",mod,TRANSPOSE,NULL);
1389  defun(ctx,"RPY-ANGLE",mod,INV_RPY,NULL);
1390  defun(ctx,"EULER-ANGLE",mod,INV_EULER,NULL);
1391  defun(ctx,"LU-DECOMPOSE",mod,LU_DECOMPOSE,NULL);
1392  defun(ctx,"LU-SOLVE",mod,LU_SOLVE,NULL);
1393  defun(ctx,"LU-DETERMINANT",mod,LU_DETERMINANT,NULL);}
pointer MIDPOINT(context *ctx, int n, argv)
Definition: matrix.c:476
eusinteger_t iv[1]
Definition: eus.h:303
d
pointer TRANSFORM(context *ctx, int n, argv)
Definition: matrix.c:809
Definition: eus.h:366
f
struct vector vec
Definition: eus.h:412
#define makeint(v)
Definition: sfttest.c:2
struct cell * pointer
Definition: eus.h:163
pointer C_VECTOR
Definition: eus.c:144
pointer MKFLTVEC(context *ctx, int n, argv)
Definition: matrix.c:531
Definition: eus.h:522
pointer cons(context *, pointer, pointer)
Definition: makes.c:97
eusfloat_t ratio2flt(pointer r)
Definition: arith.c:506
pointer ROTANGLE(context *ctx, int n, argv)
Definition: matrix.c:1056
pointer MINIMALBOX(context *ctx, int n, argv)
Definition: matrix.c:608
struct string str
Definition: eus.h:400
byte chars[1]
Definition: eus.h:210
static pointer K_Y
Definition: matrix.c:22
double cos()
double sin()
pointer VPLUSPLUS(context *ctx, int n, pointer *argv)
Definition: matrix.c:70
pointer T
Definition: eus.c:110
static pointer K_X
Definition: matrix.c:22
GLfloat n[6][3]
Definition: cube.c:15
struct arrayheader ary
Definition: eus.h:411
Definition: eus.h:945
pointer VLESSP(context *ctx, int n, argv)
Definition: matrix.c:563
pointer MATTIMES(context *ctx, int n, argv)
Definition: matrix.c:757
static pointer MK_X
Definition: matrix.c:22
pointer VDIRECTION(context *ctx, int n, argv)
Definition: matrix.c:369
pointer VMINUS(context *ctx, int n, pointer *argv)
Definition: matrix.c:92
pointer makefvector()
pointer INV_RPY(context *ctx, int n, argv)
Definition: matrix.c:1157
#define intval(p)
Definition: sfttest.c:1
defun("ADR_TO_STRING", mod, ADR_TO_STRING)
static void copymat(pointer dest, pointer src, int size)
Definition: matrix.c:905
pointer LU_DECOMPOSE(context *ctx, int n, argv)
Definition: matrix.c:1263
pointer makevector(pointer, int)
Definition: makes.c:417
pointer ROTMAT(context *ctx, int n, argv)
Definition: matrix.c:914
ckarg(2)
pointer VMINUS_ABS(context *ctx, int n, pointer *argv)
Definition: matrix.c:156
Definition: eus.h:960
float ckfltval()
pointer INV_EULER(context *ctx, int n, argv)
Definition: matrix.c:1189
pointer defkeyword()
pointer VNORM2(context *ctx, int n, pointer *argv)
Definition: matrix.c:268
static void solve(eusfloat_t *a, pointer pv, int s, pointer b, pointer x)
Definition: matrix.c:1288
struct intvector ivec
Definition: eus.h:414
pointer VDISTANCE(context *ctx, int n, argv)
Definition: matrix.c:313
union cell::cellunion c
pointer entity
Definition: eus.h:311
pointer TRANSPOSE(context *ctx, int n, argv)
Definition: matrix.c:1126
Definition: eus.h:426
double atan2()
long l
Definition: structsize.c:3
#define M_PI
Definition: dinoshade.c:46
double sqrt()
pointer VNORMALIZE(context *ctx, int n, argv)
Definition: matrix.c:290
Definition: eus.h:379
static pointer MK_Y
Definition: matrix.c:22
pointer ROTATION_MATRIX(context *ctx, int n, pointer *argv)
Definition: matrix.c:992
pointer ROTVEC(context *ctx, int n, argv)
Definition: matrix.c:864
short s
Definition: structsize.c:2
pointer LU_SOLVE(context *ctx, int n, argv)
Definition: matrix.c:1309
static int decompose(eusfloat_t *a, int s, p)
Definition: matrix.c:1234
pointer error(enum errorcode ec,...) pointer error(va_alist) va_dcl
Definition: eus.c:297
long eusinteger_t
Definition: eus.h:19
eusfloat_t big_to_float(pointer)
Definition: big.c:953
byte * get_string()
#define colsize(p)
Definition: matrix.c:735
#define ismatrix(p)
Definition: matrix.c:731
Definition: eus.h:973
pointer SCALEVEC(context *ctx, int n, argv)
Definition: matrix.c:440
#define rowsize(p)
Definition: matrix.c:734
Definition: eus.h:985
float fltval()
pointer VNORM(context *ctx, int n, pointer *argv)
Definition: matrix.c:244
static pointer MK_Z
Definition: matrix.c:22
pointer VCROSSPRODUCT(context *ctx, int n, pointer *argv)
Definition: matrix.c:393
static pointer K_Z
Definition: matrix.c:22
pointer VGREATERP(context *ctx, int n, argv)
Definition: matrix.c:585
#define NULL
Definition: transargv.c:8
pointer makematrix()
pointer VPLUS(context *ctx, int n, pointer *argv)
Definition: matrix.c:28
pointer VMAX(context *ctx, int n, argv)
Definition: matrix.c:689
GLfloat v[8][3]
Definition: cube.c:21
pointer SCA3PROD(context *ctx, int n, argv)
Definition: matrix.c:419
pointer VDISTANCE2(context *ctx, int n, argv)
Definition: matrix.c:341
void matrix(context *ctx, pointer mod)
Definition: matrix.c:1348
static int ix
Definition: printer.c:29
eusfloat_t fv[1]
Definition: eus.h:307
pointer LU_DETERMINANT(context *ctx, int n, argv)
Definition: matrix.c:1330
double eusfloat_t
Definition: eus.h:20
static char * rcsid
Definition: matrix.c:13
pointer NIL
Definition: eus.c:110
#define EPS
Definition: arith.h:15
pointer v[1]
Definition: eus.h:299
pointer VMIN(context *ctx, int n, argv)
Definition: matrix.c:654
if(n==1)
Definition: unixcall.c:491
char a[26]
Definition: freq.c:4
pointer VINNERPRODUCT(context *ctx, int n, pointer *argv)
Definition: matrix.c:219
struct floatvector fvec
Definition: eus.h:413
pointer makeflt()


euslisp
Author(s): Toshihiro Matsui
autogenerated on Thu Jun 6 2019 20:00:44