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++) fv[i]=ckfltval(argv[i]);
00542   return(r);
00543   }
00544 
00545 /* comparation of vectors */
00546 
00547 pointer VLESSP(ctx,n,argv)
00548 register context *ctx;
00549 int n;
00550 pointer argv[];
00551 { register int i,s;
00552   register pointer a,b;
00553   register int isf,isi;
00554   ckarg(2);
00555   a=argv[0]; b=argv[1];
00556   if (!((isf=isfltvector(a))&&isfltvector(b)) &&
00557       !((isi=isintvector(a))&&isintvector(b))) error(E_FLOATVECTOR);
00558   s=ckvsize(a,b);
00559   if (isf) {
00560     register eusfloat_t *av=a->c.fvec.fv, *bv=b->c.fvec.fv;
00561     for (i=0; i<s; i++) if (av[i] > bv[i]) return(NIL);
00562     return(T);}
00563   else if (isi) {
00564     register eusinteger_t *av=a->c.ivec.iv, *bv=b->c.ivec.iv;
00565     for (i=0; i<s; i++) if (av[i] > bv[i]) return(NIL);
00566     return(T);}
00567   else  error(E_FLOATVECTOR);}
00568 
00569 pointer VGREATERP(ctx,n,argv)
00570 register context *ctx;
00571 int n;
00572 pointer argv[];
00573 /*every element of argv[0] is greater than corresponding elem of arg[1]*/
00574 { register int i,s;
00575   register pointer a,b;
00576   register int isf,isi;
00577   ckarg(2);     
00578   a=argv[0]; b=argv[1];
00579   if (!((isf=isfltvector(a))&&isfltvector(b)) &&
00580       !((isi=isintvector(a))&&isintvector(b))) error(E_FLOATVECTOR);
00581   s=ckvsize(a,b);
00582   if (isf) {
00583     register eusfloat_t *av=a->c.fvec.fv, *bv=b->c.fvec.fv;
00584     for (i=0; i<s; i++) if (av[i] < bv[i]) return(NIL);
00585     return(T);}
00586   else if (isi) {
00587     register eusinteger_t *av=a->c.ivec.iv, *bv=b->c.ivec.iv;
00588     for (i=0; i<s; i++) if (av[i] < bv[i]) return(NIL);
00589     return(T);}
00590   else  error(E_FLOATVECTOR);}
00591 
00592 pointer MINIMALBOX(ctx,n,argv)
00593 register context *ctx;
00594 int n;
00595 pointer argv[];
00596 {
00597   eusfloat_t dx,dy,dz,err=0.0,diameter;
00598   register int i;
00599   register pointer a;
00600   pointer v;
00601   register eusfloat_t *f;
00602   register pointer vmin,vmax;
00603   numunion nu;
00604 
00605   ckarg2(3,4);
00606   a=argv[0];
00607   if (!islist(a)) error(E_NOLIST);
00608   v=ccar(a); a=ccdr(a);
00609   vmin=argv[1];
00610   if (!isfltvector(vmin)) error(E_FLOATVECTOR); 
00611   vmax=argv[2];
00612   if (!isfltvector(vmax)) error(E_FLOATVECTOR); 
00613   if (n==4) err=ckfltval(argv[3]);
00614   for (i=0; i<3; i++) vmin->c.fvec.fv[i]=vmax->c.fvec.fv[i]=v->c.fvec.fv[i];
00615   while (islist(a)) {
00616     v=ccar(a); a=ccdr(a);
00617     if (!isfltvector(v)) error(E_FLOATVECTOR);
00618     f=v->c.fvec.fv;
00619     if (f[0]<vmin->c.fvec.fv[0]) vmin->c.fvec.fv[0]=f[0];
00620     else if (f[0]>vmax->c.fvec.fv[0]) vmax->c.fvec.fv[0]=f[0];
00621     if (f[1]<vmin->c.fvec.fv[1]) vmin->c.fvec.fv[1]=f[1];
00622     else if (f[1]>vmax->c.fvec.fv[1]) vmax->c.fvec.fv[1]=f[1];
00623     if (f[2]<vmin->c.fvec.fv[2]) vmin->c.fvec.fv[2]=f[2];
00624     else if (f[2]>vmax->c.fvec.fv[2]) vmax->c.fvec.fv[2]=f[2];}
00625   dx= vmax->c.fvec.fv[0] - vmin->c.fvec.fv[0];
00626   dy= vmax->c.fvec.fv[1] - vmin->c.fvec.fv[1];
00627   dz= vmax->c.fvec.fv[2] - vmin->c.fvec.fv[2];
00628   diameter=sqrt(dx*dx + dy*dy + dz*dz);
00629   if (err!=0.0) {
00630     vmin->c.fvec.fv[0]-= diameter*err;
00631     vmin->c.fvec.fv[1]-= diameter*err;
00632     vmin->c.fvec.fv[2]-= diameter*err;
00633     vmax->c.fvec.fv[0]+= diameter*err;
00634     vmax->c.fvec.fv[1]+= diameter*err;
00635     vmax->c.fvec.fv[2]+= diameter*err; }
00636   return(makeflt(diameter));}
00637 
00638 pointer VMIN(ctx,n,argv)
00639 register context *ctx;
00640 int n;
00641 pointer argv[];
00642 { register int i,j,s;
00643   pointer r;
00644   register pointer v;
00645   register eusfloat_t *vf,*rf;
00646   register eusinteger_t *irf, *ivf;
00647   register int isf,isi;
00648 
00649   v=argv[0];
00650   if (!(isf=isfltvector(v)) && !(isi=isintvector(v))) error(E_FLOATVECTOR);
00651   s=vecsize(v);
00652   r=makevector(classof(v),s);
00653   rf=r->c.fvec.fv;
00654   for (i=0; i<s; i++) r->c.fvec.fv[i]=v->c.fvec.fv[i];
00655   if (isf) {
00656     for (i=1; i<n; i++) {
00657       v=argv[i];
00658       if (!isfltvector(v)) error(E_FLOATVECTOR);
00659       if (vecsize(v)!=s) error(E_VECINDEX);
00660       vf=v->c.fvec.fv;
00661       for (j=0; j<s; j++) if (vf[j]<rf[j]) rf[j]=vf[j]; } }
00662   else if (isi) {
00663     irf=r->c.ivec.iv;
00664     for (i=1; i<n; i++) {
00665       v=argv[i];
00666       if (!isintvector(v)) error(E_NOINTVECTOR);
00667       if (vecsize(v)!=s) error(E_VECINDEX);
00668       ivf=v->c.ivec.iv;
00669       for (j=0; j<s; j++) if (ivf[j]<irf[j]) irf[j]=ivf[j]; } }
00670   else error(E_NOSEQ);
00671   return(r);}
00672 
00673 pointer VMAX(ctx,n,argv)
00674 register context *ctx;
00675 int n;
00676 pointer argv[];
00677 { register int i,j,s;
00678   pointer r;
00679   register pointer v;
00680   register eusfloat_t *vf,*rf;
00681   register eusinteger_t *irf, *ivf;
00682   register int isf,isi;
00683 
00684   v=argv[0];
00685   if (!(isf=isfltvector(v)) && !(isi=isintvector(v))) error(E_FLOATVECTOR);
00686   s=vecsize(v);
00687   r=makevector(classof(v),s);
00688   rf=r->c.fvec.fv;
00689   for (i=0; i<s; i++) r->c.fvec.fv[i]=v->c.fvec.fv[i];
00690   if (isf) {
00691     for (i=1; i<n; i++) {
00692       v=argv[i];
00693       if (!isfltvector(v)) error(E_FLOATVECTOR);
00694       if (vecsize(v)!=s) error(E_VECINDEX);
00695       vf=v->c.fvec.fv;
00696       for (j=0; j<s; j++) if (vf[j]>rf[j]) rf[j]=vf[j]; } }
00697   else if (isi) {
00698     irf=r->c.ivec.iv;
00699     for (i=1; i<n; i++) {
00700       v=argv[i];
00701       if (!isintvector(v)) error(E_NOINTVECTOR);
00702       if (vecsize(v)!=s) error(E_VECINDEX);
00703       ivf=v->c.ivec.iv;
00704       for (j=0; j<s; j++) if (ivf[j]>irf[j]) irf[j]=ivf[j]; } }
00705   else error(E_NOSEQ);
00706   return(r);}
00707 
00708 
00709 /****************************************************************/
00710 /*      (m* m1 m2 [result])
00711 /*      (transform mat vector [result]) ; mat*vec
00712 /*      (transform vector mat [result]) ; vec*mat
00713 /****************************************************************/
00714 
00715 #define ismatrix(p) ((isarray(p) && \
00716                       p->c.ary.rank==makeint(2) && \
00717                       elmtypeof(p->c.ary.entity)==ELM_FLOAT))
00718 #define rowsize(p) (intval(p->c.ary.dim[0]))
00719 #define colsize(p) (intval(p->c.ary.dim[1]))
00720 
00721 /*
00722 float scaprod(v1,v2,s)
00723 pointer v1,v2;
00724 registerint s;
00725 { float sum=0;
00726   register int i;
00727   for (i=0; i<s; i++) sum+= v1->c.fvec.fv[i] * v2->c.fvec.fv[i];
00728   return(sum);}
00729 
00730 float vtrans(mat,n,vec,s)
00731 pointer mat,vec;
00732 int n,s;
00733 { pointer m;
00734   register int i;
00735   float f=0.0;
00736   for (i=0; i<s; i++) 
00737     f+ = mat->c.vec.v[i]->c.fvec.fv[n] * vec->c.fvec.fv[i];
00738   return(f);}
00739 */
00740 
00741 pointer MATTIMES(ctx,n,argv)
00742 register context *ctx;
00743 int n;
00744 register pointer argv[];
00745 { pointer rm;
00746   register int k,i,j,ii,jj,row1,column1,row2,column2;
00747   register eusfloat_t *fm1,*fm2,*fm;
00748   eusfloat_t *fv,x,fvv[256];
00749   fv = fvv;
00750 
00751   ckarg2(2,3);
00752   if (!ismatrix(argv[0]) || !ismatrix(argv[1])) error(E_NOVECTOR);
00753   fm1=argv[0]->c.ary.entity->c.fvec.fv;
00754   fm2=argv[1]->c.ary.entity->c.fvec.fv;
00755   row1=rowsize(argv[0]);        row2=rowsize(argv[1]);
00756   column1=colsize(argv[0]);     column2=colsize(argv[1]);
00757   if (column1!=row2) error(E_VECINDEX);
00758   if (n==3) {
00759     rm=argv[2];
00760     if (!ismatrix(rm)) error(E_NOVECTOR);
00761     if (row1!=rowsize(rm) || column2!=colsize(rm)) error(E_VECINDEX);
00762     }
00763   else rm=makematrix(ctx,row1,column2);
00764   if (row1>256 || column2>256){
00765     fv = (eusfloat_t *)malloc(sizeof(eusfloat_t) * ((row1>column2)?row1:column2));
00766     //error(E_VECINDEX);
00767   }
00768   fm=rm->c.ary.entity->c.fvec.fv;
00769   if (fm2!=fm) 
00770     for (i=0; i<row1; i++) {
00771       ii=i*column1;
00772       for (j=0; j<column2; j++) {
00773         x=0.0; jj=j;
00774         for (k=0; k<column1; k++) {
00775           x += fm1[ii+k] * fm2[jj];
00776           jj+=column2;}
00777         fv[j]=x;}
00778       ii=i*column2;
00779       for (j=0; j<column2; j++) fm[ii+j]=fv[j];}
00780   else 
00781     for (i=0; i<column2; i++) {
00782       for (j=0; j<row1; j++) {
00783         x=0.0; jj=j*column1; ii=0;
00784         for (k=0; k<column1; k++) {
00785           x += fm1[jj+k] * fm2[i+ii];
00786           ii+=column2;}
00787         fv[j]=x;}
00788       jj=0;
00789       for (j=0; j<row1; j++, jj+=column2) fm[i+jj]=fv[j];}
00790   if (fv!=fvv) free(fv);
00791   return(rm);}
00792 
00793 pointer TRANSFORM(ctx,n,argv)
00794 register context *ctx;
00795 int n;
00796 register pointer argv[];
00797 { register pointer result;
00798   register eusfloat_t *m,*v,x;
00799   eusfloat_t *fv,fvv[256];
00800   register int i,j,ii,s,s2;
00801   fv = fvv;
00802 
00803   ckarg2(2,3);
00804   if (ismatrix(argv[0]) && isfltvector(argv[1])) {
00805     m=argv[0]->c.ary.entity->c.fvec.fv;
00806     v=argv[1]->c.fvec.fv;
00807     s=colsize(argv[0]); s2=rowsize(argv[0]);
00808     if (s!=vecsize(argv[1])) error(E_VECINDEX); }
00809   else if (isfltvector(argv[0]) && ismatrix(argv[1])) {
00810     v=argv[0]->c.fvec.fv;
00811     m=argv[1]->c.ary.entity->c.fvec.fv;
00812     s2=colsize(argv[1]); s=rowsize(argv[1]);
00813     if (s!=vecsize(argv[0])) error(E_VECINDEX); }
00814   else error(E_NOVECTOR);
00815 
00816   if (n==3) {
00817     result=argv[2];
00818     if (!isfltvector(result)) error(E_NOVECTOR);
00819     if (s2!=vecsize(result)) error(E_VECINDEX);}
00820   else result=makefvector(s2);
00821   if (s2>256){
00822     fv = (eusfloat_t *)malloc(sizeof(eusfloat_t) * s2);
00823     // error(E_VECINDEX);
00824   }
00825 
00826   if (isfltvector(argv[0])) {   /* vec*mat */
00827     for (i=0; i<s2; i++) {
00828       x=0.0; ii=i;
00829       for (j=0; j<s; j++) { x+= m[ii] * v[j]; ii+=s2;}
00830       fv[i]=x;}}
00831   else {        /* mat*vec */ 
00832     for (i=0; i<s2; i++) {
00833       x=0.0; ii=i*s;
00834       for (j=0; j<s; j++) x+=m[ii+j]*v[j];
00835       fv[i]=x;} }
00836   for (i=0; i<s2; i++) result->c.fvec.fv[i]=fv[i];
00837   if (s2>256) free(fv);
00838   return(result);
00839   }
00840 
00841 /****************************************************************/
00842 /*      rotate vector
00843 /*      (rotate-vector fltvec theta :axis [result])
00844 /*      theta : radian
00845 /*      axis : one of :x,:y or :z or 0,1,2
00846 /****************************************************************/
00847 
00848 pointer ROTVEC(ctx,n,argv)
00849 register context *ctx;
00850 int n;
00851 pointer argv[];
00852 { register pointer vec,result,a;
00853   double theta,c,s1,s2;
00854   eusfloat_t f1,f2;
00855   register int k1,k2,k3,size;
00856   numunion nu;
00857 
00858   ckarg2(3,4);
00859   vec=argv[0];
00860   if (!isfltvector(vec)) error(E_FLOATVECTOR);
00861   size=vecsize(vec);
00862   theta=ckfltval(argv[1]);
00863   c=cos(theta); s1=sin(theta);
00864   a=argv[2];
00865   if (a==K_X || a==makeint(0)) {  k1=1; k2=2; k3=0; s2=s1; s1= -s1;}
00866   else if (a==K_Y || a==makeint(1)) { k1=0; k2=2; k3=1; s2= -s1;}
00867   else if (a==K_Z || a==makeint(2)) { k1=0; k2=1; k3=2; s2=s1; s1= -s1;}
00868   else if (a==NIL) { k1=0; k2=1; s2=s1; s1= -s1;}
00869   else error(E_ROTAXIS);
00870   if (n==4) {
00871     result=argv[3];
00872     if (!isfltvector(result)) error(E_FLOATVECTOR); }
00873   else result=makefvector(size);
00874   f1=vec->c.fvec.fv[k1];
00875   f2=vec->c.fvec.fv[k2];
00876   result->c.fvec.fv[k1]=c*f1+s1*f2;
00877   result->c.fvec.fv[k2]=s2*f1+c*f2;
00878   if (a!=NIL) result->c.fvec.fv[k3]=vec->c.fvec.fv[k3];
00879   return(result);}
00880 
00881 /****************************************************************/
00882 /*      rotate matrix
00883 /*      (rotate-matrix mat theta :axis [worldp] [result])
00884 /*              theta : radian
00885 /*              axis : one of :x,:y or :z or 0,1,2
00886 /*              worldp: nil--local(mult rot mat from the right)
00887 /*                      t----world(mult rot mat from the left)
00888 /****************************************************************/
00889 static copymat(dest,src,size)
00890 pointer dest,src;
00891 register int size;
00892 { register int i;
00893   register eusfloat_t *rv=dest->c.ary.entity->c.fvec.fv;
00894   register eusfloat_t *mv=src->c.ary.entity->c.fvec.fv;
00895   size=size*size;
00896   for (i=0; i<size; i++) rv[i]=mv[i]; }
00897 
00898 pointer ROTMAT(ctx,n,argv)
00899 register context *ctx;
00900 int n;                  /* rot theata axis worldp result */
00901 pointer argv[];
00902 { register pointer mat,result,a;
00903   double theta,c,s1,s2;
00904   eusfloat_t *m1,*m2,f1,f2;
00905   register eusfloat_t *rm,*m;
00906   register int i,size,k1,k2,worldp=0,ss;
00907   numunion nu;
00908 
00909   ckarg2(3,5);
00910   mat=argv[0];
00911   if (!ismatrix(mat)) error(E_NOVECTOR);
00912   size=colsize(mat);
00913   theta=ckfltval(argv[1]);
00914   c=cos(theta); s1=sin(theta);
00915 
00916   a=argv[2];
00917   if (n>=4) worldp=(argv[3]!=NIL);      /*0:local 1:world*/
00918 
00919   if (n==5) {
00920     result=argv[4];
00921     if (!ismatrix(result)) error(E_NOVECTOR); }
00922   else result=makematrix(ctx,size,size);
00923   rm=result->c.ary.entity->c.fvec.fv;
00924   m=mat->c.ary.entity->c.fvec.fv;
00925 
00926   if (a==K_X || a==makeint(0)) {
00927     k1=1; k2=2;
00928     if (worldp) { s2=s1; s1= -s1;}
00929     else s2= -s1;}
00930   else if (a==MK_X) {
00931     k1=1; k2=2;
00932     if (worldp) s2= -s1;
00933     else { s2=s1; s1= -s1;}}
00934   else if (a==K_Y || a==makeint(1)) {
00935     k1=0; k2=2;
00936     if (worldp) s2= -s1;
00937     else { s2=s1; s1= -s1;}}
00938   else if (a==MK_Y) {
00939     k1=0; k2=2;
00940     if (worldp) { s2=s1; s1= -s1;}
00941     else s2= -s1;}
00942   else if (a==K_Z || a==makeint(2)) {
00943     k1=0; k2=1;
00944     if (worldp) { s2=s1; s1= -s1;}
00945     else s2= -s1;}
00946   else if (a==MK_Z) {
00947     k1=0; k2=1;
00948     if (worldp) s2= -s1;
00949     else { s2=s1; s1= -s1;}}
00950   else if (a==NIL) { /* 2dimensional m.i */
00951     eusfloat_t m0,m1,m2,m3;
00952     m0=c*m[0]-s1*m[2];  m1=c*m[1]-s1*m[3];
00953     m2=s1*m[0]+c*m[2];  m3=s1*m[1]+c*m[3];
00954     rm[0]=m0; rm[1]=m1; rm[2]=m2; rm[3]=m3;
00955     return(result);   }
00956   else error(E_ROTAXIS);
00957 
00958   ss=size*size;
00959   if (mat!=result) for (i=0; i<ss; i++) rm[i]=m[i];
00960   for (i=0; i<size; i++) {
00961     if (worldp) {
00962       m1= &rm[k1*size+i];
00963       m2= &rm[k2*size+i];}
00964     else {
00965       m1= &rm[i*size+k1];
00966       m2= &rm[i*size+k2];}
00967     f1= (*m1)*c + (*m2)*s1;
00968     f2= (*m1)*s2 + (*m2)*c;
00969     *m1=f1; *m2=f2;}
00970   return(result);}
00971 
00972 /*      (rotation theta floatvector [result])
00973 /*      make rotation matrix along arbitrary axis
00974 /*      Axis is given by floatvector, it need not to be normalized.
00975 */
00976 pointer ROTATION_MATRIX(ctx,n,argv)
00977 register context *ctx;
00978 int n;
00979 pointer *argv;
00980 { register pointer a,result;
00981   register eusfloat_t *rm;
00982   register int size;
00983   double x, y, z, s;
00984   double cs, sn, vers, xv, yv,zv, xyv, yzv, zxv, xs, ys, zs, norm;
00985   numunion nu;
00986 
00987   ckarg2(1,3);
00988   s=ckfltval(argv[0]);
00989   cs = cos(s); sn = sin(s); vers = 1.0 - cs;
00990   a=argv[1];
00991   if (n==1 || (n==2 && ismatrix(a))) {  /*2D rot. matrix*/
00992     if (n==2) result=a;
00993     else result=makematrix(ctx,2,2);
00994     rm=result->c.ary.entity->c.fvec.fv;
00995     rm[0]=rm[3]=cs; rm[1]=-sn; rm[2]=sn;
00996     return(result);}
00997   if (a==NIL) size=2; else size=3;
00998   if (n==3) { result=argv[2]; if (!ismatrix(result)) error(E_NOVECTOR);}
00999   else result=makematrix(ctx,size,size);
01000   rm=result->c.ary.entity->c.fvec.fv;
01001   if (isfltvector(a)) {
01002     if (size<=2) error(E_VECINDEX);
01003     x=a->c.fvec.fv[0]; y=a->c.fvec.fv[1]; z=a->c.fvec.fv[2];
01004     norm = sqrt(x*x + y*y + z*z);
01005     if (fabs(norm)<0.00001) return(NIL); /*error(E_USER,(pointer)"too small axis vector");*/
01006     x = x/norm; y = y/norm; z= z/norm;
01007     xv = x*x*vers; yv = y*y*vers; zv = z*z*vers;
01008     xyv = x*y*vers; yzv = y*z*vers; zxv = z*x*vers;
01009     xs = x*sn; ys = y*sn; zs = z*sn;
01010     rm[0] = xv + cs;
01011     rm[1] = xyv - zs;
01012     rm[2] = zxv + ys;
01013     rm[size] = xyv + zs;
01014     rm[size+1] = yv + cs;
01015     rm[size+2] = yzv - xs;
01016     rm[size+size] = zxv - ys;
01017     rm[size+size+1] = yzv + xs;
01018     rm[size+size+2] = zv + cs;
01019     return(result);}
01020   else {
01021     if (size==2) {
01022       rm[0]=rm[3]=cs; rm[1]=-sn; rm[2]=sn;
01023       return(result);}
01024     for (size=0; size<9; size++) rm[size]=0.0;
01025     if (a==makeint(0) || a==K_X) {
01026       rm[0]=1.0; rm[4]=cs; rm[5]= -sn; rm[7]=sn; rm[8]=cs;}
01027     else if (a==MK_X) {
01028       rm[0]=1.0; rm[4]=cs; rm[5]= sn; rm[7]=-sn; rm[8]=cs;}
01029     else if (a==makeint(1) || a==K_Y) {
01030       rm[0]=cs; rm[2]=sn; rm[4]=1.0; rm[6]= -sn; rm[8]=cs;}
01031     else if (a==MK_Y) {
01032       rm[0]=cs; rm[2]=-sn; rm[4]=1.0; rm[6]= sn; rm[8]=cs;}
01033     else if (a==makeint(2) || a==K_Z) {
01034       rm[0]=cs; rm[1]= -sn; rm[3]=sn; rm[4]=cs; rm[8]=1.0;}
01035     else if (a==MK_Z) {
01036       rm[0]=cs; rm[1]= sn; rm[3]=-sn; rm[4]=cs; rm[8]=1.0;}
01037     else error(E_NOVECTOR);
01038     return(result);} }
01039 
01040 pointer ROTANGLE(ctx,n,argv)    /*equivalent rotation axis and angle*/
01041 register context *ctx;
01042 int n;
01043 pointer argv[];
01044 { pointer r=argv[0];
01045   int size;
01046   eusfloat_t *m,*krv,kx,ky,kz;
01047   eusfloat_t th,t1,t2,t3;
01048   eusfloat_t cs,cs2,vers,sn2,norm;
01049   pointer kr;
01050   numunion nu;
01051 
01052   ckarg(1);
01053   if (!ismatrix(r)) error(E_NOVECTOR);
01054   size=colsize(r);
01055   m=r->c.ary.entity->c.fvec.fv;
01056 
01057   if (size==2) {        /*2D rotation*/
01058     th=atan2(m[2],m[0]);
01059     return(makeflt(th));}
01060 
01061   kr=makefvector(3);    /*axis vector*/
01062   krv=kr->c.fvec.fv;
01063   t1=m[size+size+1]-m[size+2];
01064   t2=m[2]          -m[size+size];
01065   t3=m[size]       -m[1];
01066   cs2=m[0]+m[size+1]+m[size+size+2]-1.0;
01067   cs=cs2/2;
01068   vers=1-cs;
01069   sn2=sqrt(t1*t1 + t2*t2 + t3*t3);
01070   th=atan2(sn2,cs2);
01071   if (th<1e-10||vers<1e-10) return(NIL);
01072   else if (th<2.6) {
01073     kx=(m[size+size+1]-m[size+2])/sn2;
01074     ky=(m[2]-m[size+size])/sn2;
01075     kz=(m[size]-m[1])/sn2;
01076     }
01077   else {
01078     kx=sqrt((m[0]-cs)/vers);
01079     if (m[size+size+1]-m[size+2]<0) kx= -kx;
01080     ky=sqrt((m[size+1]-cs)/vers);
01081     if (m[2]-m[size+size]<0) ky= -ky;
01082     kz=sqrt((m[size+size+2]-cs)/vers);
01083     if (m[size]-m[1]<0) kz= -kz;
01084     }
01085   
01086   if (debug) 
01087     printf("rotation-angle1: %f %f %f\n",kx,ky,kz);
01088   if ((fabs(kx) > fabs(ky)) && (fabs(kx) > fabs(kz))) {
01089     ky=(m[size]+m[1])/(2*kx*vers); kz=(m[2]+m[size+size])/(2*kx*vers);
01090     norm=sqrt((ky*ky+kz*kz)/(1.0-kx*kx)); ky/=norm; kz/=norm;}
01091   else if ((fabs(ky) > fabs(kx)) && (fabs(ky) > fabs(kz))) {
01092     kx=(m[size]+m[1])/(2*ky*vers); kz=(m[size+2]+m[size+size+1])/(2*ky*vers);
01093     norm=sqrt((kx*kx+kz*kz)/(1.0-ky*ky)); kx/=norm; kz/=norm;}
01094   else {
01095     kx=(m[2]+m[size+size])/(2*kz*vers);
01096     ky=(m[size+2]+m[size+size+1])/(2*kz*vers);
01097     norm=sqrt((kx*kx+ky*ky)/(1.0-kz*kz)); kx/=norm; ky/=norm;}
01098   
01099   norm=sqrt(kx*kx + ky*ky + kz*kz);
01100   if (debug) 
01101     printf("rotation-angle2: %f %f %f norm=%f\n",kx,ky,kz,norm);
01102   krv[0]=kx/norm; krv[1]=ky/norm; krv[2]=kz/norm;
01103   kx=kx/norm; ky=ky/norm; kz=kz/norm;
01104   norm=sqrt(kx*kx + ky*ky + kz*kz);
01105   return(cons(ctx,makeflt(th),cons(ctx,kr,NIL)));}
01106 
01107              
01108 /*      (transpose mat [result])
01109 */
01110 pointer TRANSPOSE(ctx,n,argv)
01111 register context *ctx;
01112 int n;
01113 pointer argv[];
01114 { register eusfloat_t *rm,*m,x;
01115   pointer mat,result;
01116   register int i,j,row,column;
01117 
01118   ckarg2(1,2);
01119   mat=argv[0];
01120   if (!ismatrix(mat)) error(E_NOVECTOR);
01121   column=colsize(mat); row=rowsize(mat);
01122   if (n==2) {
01123     result=argv[1];
01124     if (!ismatrix(result)) error(E_NOVECTOR);
01125     if (rowsize(result)!=column || colsize(result)!=row) error(E_VECINDEX);}
01126   else result=makematrix(ctx,column,row);
01127   rm=result->c.ary.entity->c.fvec.fv;
01128   m=mat->c.ary.entity->c.fvec.fv;
01129   if (result==mat) /*square matrix*/
01130     for (i=0; i<row; i++) 
01131       for (j=0; j<=i; j++) {
01132         x=m[j*column+i];
01133         rm[j*column+i]=m[column*i+j];
01134         rm[column*i+j]=x;}
01135   else 
01136     for (i=0; i<row; i++) 
01137       for (j=0; j<column; j++) rm[j*row+i]=m[i*column+j];
01138   return(result);}
01139 
01140 /* get roll, pitch yaw angles out of rotation matrix*/
01141 pointer INV_RPY(ctx,n,argv)
01142 register context *ctx;
01143 int n;
01144 pointer argv[];
01145 { pointer mat;
01146   eusfloat_t a, b, c, sa, ca;
01147   register eusfloat_t *mv;
01148   pointer result,result2;
01149   numunion nu;
01150 
01151   ckarg(1);
01152   mat=argv[0];
01153   if (!ismatrix(mat)) error(E_NOVECTOR);
01154   if (colsize(mat)!=3) error(E_VECINDEX);
01155   mv=mat->c.ary.entity->c.fvec.fv;
01156   a = atan2(mv[3],mv[0]);
01157   sa = sin(a); ca = cos(a);
01158   b = atan2(-mv[6], ca*mv[0] + sa*mv[3]);
01159   c = atan2(sa*mv[2] - ca*mv[5], -sa*mv[1] + ca*mv[4]);
01160   result = cons(ctx,makeflt(c), NIL);
01161   result = cons(ctx,makeflt(b), result);
01162   result = cons(ctx,makeflt(a), result);
01163   a=a + M_PI;
01164   sa = sin(a); ca = cos(a);
01165   b = atan2(-mv[6], ca*mv[0] + sa*mv[3]);
01166   c = atan2(sa*mv[2] - ca*mv[5], -sa*mv[1] + ca*mv[4]);
01167   result2 = cons(ctx,makeflt(c), NIL);
01168   result2 = cons(ctx,makeflt(b), result2);
01169   result2 = cons(ctx,makeflt(a), result2);
01170   result2 = cons(ctx,result2,NIL);
01171   return(cons(ctx,result,result2));}
01172 
01173 pointer INV_EULER(ctx,n,argv)
01174 register context *ctx;
01175 int n;
01176 pointer argv[];
01177 { pointer mat;
01178   eusfloat_t a, b, c, sa, ca;
01179   register eusfloat_t *mv;
01180   pointer result,result2;
01181   numunion nu;
01182 
01183   ckarg(1);
01184   mat=argv[0];
01185   if (!ismatrix(mat)) error(E_NOVECTOR);
01186   if (colsize(mat)!=3) error(E_VECINDEX);
01187   mv=mat->c.ary.entity->c.fvec.fv;
01188   if (-0.00001<mv[5] && mv[5]<0.00001 && -0.00001<mv[2] && mv[2]<0.00001) a=0.0;
01189   else a = atan2(mv[5],mv[2]);
01190   sa = sin(a); ca = cos(a);
01191   b = atan2(ca*mv[2]+sa*mv[5],mv[8]);
01192   c = atan2(-sa*mv[0]+ca*mv[3], -sa*mv[1] + ca*mv[4]);
01193   result = cons(ctx,makeflt(c), NIL);
01194   result = cons(ctx,makeflt(b), result);
01195   result = cons(ctx,makeflt(a), result);
01196   a=a + M_PI;
01197   sa = sin(a); ca = cos(a);
01198   b = atan2(ca*mv[2]+sa*mv[5],mv[8]);
01199   c = atan2(-sa*mv[0]+ca*mv[3], -sa*mv[1] + ca*mv[4]);
01200   result2 = cons(ctx,makeflt(c), NIL);
01201   result2 = cons(ctx,makeflt(b), result2);
01202   result2 = cons(ctx,makeflt(a), result2);
01203   result2 = cons(ctx,result2,NIL);
01204   return(cons(ctx,result,result2));}
01205 
01206 /****************************************************************/
01207 /*
01208 /* simultaneous equation solver by LU decomposition method
01209 /* 
01210 /*      1987-July-1
01211 /*      Copyright Toshihiro MATSUI
01212 /*
01213 /****************************************************************/
01214 
01215 #define elm(a,i,j) (a->c.vec.v[j]->c.fvec.fv[i])
01216 #define EPS (1.0E-10)
01217 
01218 static int decompose(a,s,p)
01219 register eusfloat_t *a;
01220 register int s;
01221 eusinteger_t p[];
01222 {
01223   register int i,j,k,l;
01224   eusfloat_t al,bl;
01225 
01226   for (i=0; i<s; i++) p[i]=i;
01227   for (k=0; k<s; k++) {
01228     /*select pivot*/
01229     l=k;
01230     al=fabs(a[p[l]*s+k]);
01231     for (i=k+1; i<s; i++) 
01232       if ((bl=fabs(a[p[i]*s+k])) > al) { l=i; al=bl;}
01233     if (l!=k) {
01234       /*change rows*/
01235       j=p[k]; p[k]=p[l]; p[l]=j; }
01236     if (al<EPS) return(-1);     /* singular*/
01237     /* gauss elimination */
01238     a[p[k]*s+k]= 1.0/a[p[k]*s+k];
01239     for (i=k+1; i<s; i++) {
01240       al=a[p[i]*s+k]=a[p[i]*s+k]*a[p[k]*s+k];
01241       for (j=k+1; j<s; j++) a[p[i]*s+j] -= al*a[p[k]*s+j];
01242       }
01243     }
01244   return(0);
01245   }
01246 
01247 pointer LU_DECOMPOSE(ctx,n,argv)
01248 register context *ctx;
01249 int n;
01250 pointer argv[];
01251 /* (LU-DECOMPOSE mat [result]) */
01252 { pointer a,result,pv;
01253   int s,stat;
01254 
01255   ckarg2(1,2);
01256   a=argv[0];
01257   if (!ismatrix(a)) error(E_NOVECTOR);
01258   s=colsize(a);
01259   if (s!=rowsize(a)) error(E_VECSIZE);
01260   if (n==1) result=a;
01261   else {
01262     result=argv[1];
01263     if (!ismatrix(result)) error(E_NOVECTOR);
01264     if (s!=colsize(result)) error(E_VECSIZE);
01265     copymat(result,a,s); }
01266   pv=makevector(C_VECTOR,s);
01267   stat=decompose(result->c.ary.entity->c.fvec.fv,s,(eusinteger_t *)pv->c.vec.v);
01268   while (--s>=0) pv->c.vec.v[s]=makeint((eusinteger_t)pv->c.vec.v[s]);
01269   if (stat<0) return(NIL);
01270   else return(pv);}
01271 
01272 static solve(a,pv,s,b,x)
01273 register eusfloat_t *a;
01274 pointer b,x,pv;
01275 int s;
01276 { register int i,j;
01277   eusfloat_t t;
01278   register pointer *p;
01279   p=pv->c.vec.v;
01280 
01281   /*forward substitution*/
01282   for (i=0; i<s; i++) {
01283     t=b->c.fvec.fv[intval(p[i])];
01284     for (j=0; j<i; j++) t-= a[intval(p[i])*s+j]*x->c.fvec.fv[j];
01285     x->c.fvec.fv[i]=t;}
01286   /*backward substitution*/
01287   for (i=s-1; i>=0; i--) {
01288     t=x->c.fvec.fv[i];
01289     for (j=i+1; j<s; j++) t-= a[intval(p[i])*s+j]*x->c.fvec.fv[j];
01290     x->c.fvec.fv[i]=t*a[intval(p[i])*s+i];}
01291 }
01292 
01293 pointer LU_SOLVE(ctx,n,argv)
01294 register context *ctx;
01295 int n;
01296 pointer argv[];
01297 { pointer a,p,b,x;
01298   int s;
01299 
01300   ckarg2(3,4);
01301   a=argv[0];  p=argv[1]; b=argv[2];
01302   if (!ismatrix(a)) error(E_NOVECTOR);
01303   s=colsize(a);
01304   if (!isvector(p) || !isfltvector(b)) error(E_NOVECTOR);
01305   if (s!=vecsize(p) || s!=vecsize(b)) error(E_VECSIZE);
01306   if (n==4) {
01307     x=argv[3];
01308     if (!isvector(x)) error(E_NOVECTOR);
01309     if (s!=vecsize(x)) error(E_VECSIZE); }
01310   else  x=(pointer)makefvector(s);
01311   solve(a->c.ary.entity->c.fvec.fv,p,s,b,x);
01312   return(x);}
01313 
01314 pointer LU_DETERMINANT(ctx,n,argv)
01315 register context *ctx;
01316 int n;
01317 pointer argv[];
01318 { register pointer a,p;
01319   register eusfloat_t *av;
01320   register int i,s;
01321   eusfloat_t val=1.0;
01322   numunion nu;
01323   ckarg(2);
01324   a=argv[0]; p=argv[1];
01325   if (!isvector(p) || !ismatrix(a)) error(E_NOVECTOR);
01326   s=vecsize(p);
01327   if (colsize(a)!=s || rowsize(a)!=s) error(E_VECSIZE);
01328   av=a->c.ary.entity->c.fvec.fv;
01329   for (i=0; i<s; i++) val*=av[intval(p->c.vec.v[i])*s+i];
01330   return(makeflt(1.0/val));}
01331 
01332 void matrix(ctx,mod)
01333 register context *ctx;
01334 register pointer mod;
01335 {
01336   K_X=defkeyword(ctx,"X");
01337   K_Y=defkeyword(ctx,"Y");
01338   K_Z=defkeyword(ctx,"Z");
01339   MK_X=defkeyword(ctx,"-X");
01340   MK_Y=defkeyword(ctx,"-Y");
01341   MK_Z=defkeyword(ctx,"-Z");
01342 
01343   defun(ctx,"V+",mod,VPLUS);
01344   defun(ctx,"V++",mod,VPLUSPLUS);
01345   defun(ctx,"V-",mod,VMINUS);
01346   defun(ctx,"V-ABS",mod,VMINUS_ABS);
01347   defun(ctx,"V.",mod,VINNERPRODUCT);
01348   defun(ctx,"V*",mod,VCROSSPRODUCT);
01349   defun(ctx,"V.*",mod,SCA3PROD);
01350   defun(ctx,"V<",mod,VLESSP);
01351   defun(ctx,"V>",mod,VGREATERP);
01352   defun(ctx,"VMIN",mod,VMIN);
01353   defun(ctx,"VMAX",mod,VMAX);
01354   defun(ctx,"MINIMAL-BOX",mod,MINIMALBOX);
01355   defun(ctx,"SCALE",mod,SCALEVEC);
01356   defun(ctx,"NORM",mod,VNORM); 
01357   defun(ctx,"NORM2",mod,VNORM2);
01358   defun(ctx,"NORMALIZE-VECTOR",mod,VNORMALIZE);
01359   defun(ctx,"DISTANCE",mod,VDISTANCE);
01360   defun(ctx,"DISTANCE2",mod,VDISTANCE2);
01361   defun(ctx,"DIRECTION",mod,VDIRECTION);
01362   defun(ctx,"MIDPOINT",mod,MIDPOINT);
01363 /*  defun(ctx,"TRIANGLE",mod,TRIANGLE); */
01364   defun(ctx,"FLOATVECTOR",mod,MKFLTVEC);
01365   defun(ctx,"FLOAT-VECTOR",mod,MKFLTVEC);
01366   defun(ctx,"TRANSFORM",mod,TRANSFORM);
01367   defun(ctx,"M*",mod,MATTIMES);
01368   defun(ctx,"ROTATE-VECTOR",mod,ROTVEC);
01369   defun(ctx,"ROTATE-MATRIX",mod,ROTMAT);
01370   defun(ctx,"ROTATION-MATRIX",mod,ROTATION_MATRIX);
01371   defun(ctx,"ROTATION-ANGLE",mod,ROTANGLE);
01372   defun(ctx,"TRANSPOSE",mod,TRANSPOSE);
01373   defun(ctx,"RPY-ANGLE",mod,INV_RPY);
01374   defun(ctx,"EULER-ANGLE",mod,INV_EULER);
01375   defun(ctx,"LU-DECOMPOSE",mod,LU_DECOMPOSE);
01376   defun(ctx,"LU-SOLVE",mod,LU_SOLVE);
01377   defun(ctx,"LU-DETERMINANT",mod,LU_DETERMINANT);}


euslisp
Author(s): Toshihiro Matsui
autogenerated on Thu Sep 3 2015 10:36:20