matrix.c
Go to the documentation of this file.
00001 /***************************************************/
00002 /* float vector and matrix operations
00003 /*      Copyright(c) Toshihiro Matsui
00004 /*      Electrotechnical Laboratory
00005 /*
00006 /*      1986-Nov
00007 /*      1987-Feb        complete floatvector
00008 /*      1987-Mar        modify rotation 
00009 /*      1987-Nov        simultaneous equations
00010 /*      1988-Jan        matrix is represented by 2D array,
00011 /*                      not by an vector of vectors
00012 /**************************************************************/
00013 static char *rcsid="@(#)$Id$";
00014 
00015 #include "math.h"
00016 #include "eus.h"
00017 
00018 extern double sin(), cos(), sqrt(), atan2();
00019 extern pointer makefvector(),defkeyword();
00020 extern pointer makematrix();
00021 
00022 static pointer K_X,K_Y,K_Z,MK_X,MK_Y,MK_Z;
00023 
00024 /* #define PI 3.1415926536 */
00025 
00026 #define ckvsize(a,b) ((a->c.vec.size==b->c.vec.size)?vecsize(a):(int)(eusinteger_t)error(E_VECINDEX))
00027 
00028 pointer VPLUS(ctx,n,argv)
00029 register context *ctx;
00030 int n;
00031 register pointer *argv;
00032 { register int i,s;
00033   register pointer result;
00034   register int isf,isi,iss;
00035 
00036   ckarg2(2,3);
00037   if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
00038       !((isi=isintvector(argv[0])) && isintvector(argv[1])) &&
00039       !((iss=isstring(argv[0]))    && isstring(argv[1])))
00040     error(E_TYPEMISMATCH);
00041   s=ckvsize(argv[0],argv[1]);
00042   if (n==3) {
00043     result=argv[2];
00044     if (s!=vecsize(result)) error(E_FLOATVECTOR);}
00045   else result=makevector(classof(argv[0]), s);
00046   if (isf) {
00047     register eusfloat_t *a,*b,*r;
00048     if (!isfltvector(result))  error(E_FLOATVECTOR);
00049     a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
00050     r=result->c.fvec.fv;
00051     for(i=0; i<s; i++) r[i]= a[i] + b[i];
00052     return(result);}
00053   else if (isi) {
00054     register eusinteger_t *ia, *ib, *ir;
00055     if (!isintvector(result))  error(E_NOINTVECTOR);
00056     ia=argv[0]->c.ivec.iv; ib=argv[1]->c.ivec.iv;
00057     ir=result->c.ivec.iv;
00058     for(i=0; i<s; i++) ir[i]= ia[i] + ib[i];
00059     return(result);}
00060   else if (iss) {
00061     register unsigned char *ba, *bb, *br;
00062     ba=argv[0]->c.str.chars;
00063     bb=argv[1]->c.str.chars;
00064     br=result->c.str.chars;
00065     for(i=0; i<s; i++) br[i]=ba[i] + bb[i];
00066     return(result);}
00067   else error(E_NOSEQ);
00068   }
00069 
00070 pointer VPLUSPLUS(ctx,n,argv)
00071 register context *ctx;
00072 int n;
00073 register pointer *argv;
00074 { register int i,s;
00075   register pointer p,result;
00076   eusfloat_t *a,*r;
00077 
00078   if (n==1) return(argv[0]);
00079   if (!isfltvector(argv[0])) error(E_FLOATVECTOR);
00080   s=vecsize(argv[0]);
00081   result=makefvector(s);
00082   r=result->c.fvec.fv;   a=argv[0]->c.fvec.fv;
00083   for (i=0; i<s; i++) r[i]=a[i];
00084   while (--n>0) {
00085     p=argv[n];
00086     if (!isfltvector(p)) error(E_FLOATVECTOR);
00087     if (vecsize(p) != s) error(E_VECINDEX);
00088     a=p->c.fvec.fv;
00089     for (i=0; i<s; i++) r[i]+=a[i];}
00090   return(result);}
00091 
00092 pointer VMINUS(ctx,n,argv)
00093 register context *ctx;
00094 int n;
00095 register pointer *argv;
00096 { register int i,s;
00097   register eusinteger_t *ia, *ib, *ir;
00098   register pointer result;
00099   register eusfloat_t *a,*b,*r;
00100   register int isi,isf,iss;
00101 
00102   ckarg2(1,3);  
00103   isi=isintvector(argv[0]);
00104   isf=isfltvector(argv[0]);
00105   iss=isstring   (argv[0]);
00106   if (!isi && !isf && !iss)
00107     error(E_NOVECTOR);
00108   s=vecsize(argv[0]);
00109   if (n==1) {   /*negate float vector*/
00110     result=makevector(classof(argv[0]),s);
00111     if (isi) {
00112       ia=argv[0]->c.ivec.iv;
00113       for (i=0; i<s; i++) result->c.ivec.iv[i]= -ia[i];
00114       return(result);}
00115     else if (isf) {
00116       a=argv[0]->c.fvec.fv;
00117       for (i=0; i<s; i++) result->c.fvec.fv[i]= -a[i];
00118       return(result);}
00119     else error(E_NOVECTOR);}
00120 
00121   /* argc>=2 */
00122   if (!(isintvector(argv[1])&&isi) &&
00123       !(isfltvector(argv[1])&&isf) &&
00124       !(isstring(argv[1])   &&iss))
00125     error(E_TYPEMISMATCH);
00126   s=ckvsize(argv[0],argv[1]);
00127   if (n==3) {
00128     result=argv[2];
00129     if (!(isf&&isfltvector(result)) && !(isi&&isintvector(result)) &&
00130         !(iss&&isstring(result)) )
00131          error(E_TYPEMISMATCH);
00132     if (vecsize(result)!=s) error(E_VECINDEX);}
00133   else  result=makevector(classof(argv[0]),s);
00134 
00135   if (isf) {
00136     a=argv[0]->c.fvec.fv;
00137     b=argv[1]->c.fvec.fv;
00138     r=result->c.fvec.fv;
00139     for(i=0; i<s; i++) r[i]=a[i]-b[i];
00140     return(result);}
00141   else if (isi) {
00142     ia=argv[0]->c.ivec.iv;
00143     ib=argv[1]->c.ivec.iv;
00144     ir=result->c.ivec.iv;
00145     for(i=0; i<s; i++) ir[i]=ia[i]-ib[i];
00146     return(result);}
00147   else if (iss) {
00148     register unsigned char *ba, *bb, *br;
00149     ba=argv[0]->c.str.chars;
00150     bb=argv[1]->c.str.chars;
00151     br=result->c.str.chars;
00152     for(i=0; i<s; i++) br[i]=ba[i]-bb[i];
00153     return(result);}
00154   else error(E_NOSEQ);}
00155 
00156 pointer VMINUS_ABS(ctx,n,argv)
00157 register context *ctx;
00158 int n;
00159 register pointer *argv;
00160 { register int i,s;
00161   register eusinteger_t *ia, *ib, *ir;
00162   register pointer result;
00163   register eusfloat_t *a,*b,*r;
00164   register eusfloat_t x;
00165   register int isi,isf,iss;
00166 
00167   register int ix;
00168 
00169   ckarg2(2,3);  
00170   if (!(isi=isintvector(argv[0])) && !(isf=isfltvector(argv[0])) &&
00171       !(iss=isstring   (argv[0])))
00172     error(E_NOVECTOR);
00173   s=vecsize(argv[0]);
00174 
00175   /* argc>=2 */
00176   if (!(isintvector(argv[1])&&isi) &&
00177       !(isfltvector(argv[1])&&isf) &&
00178       !(isstring(argv[1])   &&iss))
00179     error(E_TYPEMISMATCH);
00180   s=ckvsize(argv[0],argv[1]);
00181   if (n==3) {
00182     result=argv[2];
00183     if (!(isf&&isfltvector(result)) && !(isi&&isintvector(result)) &&
00184         !(iss&&isstring(result)) )
00185          error(E_TYPEMISMATCH);
00186     if (vecsize(result)!=s) error(E_VECINDEX);}
00187   else  result=makevector(classof(argv[0]),s);
00188 
00189   if (isf) {
00190     a=argv[0]->c.fvec.fv;
00191     b=argv[1]->c.fvec.fv;
00192     r=result->c.fvec.fv;
00193     for(i=0; i<s; i++) {
00194        x=a[i]-b[i];
00195        if (x<0) x= -x;
00196        r[i]=x;}
00197     return(result);}
00198   else if (isi) {
00199     ia=argv[0]->c.ivec.iv;
00200     ib=argv[1]->c.ivec.iv;
00201     ir=result->c.ivec.iv;
00202     for(i=0; i<s; i++) {
00203       ix=ia[i]-ib[i];
00204       if (ix<0) ix= -ix;
00205       ir[i]=ix;}
00206     return(result);}
00207   else if (iss) {
00208     register unsigned char *ba, *bb, *br;
00209     ba=argv[0]->c.str.chars;
00210     bb=argv[1]->c.str.chars;
00211     br=result->c.str.chars;
00212     for(i=0; i<s; i++) {
00213       ix=ba[i]-bb[i];
00214       if (ix<0) ix= -ix;
00215       br[i]=ix;}
00216     return(result);}
00217   else error(E_NOSEQ);}
00218 
00219 pointer VINNERPRODUCT(ctx,n,argv)
00220 register context *ctx;
00221 int n;
00222 register pointer *argv;
00223 { register eusfloat_t *a,*b;
00224   register long i,s;
00225   register eusinteger_t *ia, *ib; register long isum=0;
00226   eusfloat_t sum=0.0;
00227   register int isf,isi;
00228   numunion nu;
00229   ckarg(2);
00230   if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
00231       !((isi=isintvector(argv[0])) && isintvector(argv[1])))
00232     error(E_TYPEMISMATCH);
00233   s=ckvsize(argv[0],argv[1]);
00234   if (isf) {
00235     a= (argv[0]->c.fvec.fv); b= (argv[1]->c.fvec.fv);
00236     for (i=0; i<s; i++) sum+= a[i] * b[i];
00237     return(makeflt(sum));}
00238   if (isi) {
00239     ia= (argv[0]->c.ivec.iv); ib= (argv[1]->c.ivec.iv);
00240     for (i=0; i<s; i++) isum+= ia[i] * ib[i];
00241     return(makeint(isum));}
00242   else error(E_NOSEQ);}
00243 
00244 pointer VNORM(ctx,n,argv)
00245 register context *ctx;
00246 int n;
00247 pointer *argv;
00248 { register int i,s;
00249   register eusfloat_t *a,sum=0.0;
00250   numunion nu;
00251   ckarg(1);
00252   if (!isvector(argv[0])) error(E_NOVECTOR);
00253   s=vecsize(argv[0]);
00254   if (elmtypeof(argv[0])==ELM_FLOAT) {
00255     a=argv[0]->c.fvec.fv;
00256     for (i=0; i<s; i++) sum+=a[i]*a[i];
00257     sum=sqrt(sum);
00258     return(makeflt(sum));}
00259   else if (elmtypeof(argv[0])==ELM_INT) {
00260     eusinteger_t *ia;
00261     ia=argv[0]->c.ivec.iv;
00262     for (i=0; i<s; i++) sum+=ia[i]*ia[i];
00263     sum=sqrt(sum);
00264     return(makeflt(sum));}
00265   else error(E_FLOATVECTOR);
00266   }
00267 
00268 pointer VNORM2(ctx,n,argv)
00269 register context *ctx;
00270 int n;
00271 pointer *argv;
00272 { register int i,s;
00273   register eusfloat_t *a,sum=0.0;
00274   numunion nu;
00275   ckarg(1);
00276   if (!isvector(argv[0])) error(E_NOVECTOR);
00277   s=vecsize(argv[0]);
00278   if (elmtypeof(argv[0])==ELM_FLOAT) {
00279     a=argv[0]->c.fvec.fv;
00280     for (i=0; i<s; i++) sum+=a[i]*a[i];
00281     /* sum=sqrt(sum); no square root */
00282     return(makeflt(sum));}
00283   else if (elmtypeof(argv[0])==ELM_INT) {
00284     eusinteger_t *ia;
00285     ia=argv[0]->c.ivec.iv;
00286     for (i=0; i<s; i++) sum+=ia[i]*ia[i];
00287     return(makeflt(sum));}
00288   else error(E_FLOATVECTOR);}
00289 
00290 pointer VNORMALIZE(ctx,n,argv)
00291 register context *ctx;
00292 int n;
00293 register pointer argv[];
00294 { register pointer result;
00295   register eusfloat_t *a,*r,sum=0.0;
00296   register int i,s;
00297 
00298   ckarg2(1,2);
00299   if (!isfltvector(argv[0])) error(E_FLOATVECTOR);
00300   s=vecsize(argv[0]);
00301   if (n==2) { 
00302     result=argv[1];
00303     if (!isfltvector(result)) error(E_FLOATVECTOR);
00304     if (s!=vecsize(result)) error(E_VECINDEX);}
00305   else result=makefvector(s);
00306   a=argv[0]->c.fvec.fv;
00307   r=result->c.fvec.fv;
00308   for (i=0; i<s; i++) sum+= a[i]*a[i];
00309   sum=sqrt(sum);
00310   for (i=0; i<s; i++) r[i]=a[i]/sum;
00311   return(result);}
00312   
00313 pointer VDISTANCE(ctx,n,argv)
00314 register context *ctx;
00315 int n;
00316 pointer argv[];
00317 {
00318   register eusfloat_t *a, *b;
00319   register int s;
00320   eusfloat_t d,dist=0.0;
00321   register int isf,isi;
00322   numunion nu;
00323   ckarg(2);
00324   if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
00325       !((isi=isintvector(argv[0])) && isintvector(argv[1])))
00326     error(E_TYPEMISMATCH);
00327   s=ckvsize(argv[0],argv[1]);
00328   if (isf) {
00329     a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
00330     while (--s>=0) { d=a[s]-b[s]; dist+= d*d;}
00331     return(makeflt(sqrt(dist)));}
00332   else if (isi) {
00333     register eusinteger_t *ia, *ib;
00334     register long id, idist=0;
00335     ia=argv[0]->c.ivec.iv; ib=argv[1]->c.ivec.iv;
00336     while (--s>=0) { id= ia[s]-ib[s]; idist+= id * id;}
00337     dist=idist;
00338     return(makeflt(sqrt(dist)));}
00339   else error(E_FLOATVECTOR);}
00340 
00341 pointer VDISTANCE2(ctx,n,argv)
00342 register context *ctx;
00343 int n;
00344 pointer argv[];
00345 {
00346   register eusfloat_t *a, *b;
00347   register int s;
00348   eusfloat_t d,dist=0.0;
00349   register int isf,isi;
00350   numunion nu;
00351   ckarg(2);
00352   if (!((isf=isfltvector(argv[0])) && isfltvector(argv[1])) &&
00353       !((isi=isintvector(argv[0])) && isintvector(argv[1])))
00354     error(E_TYPEMISMATCH);
00355   s=ckvsize(argv[0],argv[1]);
00356   if (isf) {
00357     a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
00358     while (--s>=0) { d=a[s]-b[s]; dist+= d*d;}
00359     return(makeflt(dist));}
00360   else if (isi) {
00361     register eusinteger_t *ia, *ib;
00362     register long id, idist=0;
00363     ia=argv[0]->c.ivec.iv; ib=argv[1]->c.ivec.iv;
00364     while (--s>=0) { id= ia[s]-ib[s]; idist+= id * id;}
00365     dist=idist;
00366     return(makeflt(dist));}
00367   else error(E_FLOATVECTOR);}
00368 
00369 pointer VDIRECTION(ctx,n,argv)
00370 register context *ctx;
00371 int n;
00372 pointer argv[];
00373 {
00374   register eusfloat_t *a, *b, *r;
00375   register int i,s;
00376   pointer result;
00377   eusfloat_t norm=0.0;
00378   ckarg2(2,3);
00379   if (!isfltvector(argv[0]) || !isfltvector(argv[1])) error(E_FLOATVECTOR);
00380   s=ckvsize(argv[0],argv[1]);
00381   if (n==3) {
00382     result=argv[2]; 
00383     if (!isfltvector(result)) error(E_FLOATVECTOR);
00384     if (vecsize(result)!=s) error(E_VECINDEX);
00385     }
00386   else result=makefvector(s);
00387   r=result->c.fvec.fv;  a=argv[0]->c.fvec.fv; b=argv[1]->c.fvec.fv;
00388   for (i=0; i<s; i++) { r[i]=b[i]-a[i]; norm+= r[i]*r[i];}
00389   norm=sqrt(norm);
00390   for (i=0; i<s; i++)  { r[i]=r[i]/norm;}
00391   return(result);}
00392 
00393 pointer VCROSSPRODUCT(ctx,n,argv)
00394 register context *ctx;
00395 int n;
00396 register pointer *argv;
00397 {
00398   register eusfloat_t *fv1,*fv2,*rfv;
00399   register pointer result;
00400   register int s;
00401 
00402   ckarg2(2,3);
00403   if (!isfltvector(argv[0]) || !isfltvector(argv[1])) error(E_FLOATVECTOR);
00404   s=ckvsize(argv[0],argv[1]);
00405   if (s!=3) error(E_VECINDEX);
00406   if (n==3) {
00407     result=argv[2]; 
00408     if (!isfltvector(result)) error(E_FLOATVECTOR);
00409     if (vecsize(result)!=3) error(E_VECINDEX);
00410     }
00411   else result=makefvector(3);
00412   rfv=result->c.fvec.fv;
00413   fv1= argv[0]->c.fvec.fv; fv2= argv[1]->c.fvec.fv;
00414   rfv[0]=fv1[1] * fv2[2] - fv1[2] * fv2[1];
00415   rfv[1]=fv1[2] * fv2[0] - fv1[0] * fv2[2];
00416   rfv[2]=fv1[0] * fv2[1] - fv1[1] * fv2[0];
00417   return(result);}
00418 
00419 pointer SCA3PROD(ctx,n,argv)    /*scalar tripple product [A,B,C] */
00420 register context *ctx;
00421 int n;
00422 pointer argv[];
00423 {
00424   register eusfloat_t *va,*vb,*vc;
00425   eusfloat_t val;
00426   numunion nu;
00427   ckarg(3);
00428   if (!isfltvector(argv[0]) || !isfltvector(argv[1]) || !isfltvector(argv[2]))
00429          error(E_FLOATVECTOR);
00430   if (vecsize(argv[0])!=3 || vecsize(argv[1])!=3 || vecsize(argv[2])!=3) error(E_VECINDEX);
00431   va=argv[0]->c.fvec.fv; vb=argv[1]->c.fvec.fv; vc=argv[2]->c.fvec.fv;
00432   val =va[0] * vb[1] * vc[2];
00433   val+=va[2] * vb[0] * vc[1];
00434   val+=va[1] * vb[2] * vc[0];
00435   val-=va[2] * vb[1] * vc[0];
00436   val-=va[0] * vb[2] * vc[1];
00437   val-=va[1] * vb[0] * vc[2];
00438   return(makeflt(val));}
00439   
00440 pointer SCALEVEC(ctx,n,argv)
00441 register context *ctx;
00442 int n;
00443 pointer argv[];
00444 {
00445   eusfloat_t scale;
00446   pointer result;
00447   register eusfloat_t *a,*r;
00448   register int s,i;
00449   register int isf,isi;
00450   numunion nu;
00451 
00452   ckarg2(2,3);
00453   scale=ckfltval(argv[0]);
00454   if (!(isf=isfltvector(argv[1])) && !(isi=isintvector(argv[1])))
00455     error(E_FLOATVECTOR);
00456   s=vecsize(argv[1]);
00457   if (n==3) {
00458     result=argv[2];
00459     if (!isvector(result)) error(E_NOVECTOR);
00460     if (elmtypeof(result)!=elmtypeof(argv[1])) error(E_TYPEMISMATCH);
00461     if (s!=vecsize(result)) error(E_VECINDEX);}
00462   else result=makevector(classof(argv[1]), s);
00463   if (isf) {
00464     a=argv[1]->c.fvec.fv;
00465     r=result->c.fvec.fv;
00466     for (i=0; i<s; i++) r[i]=scale*(a[i]);
00467     return(result);}
00468   else if (isi) {
00469     register eusinteger_t *ia, *ir;
00470     ia=argv[1]->c.ivec.iv;
00471     ir=result->c.ivec.iv;
00472     for (i=0; i<s; i++) ir[i]=scale*(ia[i]);
00473     return(result);}
00474   else error(E_NOSEQ);}
00475 
00476 pointer MIDPOINT(ctx,n,argv)
00477 register context *ctx;
00478 int n;
00479 register pointer argv[];
00480 { int i,vsize;
00481   double ratio, ratio2;
00482   pointer p1,p2,result;
00483   register int isf,isi;
00484   numunion nu;
00485 
00486   ckarg2(3,4);
00487   ratio=ckfltval(argv[0]); ratio2=1.0-ratio;
00488   p1=argv[1]; p2=argv[2];
00489   if (!((isf=isfltvector(p1))&&isfltvector(p2)) &&
00490       !((isi=isintvector(p1))&&isintvector(p2)))
00491     error(E_TYPEMISMATCH);
00492   vsize=ckvsize(p1,p2);  
00493   if (n==4) {
00494     result=argv[3];
00495     if (!(isf&&isfltvector(result))&&!(isi&&isintvector(result)))
00496       error(E_TYPEMISMATCH);
00497     if (vsize!=vecsize(result)) error(E_VECINDEX);}
00498   else result=makevector(classof(p1), vsize);
00499   if (isf) {
00500     register eusfloat_t *r,*pp1,*pp2;
00501     r=result->c.fvec.fv; pp1=p1->c.fvec.fv; pp2=p2->c.fvec.fv;
00502     for (i=0; i<vsize; i++) 
00503       r[i]=pp1[i]*ratio2 + pp2[i]*ratio;
00504     return(result);}
00505   else if (isi) {
00506     register eusinteger_t *r,*pp1,*pp2;
00507     r=result->c.ivec.iv; pp1=p1->c.ivec.iv; pp2=p2->c.ivec.iv;
00508     for (i=0; i<vsize; i++) 
00509       r[i]=pp1[i]*ratio2 + pp2[i]*ratio;
00510     return(result);}
00511   else error(E_FLOATVECTOR);}
00512 
00513 /*
00514 pointer TRIANGLE(ctx,n,argv)
00515 register context *ctx;
00516 int n;
00517 register pointer argv[];
00518 { register float *v1,*v2,*v3,tr;
00519   ckarg(3);
00520   if (!isfltvector(argv[0])) error(E_FLOATVECTOR);
00521   if (!isfltvector(argv[1])) error(E_FLOATVECTOR);
00522   if (!isfltvector(argv[2])) error(E_FLOATVECTOR);
00523   v1=argv[0]->c.fvec.fv;
00524   v2=argv[1]->c.fvec.fv;
00525   v3=argv[2]->c.fvec.fv;
00526   tr=v1[0]*v2[1] + v2[0]*v3[1] + v3[0]*v1[1] 
00527      - v3[0]*v2[1] - v2[0]*v1[1] - v1[0]*v3[1];
00528   return(makeflt(tr));}
00529 */
00530 
00531 pointer MKFLTVEC(ctx,n,argv)
00532 register context *ctx;
00533 int n;
00534 pointer argv[];
00535 { register pointer r;
00536   register int i;
00537   register eusfloat_t *fv;
00538   numunion nu;
00539   r=makefvector(n);
00540   fv=r->c.fvec.fv;
00541   for (i=0; i<n; i++) {
00542     //fv[i]=ckfltval(argv[i]);
00543     if ( isflt(argv[i]) ) fv[i] = fltval(argv[i]);
00544     else if ( isint(argv[i]) ) fv[i] = intval(argv[i]);
00545     else if ( pisbignum(argv[i]) ) fv[i] = big_to_float(argv[i]);
00546     else if ( pisratio(argv[i]) ) fv[i] = ratio2flt(argv[i]);
00547     else if ( issymbol(argv[i]) ) {
00548         char *sym = get_string(argv[i]);
00549         if ( strcmp(sym, "NAN") == 0 ) fv[i] = NAN;
00550         else if ( strcmp(sym, "-NAN") == 0 ) fv[i] = -NAN;
00551         else if ( strcmp(sym,  "INF") == 0 ) fv[i] = INFINITY;
00552         else if ( strcmp(sym, "-INF") == 0 ) fv[i] = -INFINITY;
00553         else { (eusinteger_t)error(E_NONUMBER); }
00554     }
00555     else { (eusinteger_t)error(E_NONUMBER); }
00556   }
00557 
00558   return(r);
00559   }
00560 
00561 /* comparation of vectors */
00562 
00563 pointer VLESSP(ctx,n,argv)
00564 register context *ctx;
00565 int n;
00566 pointer argv[];
00567 { register int i,s;
00568   register pointer a,b;
00569   register int isf,isi;
00570   ckarg(2);
00571   a=argv[0]; b=argv[1];
00572   if (!((isf=isfltvector(a))&&isfltvector(b)) &&
00573       !((isi=isintvector(a))&&isintvector(b))) error(E_FLOATVECTOR);
00574   s=ckvsize(a,b);
00575   if (isf) {
00576     register eusfloat_t *av=a->c.fvec.fv, *bv=b->c.fvec.fv;
00577     for (i=0; i<s; i++) if (av[i] > bv[i]) return(NIL);
00578     return(T);}
00579   else if (isi) {
00580     register eusinteger_t *av=a->c.ivec.iv, *bv=b->c.ivec.iv;
00581     for (i=0; i<s; i++) if (av[i] > bv[i]) return(NIL);
00582     return(T);}
00583   else  error(E_FLOATVECTOR);}
00584 
00585 pointer VGREATERP(ctx,n,argv)
00586 register context *ctx;
00587 int n;
00588 pointer argv[];
00589 /*every element of argv[0] is greater than corresponding elem of arg[1]*/
00590 { register int i,s;
00591   register pointer a,b;
00592   register int isf,isi;
00593   ckarg(2);     
00594   a=argv[0]; b=argv[1];
00595   if (!((isf=isfltvector(a))&&isfltvector(b)) &&
00596       !((isi=isintvector(a))&&isintvector(b))) error(E_FLOATVECTOR);
00597   s=ckvsize(a,b);
00598   if (isf) {
00599     register eusfloat_t *av=a->c.fvec.fv, *bv=b->c.fvec.fv;
00600     for (i=0; i<s; i++) if (av[i] < bv[i]) return(NIL);
00601     return(T);}
00602   else if (isi) {
00603     register eusinteger_t *av=a->c.ivec.iv, *bv=b->c.ivec.iv;
00604     for (i=0; i<s; i++) if (av[i] < bv[i]) return(NIL);
00605     return(T);}
00606   else  error(E_FLOATVECTOR);}
00607 
00608 pointer MINIMALBOX(ctx,n,argv)
00609 register context *ctx;
00610 int n;
00611 pointer argv[];
00612 {
00613   eusfloat_t dx,dy,dz,err=0.0,diameter;
00614   register int i;
00615   register pointer a;
00616   pointer v;
00617   register eusfloat_t *f;
00618   register pointer vmin,vmax;
00619   numunion nu;
00620 
00621   ckarg2(3,4);
00622   a=argv[0];
00623   if (!islist(a)) error(E_NOLIST);
00624   v=ccar(a); a=ccdr(a);
00625   vmin=argv[1];
00626   if (!isfltvector(vmin)) error(E_FLOATVECTOR); 
00627   vmax=argv[2];
00628   if (!isfltvector(vmax)) error(E_FLOATVECTOR); 
00629   if (n==4) err=ckfltval(argv[3]);
00630   for (i=0; i<3; i++) vmin->c.fvec.fv[i]=vmax->c.fvec.fv[i]=v->c.fvec.fv[i];
00631   while (islist(a)) {
00632     v=ccar(a); a=ccdr(a);
00633     if (!isfltvector(v)) error(E_FLOATVECTOR);
00634     f=v->c.fvec.fv;
00635     if (f[0]<vmin->c.fvec.fv[0]) vmin->c.fvec.fv[0]=f[0];
00636     else if (f[0]>vmax->c.fvec.fv[0]) vmax->c.fvec.fv[0]=f[0];
00637     if (f[1]<vmin->c.fvec.fv[1]) vmin->c.fvec.fv[1]=f[1];
00638     else if (f[1]>vmax->c.fvec.fv[1]) vmax->c.fvec.fv[1]=f[1];
00639     if (f[2]<vmin->c.fvec.fv[2]) vmin->c.fvec.fv[2]=f[2];
00640     else if (f[2]>vmax->c.fvec.fv[2]) vmax->c.fvec.fv[2]=f[2];}
00641   dx= vmax->c.fvec.fv[0] - vmin->c.fvec.fv[0];
00642   dy= vmax->c.fvec.fv[1] - vmin->c.fvec.fv[1];
00643   dz= vmax->c.fvec.fv[2] - vmin->c.fvec.fv[2];
00644   diameter=sqrt(dx*dx + dy*dy + dz*dz);
00645   if (err!=0.0) {
00646     vmin->c.fvec.fv[0]-= diameter*err;
00647     vmin->c.fvec.fv[1]-= diameter*err;
00648     vmin->c.fvec.fv[2]-= diameter*err;
00649     vmax->c.fvec.fv[0]+= diameter*err;
00650     vmax->c.fvec.fv[1]+= diameter*err;
00651     vmax->c.fvec.fv[2]+= diameter*err; }
00652   return(makeflt(diameter));}
00653 
00654 pointer VMIN(ctx,n,argv)
00655 register context *ctx;
00656 int n;
00657 pointer argv[];
00658 { register int i,j,s;
00659   pointer r;
00660   register pointer v;
00661   register eusfloat_t *vf,*rf;
00662   register eusinteger_t *irf, *ivf;
00663   register int isf,isi;
00664 
00665   v=argv[0];
00666   if (!(isf=isfltvector(v)) && !(isi=isintvector(v))) error(E_FLOATVECTOR);
00667   s=vecsize(v);
00668   r=makevector(classof(v),s);
00669   rf=r->c.fvec.fv;
00670   for (i=0; i<s; i++) r->c.fvec.fv[i]=v->c.fvec.fv[i];
00671   if (isf) {
00672     for (i=1; i<n; i++) {
00673       v=argv[i];
00674       if (!isfltvector(v)) error(E_FLOATVECTOR);
00675       if (vecsize(v)!=s) error(E_VECINDEX);
00676       vf=v->c.fvec.fv;
00677       for (j=0; j<s; j++) if (vf[j]<rf[j]) rf[j]=vf[j]; } }
00678   else if (isi) {
00679     irf=r->c.ivec.iv;
00680     for (i=1; i<n; i++) {
00681       v=argv[i];
00682       if (!isintvector(v)) error(E_NOINTVECTOR);
00683       if (vecsize(v)!=s) error(E_VECINDEX);
00684       ivf=v->c.ivec.iv;
00685       for (j=0; j<s; j++) if (ivf[j]<irf[j]) irf[j]=ivf[j]; } }
00686   else error(E_NOSEQ);
00687   return(r);}
00688 
00689 pointer VMAX(ctx,n,argv)
00690 register context *ctx;
00691 int n;
00692 pointer argv[];
00693 { register int i,j,s;
00694   pointer r;
00695   register pointer v;
00696   register eusfloat_t *vf,*rf;
00697   register eusinteger_t *irf, *ivf;
00698   register int isf,isi;
00699 
00700   v=argv[0];
00701   if (!(isf=isfltvector(v)) && !(isi=isintvector(v))) error(E_FLOATVECTOR);
00702   s=vecsize(v);
00703   r=makevector(classof(v),s);
00704   rf=r->c.fvec.fv;
00705   for (i=0; i<s; i++) r->c.fvec.fv[i]=v->c.fvec.fv[i];
00706   if (isf) {
00707     for (i=1; i<n; i++) {
00708       v=argv[i];
00709       if (!isfltvector(v)) error(E_FLOATVECTOR);
00710       if (vecsize(v)!=s) error(E_VECINDEX);
00711       vf=v->c.fvec.fv;
00712       for (j=0; j<s; j++) if (vf[j]>rf[j]) rf[j]=vf[j]; } }
00713   else if (isi) {
00714     irf=r->c.ivec.iv;
00715     for (i=1; i<n; i++) {
00716       v=argv[i];
00717       if (!isintvector(v)) error(E_NOINTVECTOR);
00718       if (vecsize(v)!=s) error(E_VECINDEX);
00719       ivf=v->c.ivec.iv;
00720       for (j=0; j<s; j++) if (ivf[j]>irf[j]) irf[j]=ivf[j]; } }
00721   else error(E_NOSEQ);
00722   return(r);}
00723 
00724 
00725 /****************************************************************/
00726 /*      (m* m1 m2 [result])
00727 /*      (transform mat vector [result]) ; mat*vec
00728 /*      (transform vector mat [result]) ; vec*mat
00729 /****************************************************************/
00730 
00731 #define ismatrix(p) ((isarray(p) && \
00732                       p->c.ary.rank==makeint(2) && \
00733                       elmtypeof(p->c.ary.entity)==ELM_FLOAT))
00734 #define rowsize(p) (intval(p->c.ary.dim[0]))
00735 #define colsize(p) (intval(p->c.ary.dim[1]))
00736 
00737 /*
00738 float scaprod(v1,v2,s)
00739 pointer v1,v2;
00740 registerint s;
00741 { float sum=0;
00742   register int i;
00743   for (i=0; i<s; i++) sum+= v1->c.fvec.fv[i] * v2->c.fvec.fv[i];
00744   return(sum);}
00745 
00746 float vtrans(mat,n,vec,s)
00747 pointer mat,vec;
00748 int n,s;
00749 { pointer m;
00750   register int i;
00751   float f=0.0;
00752   for (i=0; i<s; i++) 
00753     f+ = mat->c.vec.v[i]->c.fvec.fv[n] * vec->c.fvec.fv[i];
00754   return(f);}
00755 */
00756 
00757 pointer MATTIMES(ctx,n,argv)
00758 register context *ctx;
00759 int n;
00760 register pointer argv[];
00761 { pointer rm;
00762   register int k,i,j,ii,jj,row1,column1,row2,column2;
00763   register eusfloat_t *fm1,*fm2,*fm;
00764   eusfloat_t *fv,x,fvv[256];
00765   fv = fvv;
00766 
00767   ckarg2(2,3);
00768   if (!ismatrix(argv[0]) || !ismatrix(argv[1])) error(E_NOVECTOR);
00769   fm1=argv[0]->c.ary.entity->c.fvec.fv;
00770   fm2=argv[1]->c.ary.entity->c.fvec.fv;
00771   row1=rowsize(argv[0]);        row2=rowsize(argv[1]);
00772   column1=colsize(argv[0]);     column2=colsize(argv[1]);
00773   if (column1!=row2) error(E_VECINDEX);
00774   if (n==3) {
00775     rm=argv[2];
00776     if (!ismatrix(rm)) error(E_NOVECTOR);
00777     if (row1!=rowsize(rm) || column2!=colsize(rm)) error(E_VECINDEX);
00778     }
00779   else rm=makematrix(ctx,row1,column2);
00780   if (row1>256 || column2>256){
00781     fv = (eusfloat_t *)malloc(sizeof(eusfloat_t) * ((row1>column2)?row1:column2));
00782     //error(E_VECINDEX);
00783   }
00784   fm=rm->c.ary.entity->c.fvec.fv;
00785   if (fm2!=fm) 
00786     for (i=0; i<row1; i++) {
00787       ii=i*column1;
00788       for (j=0; j<column2; j++) {
00789         x=0.0; jj=j;
00790         for (k=0; k<column1; k++) {
00791           x += fm1[ii+k] * fm2[jj];
00792           jj+=column2;}
00793         fv[j]=x;}
00794       ii=i*column2;
00795       for (j=0; j<column2; j++) fm[ii+j]=fv[j];}
00796   else 
00797     for (i=0; i<column2; i++) {
00798       for (j=0; j<row1; j++) {
00799         x=0.0; jj=j*column1; ii=0;
00800         for (k=0; k<column1; k++) {
00801           x += fm1[jj+k] * fm2[i+ii];
00802           ii+=column2;}
00803         fv[j]=x;}
00804       jj=0;
00805       for (j=0; j<row1; j++, jj+=column2) fm[i+jj]=fv[j];}
00806   if (fv!=fvv) free(fv);
00807   return(rm);}
00808 
00809 pointer TRANSFORM(ctx,n,argv)
00810 register context *ctx;
00811 int n;
00812 register pointer argv[];
00813 { register pointer result;
00814   register eusfloat_t *m,*v,x;
00815   eusfloat_t *fv,fvv[256];
00816   register int i,j,ii,s,s2;
00817   fv = fvv;
00818 
00819   ckarg2(2,3);
00820   if (ismatrix(argv[0]) && isfltvector(argv[1])) {
00821     m=argv[0]->c.ary.entity->c.fvec.fv;
00822     v=argv[1]->c.fvec.fv;
00823     s=colsize(argv[0]); s2=rowsize(argv[0]);
00824     if (s!=vecsize(argv[1])) error(E_VECINDEX); }
00825   else if (isfltvector(argv[0]) && ismatrix(argv[1])) {
00826     v=argv[0]->c.fvec.fv;
00827     m=argv[1]->c.ary.entity->c.fvec.fv;
00828     s2=colsize(argv[1]); s=rowsize(argv[1]);
00829     if (s!=vecsize(argv[0])) error(E_VECINDEX); }
00830   else error(E_NOVECTOR);
00831 
00832   if (n==3) {
00833     result=argv[2];
00834     if (!isfltvector(result)) error(E_NOVECTOR);
00835     if (s2!=vecsize(result)) error(E_VECINDEX);}
00836   else result=makefvector(s2);
00837   if (s2>256){
00838     fv = (eusfloat_t *)malloc(sizeof(eusfloat_t) * s2);
00839     // error(E_VECINDEX);
00840   }
00841 
00842   if (isfltvector(argv[0])) {   /* vec*mat */
00843     for (i=0; i<s2; i++) {
00844       x=0.0; ii=i;
00845       for (j=0; j<s; j++) { x+= m[ii] * v[j]; ii+=s2;}
00846       fv[i]=x;}}
00847   else {        /* mat*vec */ 
00848     for (i=0; i<s2; i++) {
00849       x=0.0; ii=i*s;
00850       for (j=0; j<s; j++) x+=m[ii+j]*v[j];
00851       fv[i]=x;} }
00852   for (i=0; i<s2; i++) result->c.fvec.fv[i]=fv[i];
00853   if (s2>256) free(fv);
00854   return(result);
00855   }
00856 
00857 /****************************************************************/
00858 /*      rotate vector
00859 /*      (rotate-vector fltvec theta :axis [result])
00860 /*      theta : radian
00861 /*      axis : one of :x,:y or :z or 0,1,2
00862 /****************************************************************/
00863 
00864 pointer ROTVEC(ctx,n,argv)
00865 register context *ctx;
00866 int n;
00867 pointer argv[];
00868 { register pointer vec,result,a;
00869   double theta,c,s1,s2;
00870   eusfloat_t f1,f2;
00871   register int k1,k2,k3,size;
00872   numunion nu;
00873 
00874   ckarg2(3,4);
00875   vec=argv[0];
00876   if (!isfltvector(vec)) error(E_FLOATVECTOR);
00877   size=vecsize(vec);
00878   theta=ckfltval(argv[1]);
00879   c=cos(theta); s1=sin(theta);
00880   a=argv[2];
00881   if (a==K_X || a==makeint(0)) {  k1=1; k2=2; k3=0; s2=s1; s1= -s1;}
00882   else if (a==K_Y || a==makeint(1)) { k1=0; k2=2; k3=1; s2= -s1;}
00883   else if (a==K_Z || a==makeint(2)) { k1=0; k2=1; k3=2; s2=s1; s1= -s1;}
00884   else if (a==NIL) { k1=0; k2=1; s2=s1; s1= -s1;}
00885   else error(E_ROTAXIS);
00886   if (n==4) {
00887     result=argv[3];
00888     if (!isfltvector(result)) error(E_FLOATVECTOR); }
00889   else result=makefvector(size);
00890   f1=vec->c.fvec.fv[k1];
00891   f2=vec->c.fvec.fv[k2];
00892   result->c.fvec.fv[k1]=c*f1+s1*f2;
00893   result->c.fvec.fv[k2]=s2*f1+c*f2;
00894   if (a!=NIL) result->c.fvec.fv[k3]=vec->c.fvec.fv[k3];
00895   return(result);}
00896 
00897 /****************************************************************/
00898 /*      rotate matrix
00899 /*      (rotate-matrix mat theta :axis [worldp] [result])
00900 /*              theta : radian
00901 /*              axis : one of :x,:y or :z or 0,1,2
00902 /*              worldp: nil--local(mult rot mat from the right)
00903 /*                      t----world(mult rot mat from the left)
00904 /****************************************************************/
00905 static void copymat(dest,src,size)
00906 pointer dest,src;
00907 register int size;
00908 { register int i;
00909   register eusfloat_t *rv=dest->c.ary.entity->c.fvec.fv;
00910   register eusfloat_t *mv=src->c.ary.entity->c.fvec.fv;
00911   size=size*size;
00912   for (i=0; i<size; i++) rv[i]=mv[i]; }
00913 
00914 pointer ROTMAT(ctx,n,argv)
00915 register context *ctx;
00916 int n;                  /* rot theata axis worldp result */
00917 pointer argv[];
00918 { register pointer mat,result,a;
00919   double theta,c,s1,s2;
00920   eusfloat_t *m1,*m2,f1,f2;
00921   register eusfloat_t *rm,*m;
00922   register int i,size,k1,k2,worldp=0,ss;
00923   numunion nu;
00924 
00925   ckarg2(3,5);
00926   mat=argv[0];
00927   if (!ismatrix(mat)) error(E_NOVECTOR);
00928   size=colsize(mat);
00929   theta=ckfltval(argv[1]);
00930   c=cos(theta); s1=sin(theta);
00931 
00932   a=argv[2];
00933   if (n>=4) worldp=(argv[3]!=NIL);      /*0:local 1:world*/
00934 
00935   if (n==5) {
00936     result=argv[4];
00937     if (!ismatrix(result)) error(E_NOVECTOR); }
00938   else result=makematrix(ctx,size,size);
00939   rm=result->c.ary.entity->c.fvec.fv;
00940   m=mat->c.ary.entity->c.fvec.fv;
00941 
00942   if (a==K_X || a==makeint(0)) {
00943     k1=1; k2=2;
00944     if (worldp) { s2=s1; s1= -s1;}
00945     else s2= -s1;}
00946   else if (a==MK_X) {
00947     k1=1; k2=2;
00948     if (worldp) s2= -s1;
00949     else { s2=s1; s1= -s1;}}
00950   else if (a==K_Y || a==makeint(1)) {
00951     k1=0; k2=2;
00952     if (worldp) s2= -s1;
00953     else { s2=s1; s1= -s1;}}
00954   else if (a==MK_Y) {
00955     k1=0; k2=2;
00956     if (worldp) { s2=s1; s1= -s1;}
00957     else s2= -s1;}
00958   else if (a==K_Z || a==makeint(2)) {
00959     k1=0; k2=1;
00960     if (worldp) { s2=s1; s1= -s1;}
00961     else s2= -s1;}
00962   else if (a==MK_Z) {
00963     k1=0; k2=1;
00964     if (worldp) s2= -s1;
00965     else { s2=s1; s1= -s1;}}
00966   else if (a==NIL) { /* 2dimensional m.i */
00967     eusfloat_t m0,m1,m2,m3;
00968     m0=c*m[0]-s1*m[2];  m1=c*m[1]-s1*m[3];
00969     m2=s1*m[0]+c*m[2];  m3=s1*m[1]+c*m[3];
00970     rm[0]=m0; rm[1]=m1; rm[2]=m2; rm[3]=m3;
00971     return(result);   }
00972   else error(E_ROTAXIS);
00973 
00974   ss=size*size;
00975   if (mat!=result) for (i=0; i<ss; i++) rm[i]=m[i];
00976   for (i=0; i<size; i++) {
00977     if (worldp) {
00978       m1= &rm[k1*size+i];
00979       m2= &rm[k2*size+i];}
00980     else {
00981       m1= &rm[i*size+k1];
00982       m2= &rm[i*size+k2];}
00983     f1= (*m1)*c + (*m2)*s1;
00984     f2= (*m1)*s2 + (*m2)*c;
00985     *m1=f1; *m2=f2;}
00986   return(result);}
00987 
00988 /*      (rotation theta floatvector [result])
00989 /*      make rotation matrix along arbitrary axis
00990 /*      Axis is given by floatvector, it need not to be normalized.
00991 */
00992 pointer ROTATION_MATRIX(ctx,n,argv)
00993 register context *ctx;
00994 int n;
00995 pointer *argv;
00996 { register pointer a,result;
00997   register eusfloat_t *rm;
00998   register int size;
00999   double x, y, z, s;
01000   double cs, sn, vers, xv, yv,zv, xyv, yzv, zxv, xs, ys, zs, norm;
01001   numunion nu;
01002 
01003   ckarg2(1,3);
01004   s=ckfltval(argv[0]);
01005   cs = cos(s); sn = sin(s); vers = 1.0 - cs;
01006   a=argv[1];
01007   if (n==1 || (n==2 && ismatrix(a))) {  /*2D rot. matrix*/
01008     if (n==2) result=a;
01009     else result=makematrix(ctx,2,2);
01010     rm=result->c.ary.entity->c.fvec.fv;
01011     rm[0]=rm[3]=cs; rm[1]=-sn; rm[2]=sn;
01012     return(result);}
01013   if (a==NIL) size=2; else size=3;
01014   if (n==3) { result=argv[2]; if (!ismatrix(result)) error(E_NOVECTOR);}
01015   else result=makematrix(ctx,size,size);
01016   rm=result->c.ary.entity->c.fvec.fv;
01017   if (isfltvector(a)) {
01018     if (size<=2) error(E_VECINDEX);
01019     x=a->c.fvec.fv[0]; y=a->c.fvec.fv[1]; z=a->c.fvec.fv[2];
01020     norm = sqrt(x*x + y*y + z*z);
01021     if (fabs(norm)<0.00001) return(NIL); /*error(E_USER,(pointer)"too small axis vector");*/
01022     x = x/norm; y = y/norm; z= z/norm;
01023     xv = x*x*vers; yv = y*y*vers; zv = z*z*vers;
01024     xyv = x*y*vers; yzv = y*z*vers; zxv = z*x*vers;
01025     xs = x*sn; ys = y*sn; zs = z*sn;
01026     rm[0] = xv + cs;
01027     rm[1] = xyv - zs;
01028     rm[2] = zxv + ys;
01029     rm[size] = xyv + zs;
01030     rm[size+1] = yv + cs;
01031     rm[size+2] = yzv - xs;
01032     rm[size+size] = zxv - ys;
01033     rm[size+size+1] = yzv + xs;
01034     rm[size+size+2] = zv + cs;
01035     return(result);}
01036   else {
01037     if (size==2) {
01038       rm[0]=rm[3]=cs; rm[1]=-sn; rm[2]=sn;
01039       return(result);}
01040     for (size=0; size<9; size++) rm[size]=0.0;
01041     if (a==makeint(0) || a==K_X) {
01042       rm[0]=1.0; rm[4]=cs; rm[5]= -sn; rm[7]=sn; rm[8]=cs;}
01043     else if (a==MK_X) {
01044       rm[0]=1.0; rm[4]=cs; rm[5]= sn; rm[7]=-sn; rm[8]=cs;}
01045     else if (a==makeint(1) || a==K_Y) {
01046       rm[0]=cs; rm[2]=sn; rm[4]=1.0; rm[6]= -sn; rm[8]=cs;}
01047     else if (a==MK_Y) {
01048       rm[0]=cs; rm[2]=-sn; rm[4]=1.0; rm[6]= sn; rm[8]=cs;}
01049     else if (a==makeint(2) || a==K_Z) {
01050       rm[0]=cs; rm[1]= -sn; rm[3]=sn; rm[4]=cs; rm[8]=1.0;}
01051     else if (a==MK_Z) {
01052       rm[0]=cs; rm[1]= sn; rm[3]=-sn; rm[4]=cs; rm[8]=1.0;}
01053     else error(E_NOVECTOR);
01054     return(result);} }
01055 
01056 pointer ROTANGLE(ctx,n,argv)    /*equivalent rotation axis and angle*/
01057 register context *ctx;
01058 int n;
01059 pointer argv[];
01060 { pointer r=argv[0];
01061   int size;
01062   eusfloat_t *m,*krv,kx,ky,kz;
01063   eusfloat_t th,t1,t2,t3;
01064   eusfloat_t cs,cs2,vers,sn2,norm;
01065   pointer kr;
01066   numunion nu;
01067 
01068   ckarg(1);
01069   if (!ismatrix(r)) error(E_NOVECTOR);
01070   size=colsize(r);
01071   m=r->c.ary.entity->c.fvec.fv;
01072 
01073   if (size==2) {        /*2D rotation*/
01074     th=atan2(m[2],m[0]);
01075     return(makeflt(th));}
01076 
01077   kr=makefvector(3);    /*axis vector*/
01078   krv=kr->c.fvec.fv;
01079   t1=m[size+size+1]-m[size+2];
01080   t2=m[2]          -m[size+size];
01081   t3=m[size]       -m[1];
01082   cs2=m[0]+m[size+1]+m[size+size+2]-1.0;
01083   cs=cs2/2;
01084   vers=1-cs;
01085   sn2=sqrt(t1*t1 + t2*t2 + t3*t3);
01086   th=atan2(sn2,cs2);
01087   if (th<1e-10||vers<1e-10) return(NIL);
01088   else if (th<2.6) {
01089     kx=(m[size+size+1]-m[size+2])/sn2;
01090     ky=(m[2]-m[size+size])/sn2;
01091     kz=(m[size]-m[1])/sn2;
01092     }
01093   else {
01094     kx=sqrt((m[0]-cs)/vers);
01095     if (m[size+size+1]-m[size+2]<0) kx= -kx;
01096     ky=sqrt((m[size+1]-cs)/vers);
01097     if (m[2]-m[size+size]<0) ky= -ky;
01098     kz=sqrt((m[size+size+2]-cs)/vers);
01099     if (m[size]-m[1]<0) kz= -kz;
01100     }
01101   
01102   if (debug) 
01103     printf("rotation-angle1: %f %f %f\n",kx,ky,kz);
01104   if ((fabs(kx) > fabs(ky)) && (fabs(kx) > fabs(kz))) {
01105     ky=(m[size]+m[1])/(2*kx*vers); kz=(m[2]+m[size+size])/(2*kx*vers);
01106     norm=sqrt((ky*ky+kz*kz)/(1.0-kx*kx)); if (!isnan(norm)) {ky/=norm; kz/=norm;}}
01107   else if ((fabs(ky) > fabs(kx)) && (fabs(ky) > fabs(kz))) {
01108     kx=(m[size]+m[1])/(2*ky*vers); kz=(m[size+2]+m[size+size+1])/(2*ky*vers);
01109     norm=sqrt((kx*kx+kz*kz)/(1.0-ky*ky)); if (!isnan(norm)) {kx/=norm; kz/=norm;}}
01110   else {
01111     kx=(m[2]+m[size+size])/(2*kz*vers);
01112     ky=(m[size+2]+m[size+size+1])/(2*kz*vers);
01113     norm=sqrt((kx*kx+ky*ky)/(1.0-kz*kz)); if (!isnan(norm)) {kx/=norm; ky/=norm;}}
01114   
01115   norm=sqrt(kx*kx + ky*ky + kz*kz);
01116   if (debug) 
01117     printf("rotation-angle2: %f %f %f norm=%f\n",kx,ky,kz,norm);
01118   krv[0]=kx/norm; krv[1]=ky/norm; krv[2]=kz/norm;
01119   kx=kx/norm; ky=ky/norm; kz=kz/norm;
01120   norm=sqrt(kx*kx + ky*ky + kz*kz);
01121   return(cons(ctx,makeflt(th),cons(ctx,kr,NIL)));}
01122 
01123              
01124 /*      (transpose mat [result])
01125 */
01126 pointer TRANSPOSE(ctx,n,argv)
01127 register context *ctx;
01128 int n;
01129 pointer argv[];
01130 { register eusfloat_t *rm,*m,x;
01131   pointer mat,result;
01132   register int i,j,row,column;
01133 
01134   ckarg2(1,2);
01135   mat=argv[0];
01136   if (!ismatrix(mat)) error(E_NOVECTOR);
01137   column=colsize(mat); row=rowsize(mat);
01138   if (n==2) {
01139     result=argv[1];
01140     if (!ismatrix(result)) error(E_NOVECTOR);
01141     if (rowsize(result)!=column || colsize(result)!=row) error(E_VECINDEX);}
01142   else result=makematrix(ctx,column,row);
01143   rm=result->c.ary.entity->c.fvec.fv;
01144   m=mat->c.ary.entity->c.fvec.fv;
01145   if (result==mat) /*square matrix*/
01146     for (i=0; i<row; i++) 
01147       for (j=0; j<=i; j++) {
01148         x=m[j*column+i];
01149         rm[j*column+i]=m[column*i+j];
01150         rm[column*i+j]=x;}
01151   else 
01152     for (i=0; i<row; i++) 
01153       for (j=0; j<column; j++) rm[j*row+i]=m[i*column+j];
01154   return(result);}
01155 
01156 /* get roll, pitch yaw angles out of rotation matrix*/
01157 pointer INV_RPY(ctx,n,argv)
01158 register context *ctx;
01159 int n;
01160 pointer argv[];
01161 { pointer mat;
01162   eusfloat_t a, b, c, sa, ca;
01163   register eusfloat_t *mv;
01164   pointer result,result2;
01165   numunion nu;
01166 
01167   ckarg(1);
01168   mat=argv[0];
01169   if (!ismatrix(mat)) error(E_NOVECTOR);
01170   if (colsize(mat)!=3) error(E_VECINDEX);
01171   mv=mat->c.ary.entity->c.fvec.fv;
01172   a = atan2(mv[3],mv[0]);
01173   sa = sin(a); ca = cos(a);
01174   b = atan2(-mv[6], ca*mv[0] + sa*mv[3]);
01175   c = atan2(sa*mv[2] - ca*mv[5], -sa*mv[1] + ca*mv[4]);
01176   result = cons(ctx,makeflt(c), NIL);
01177   result = cons(ctx,makeflt(b), result);
01178   result = cons(ctx,makeflt(a), result);
01179   a=a + M_PI;
01180   sa = sin(a); ca = cos(a);
01181   b = atan2(-mv[6], ca*mv[0] + sa*mv[3]);
01182   c = atan2(sa*mv[2] - ca*mv[5], -sa*mv[1] + ca*mv[4]);
01183   result2 = cons(ctx,makeflt(c), NIL);
01184   result2 = cons(ctx,makeflt(b), result2);
01185   result2 = cons(ctx,makeflt(a), result2);
01186   result2 = cons(ctx,result2,NIL);
01187   return(cons(ctx,result,result2));}
01188 
01189 pointer INV_EULER(ctx,n,argv)
01190 register context *ctx;
01191 int n;
01192 pointer argv[];
01193 { pointer mat;
01194   eusfloat_t a, b, c, sa, ca;
01195   register eusfloat_t *mv;
01196   pointer result,result2;
01197   numunion nu;
01198 
01199   ckarg(1);
01200   mat=argv[0];
01201   if (!ismatrix(mat)) error(E_NOVECTOR);
01202   if (colsize(mat)!=3) error(E_VECINDEX);
01203   mv=mat->c.ary.entity->c.fvec.fv;
01204   if (-0.00001<mv[5] && mv[5]<0.00001 && -0.00001<mv[2] && mv[2]<0.00001) a=0.0;
01205   else a = atan2(mv[5],mv[2]);
01206   sa = sin(a); ca = cos(a);
01207   b = atan2(ca*mv[2]+sa*mv[5],mv[8]);
01208   c = atan2(-sa*mv[0]+ca*mv[3], -sa*mv[1] + ca*mv[4]);
01209   result = cons(ctx,makeflt(c), NIL);
01210   result = cons(ctx,makeflt(b), result);
01211   result = cons(ctx,makeflt(a), result);
01212   a=a + M_PI;
01213   sa = sin(a); ca = cos(a);
01214   b = atan2(ca*mv[2]+sa*mv[5],mv[8]);
01215   c = atan2(-sa*mv[0]+ca*mv[3], -sa*mv[1] + ca*mv[4]);
01216   result2 = cons(ctx,makeflt(c), NIL);
01217   result2 = cons(ctx,makeflt(b), result2);
01218   result2 = cons(ctx,makeflt(a), result2);
01219   result2 = cons(ctx,result2,NIL);
01220   return(cons(ctx,result,result2));}
01221 
01222 /****************************************************************/
01223 /*
01224 /* simultaneous equation solver by LU decomposition method
01225 /* 
01226 /*      1987-July-1
01227 /*      Copyright Toshihiro MATSUI
01228 /*
01229 /****************************************************************/
01230 
01231 #define elm(a,i,j) (a->c.vec.v[j]->c.fvec.fv[i])
01232 #define EPS (1.0E-10)
01233 
01234 static int decompose(a,s,p)
01235 register eusfloat_t *a;
01236 register int s;
01237 eusinteger_t p[];
01238 {
01239   register int i,j,k,l;
01240   eusfloat_t al,bl;
01241 
01242   for (i=0; i<s; i++) p[i]=i;
01243   for (k=0; k<s; k++) {
01244     /*select pivot*/
01245     l=k;
01246     al=fabs(a[p[l]*s+k]);
01247     for (i=k+1; i<s; i++) 
01248       if ((bl=fabs(a[p[i]*s+k])) > al) { l=i; al=bl;}
01249     if (l!=k) {
01250       /*change rows*/
01251       j=p[k]; p[k]=p[l]; p[l]=j; }
01252     if (al<EPS) return(-1);     /* singular*/
01253     /* gauss elimination */
01254     a[p[k]*s+k]= 1.0/a[p[k]*s+k];
01255     for (i=k+1; i<s; i++) {
01256       al=a[p[i]*s+k]=a[p[i]*s+k]*a[p[k]*s+k];
01257       for (j=k+1; j<s; j++) a[p[i]*s+j] -= al*a[p[k]*s+j];
01258       }
01259     }
01260   return(0);
01261   }
01262 
01263 pointer LU_DECOMPOSE(ctx,n,argv)
01264 register context *ctx;
01265 int n;
01266 pointer argv[];
01267 /* (LU-DECOMPOSE mat [result]) */
01268 { pointer a,result,pv;
01269   int s,stat;
01270 
01271   ckarg2(1,2);
01272   a=argv[0];
01273   if (!ismatrix(a)) error(E_NOVECTOR);
01274   s=colsize(a);
01275   if (s!=rowsize(a)) error(E_VECSIZE);
01276   if (n==1) result=a;
01277   else {
01278     result=argv[1];
01279     if (!ismatrix(result)) error(E_NOVECTOR);
01280     if (s!=colsize(result)) error(E_VECSIZE);
01281     copymat(result,a,s); }
01282   pv=makevector(C_VECTOR,s);
01283   stat=decompose(result->c.ary.entity->c.fvec.fv,s,(eusinteger_t *)pv->c.vec.v);
01284   while (--s>=0) pv->c.vec.v[s]=makeint((eusinteger_t)pv->c.vec.v[s]);
01285   if (stat<0) return(NIL);
01286   else return(pv);}
01287 
01288 static void solve(a,pv,s,b,x)
01289 register eusfloat_t *a;
01290 pointer b,x,pv;
01291 int s;
01292 { register int i,j;
01293   eusfloat_t t;
01294   register pointer *p;
01295   p=pv->c.vec.v;
01296 
01297   /*forward substitution*/
01298   for (i=0; i<s; i++) {
01299     t=b->c.fvec.fv[intval(p[i])];
01300     for (j=0; j<i; j++) t-= a[intval(p[i])*s+j]*x->c.fvec.fv[j];
01301     x->c.fvec.fv[i]=t;}
01302   /*backward substitution*/
01303   for (i=s-1; i>=0; i--) {
01304     t=x->c.fvec.fv[i];
01305     for (j=i+1; j<s; j++) t-= a[intval(p[i])*s+j]*x->c.fvec.fv[j];
01306     x->c.fvec.fv[i]=t*a[intval(p[i])*s+i];}
01307 }
01308 
01309 pointer LU_SOLVE(ctx,n,argv)
01310 register context *ctx;
01311 int n;
01312 pointer argv[];
01313 { pointer a,p,b,x;
01314   int s;
01315 
01316   ckarg2(3,4);
01317   a=argv[0];  p=argv[1]; b=argv[2];
01318   if (!ismatrix(a)) error(E_NOVECTOR);
01319   s=colsize(a);
01320   if (!isvector(p) || !isfltvector(b)) error(E_NOVECTOR);
01321   if (s!=vecsize(p) || s!=vecsize(b)) error(E_VECSIZE);
01322   if (n==4) {
01323     x=argv[3];
01324     if (!isvector(x)) error(E_NOVECTOR);
01325     if (s!=vecsize(x)) error(E_VECSIZE); }
01326   else  x=(pointer)makefvector(s);
01327   solve(a->c.ary.entity->c.fvec.fv,p,s,b,x);
01328   return(x);}
01329 
01330 pointer LU_DETERMINANT(ctx,n,argv)
01331 register context *ctx;
01332 int n;
01333 pointer argv[];
01334 { register pointer a,p;
01335   register eusfloat_t *av;
01336   register int i,s;
01337   eusfloat_t val=1.0;
01338   numunion nu;
01339   ckarg(2);
01340   a=argv[0]; p=argv[1];
01341   if (!isvector(p) || !ismatrix(a)) error(E_NOVECTOR);
01342   s=vecsize(p);
01343   if (colsize(a)!=s || rowsize(a)!=s) error(E_VECSIZE);
01344   av=a->c.ary.entity->c.fvec.fv;
01345   for (i=0; i<s; i++) val*=av[intval(p->c.vec.v[i])*s+i];
01346   return(makeflt(1.0/val));}
01347 
01348 void matrix(ctx,mod)
01349 register context *ctx;
01350 register pointer mod;
01351 {
01352   K_X=defkeyword(ctx,"X");
01353   K_Y=defkeyword(ctx,"Y");
01354   K_Z=defkeyword(ctx,"Z");
01355   MK_X=defkeyword(ctx,"-X");
01356   MK_Y=defkeyword(ctx,"-Y");
01357   MK_Z=defkeyword(ctx,"-Z");
01358 
01359   defun(ctx,"V+",mod,VPLUS,NULL);
01360   defun(ctx,"V++",mod,VPLUSPLUS,NULL);
01361   defun(ctx,"V-",mod,VMINUS,NULL);
01362   defun(ctx,"V-ABS",mod,VMINUS_ABS,NULL);
01363   defun(ctx,"V.",mod,VINNERPRODUCT,NULL);
01364   defun(ctx,"V*",mod,VCROSSPRODUCT,NULL);
01365   defun(ctx,"V.*",mod,SCA3PROD,NULL);
01366   defun(ctx,"V<",mod,VLESSP,NULL);
01367   defun(ctx,"V>",mod,VGREATERP,NULL);
01368   defun(ctx,"VMIN",mod,VMIN,NULL);
01369   defun(ctx,"VMAX",mod,VMAX,NULL);
01370   defun(ctx,"MINIMAL-BOX",mod,MINIMALBOX,NULL);
01371   defun(ctx,"SCALE",mod,SCALEVEC,NULL);
01372   defun(ctx,"NORM",mod,VNORM,NULL);
01373   defun(ctx,"NORM2",mod,VNORM2,NULL);
01374   defun(ctx,"NORMALIZE-VECTOR",mod,VNORMALIZE,NULL);
01375   defun(ctx,"DISTANCE",mod,VDISTANCE,NULL);
01376   defun(ctx,"DISTANCE2",mod,VDISTANCE2,NULL);
01377   defun(ctx,"DIRECTION",mod,VDIRECTION,NULL);
01378   defun(ctx,"MIDPOINT",mod,MIDPOINT,NULL);
01379 /*  defun(ctx,"TRIANGLE",mod,TRIANGLE,NULL); */
01380   defun(ctx,"FLOATVECTOR",mod,MKFLTVEC,NULL);
01381   defun(ctx,"FLOAT-VECTOR",mod,MKFLTVEC,NULL);
01382   defun(ctx,"TRANSFORM",mod,TRANSFORM,NULL);
01383   defun(ctx,"M*",mod,MATTIMES,NULL);
01384   defun(ctx,"ROTATE-VECTOR",mod,ROTVEC,NULL);
01385   defun(ctx,"ROTATE-MATRIX",mod,ROTMAT,NULL);
01386   defun(ctx,"ROTATION-MATRIX",mod,ROTATION_MATRIX,NULL);
01387   defun(ctx,"ROTATION-ANGLE",mod,ROTANGLE,NULL);
01388   defun(ctx,"TRANSPOSE",mod,TRANSPOSE,NULL);
01389   defun(ctx,"RPY-ANGLE",mod,INV_RPY,NULL);
01390   defun(ctx,"EULER-ANGLE",mod,INV_EULER,NULL);
01391   defun(ctx,"LU-DECOMPOSE",mod,LU_DECOMPOSE,NULL);
01392   defun(ctx,"LU-SOLVE",mod,LU_SOLVE,NULL);
01393   defun(ctx,"LU-DETERMINANT",mod,LU_DETERMINANT,NULL);}


euslisp
Author(s): Toshihiro Matsui
autogenerated on Thu Jun 6 2019 18:05:53