irtgeoc.c
Go to the documentation of this file.
00001 #pragma init (register_irtgeoc)
00002 #include "eus.h"
00003 #include <math.h>
00004 
00005 extern pointer ___irtgeoc();
00006 static register_irtgeoc()
00007 { add_module_initializer("___irtgeoc", ___irtgeoc);}
00008 
00009 //
00010 //
00011 #define colsize(p) (intval(p->c.ary.dim[1]))
00012 #define rowsize(p) (intval(p->c.ary.dim[0]))
00013 #define ismatrix(p) ((isarray(p) && \
00014                       p->c.ary.rank==makeint(2) && \
00015                       elmtypeof(p->c.ary.entity)==ELM_FLOAT))
00016 
00017 pointer C_COORDS_TRNSFORM_VECTOR(ctx,n,argv)
00018      register context *ctx;
00019      int n;
00020      register pointer argv[];
00021 {
00022   numunion nu;
00023   register pointer result;
00024   eusfloat_t *pos, *rot, *mat, *ret;
00025   int inversep = 0, fill = 0;
00026   int srcsize, dstsize;
00027   int i,j;
00028 
00029   //   0,   1,      2,        3,         4
00030   // pos, rot, matrix, (matrix), (inverse)
00031 
00032   ckarg2(3,5);
00033   if ( (!isfltvector(argv[0])) || (!ismatrix(argv[1])) || (!ismatrix(argv[2]))) error(E_TYPEMISMATCH);
00034   pos = argv[0]->c.fvec.fv;
00035   rot = argv[1]->c.ary.entity->c.fvec.fv;
00036   mat = argv[2]->c.ary.entity->c.fvec.fv;
00037   if (n==5) {
00038     if(!ismatrix(argv[3])) error(E_TYPEMISMATCH);
00039     result = argv[3];
00040     inversep = 1;
00041   } else if (n==4) {
00042     if(ismatrix(argv[3])) {
00043       result = argv[3];
00044     } else {
00045       result = makematrix(ctx,rowsize(argv[2]), colsize(argv[2]));
00046       inversep = 1;
00047       fill = 1;
00048     }
00049   } else { // n == 3
00050     result = makematrix(ctx,rowsize(argv[2]), colsize(argv[2]));
00051     fill = 1;
00052   }
00053   ret = result->c.ary.entity->c.fvec.fv;
00054 
00055   srcsize = colsize(argv[2]);
00056   dstsize = colsize(result);
00057   if ((srcsize < 3) && (dstsize < 3)) error(E_TYPEMISMATCH);
00058   if (inversep) {
00059     for(i=0;i<rowsize(result);i++){
00060       eusfloat_t x = mat[i*srcsize+0] - pos[0],
00061         y = mat[i*srcsize+1] - pos[1],
00062         z = mat[i*srcsize+2] - pos[2];
00063       ret[i*dstsize+0] = rot[0]*x+rot[3]*y+rot[6]*z;
00064       ret[i*dstsize+1] = rot[1]*x+rot[4]*y+rot[7]*z;
00065       ret[i*dstsize+2] = rot[2]*x+rot[5]*y+rot[8]*z;
00066       if(fill) {
00067         for(j=3;j<dstsize;j++) {
00068           ret[i*dstsize + j] = mat[i*srcsize + j];
00069         }
00070       }
00071     }
00072   } else {
00073     for(i=0;i<rowsize(result);i++){
00074       eusfloat_t x = mat[i*srcsize+0], y = mat[i*srcsize+1], z = mat[i*srcsize+2];
00075       ret[i*dstsize+0] = rot[0]*x+rot[1]*y+rot[2]*z+pos[0];
00076       ret[i*dstsize+1] = rot[3]*x+rot[4]*y+rot[5]*z+pos[1];
00077       ret[i*dstsize+2] = rot[6]*x+rot[7]*y+rot[8]*z+pos[2];
00078       if(fill) {
00079         for(j=3;j<dstsize;j++) {
00080           ret[i*dstsize + j] = mat[i*srcsize + j];
00081         }
00082       }
00083     }
00084   }
00085 
00086   return(result);
00087 }
00088 
00089 pointer C_MATRIX_ROW(ctx,n,argv)
00090      register context *ctx;
00091      int n;
00092      register pointer argv[];
00093 {
00094   numunion nu;
00095   register pointer result;
00096   register eusfloat_t *mat, *ret;
00097   register eusinteger_t pos,cols,i;
00098   int setp = 0;
00099 
00100   //      0,       1,        2,      3
00101   // matrix, row_num, (vector), (set?)
00102   // return float-vector
00103   // if vector is set, elements in vector is over written by row-vector
00104   // if vector and set? are set, value of vector is copied to matrix-row
00105 
00106   ckarg2(2,4);
00107   //if ( (!ismatrix(argv[0])) || (!isint(argv[1])) ) error(E_TYPEMISMATCH); //no check
00108   mat = argv[0]->c.ary.entity->c.fvec.fv;
00109   cols = colsize(argv[0]);
00110   pos = cols*intval(argv[1]);
00111 
00112   if (n==4) {
00113     //if (!(isfltvector(argv[2]))) error(E_TYPEMISMATCH); //no check
00114     result = argv[2];
00115     setp=1;
00116   } else if (n==3) {
00117     //if (!(isfltvector(argv[2]))) error(E_TYPEMISMATCH); //no check
00118     result = argv[2];
00119   } else { // n == 2
00120     result = makefvector(cols);
00121   }
00122   ret = result->c.fvec.fv;
00123 
00124   if(setp) {
00125     mat += pos;
00126     for(i=0;i<cols;i++) {
00127       *mat++ = *ret++;
00128     }
00129   } else {
00130     mat += pos;
00131     for(i=0;i<cols;i++) {
00132       *ret++ = *mat++;
00133     }
00134   }
00135 
00136   return(result);
00137 }
00138 
00139 // utility for using matrix as vector-array
00140 static pointer VECTOR_ARRAY_MEAN(ctx,n,argv)
00141      register context *ctx;
00142      int n;
00143      register pointer *argv;
00144 {
00145   int i,j,size,dim,pc=0;
00146   eusfloat_t *m, *fv;
00147   pointer mat;
00148 
00149   //            0,         1
00150   // matrix-array, (average)
00151 
00152   ckarg2(1,2);
00153   if(! ismatrix(argv[0])) {
00154     error(E_NOVECTOR);
00155   }
00156   m = argv[0]->c.ary.entity->c.fvec.fv;
00157   size = rowsize(argv[0]);
00158   dim = colsize(argv[0]);
00159 
00160   if(n>1 && isfltvector(argv[1])) {
00161     mat = argv[1];
00162   } else {
00163     mat = makevector(C_FLTVECTOR, dim); vpush(mat); pc++;
00164   }
00165   fv = mat->c.fvec.fv;
00166 
00167   for(i=0;i<size;i++) {
00168     for(j=0;j<dim;j++) {
00169       fv[j] += *m++;
00170     }
00171   }
00172 
00173   for(j=0;j<dim;j++) {
00174     fv[j] /= size;
00175   }
00176 
00177   while(pc-->0) vpop();
00178   return mat;
00179 }
00180 
00181 static pointer VECTOR_ARRAY_VARIANCE(ctx,n,argv)
00182      register context *ctx;
00183      int n;
00184      register pointer *argv;
00185 {
00186   int i,j,size,dim,pc=0, free_ave=1;
00187   eusfloat_t *m, *fv, *ave;
00188   pointer mat, amat;
00189 
00190   //            0,          1,         2
00191   // matrix-array, (variance), (average)
00192 
00193   ckarg2(1,3);
00194   if(! ismatrix(argv[0])) {
00195     error(E_NOVECTOR);
00196   }
00197 
00198   size = rowsize(argv[0]);
00199   dim = colsize(argv[0]);
00200 
00201   if(n>1 && isfltvector(argv[1])) {
00202     mat = argv[1];
00203   } else {
00204     mat = makevector(C_FLTVECTOR, dim); vpush(mat); pc++;
00205   }
00206   fv = mat->c.fvec.fv;
00207 
00208   if(n>2 && isfltvector(argv[2])) {
00209     amat = argv[2];
00210     ave =  amat->c.fvec.fv;
00211     free_ave=0;
00212   } else {
00213     ave = (eusfloat_t *) malloc(sizeof(eusfloat_t)*dim);
00214   }
00215 
00216   for(i=0;i<dim;i++) {
00217     fv[i] = 0.0;
00218     ave[i] = 0.0;
00219   }
00220 
00221   m = argv[0]->c.ary.entity->c.fvec.fv;
00222   for(i=0;i<size;i++) {
00223     for(j=0;j<dim;j++) {
00224       ave[j] += *m++;
00225     }
00226   }
00227   for(j=0;j<dim;j++) {
00228     ave[j] /= size;
00229   }
00230 
00231   m = argv[0]->c.ary.entity->c.fvec.fv;
00232   for(i=0;i<size;i++) {
00233     for(j=0;j<dim;j++) {
00234       fv[j] += pow((*m++ - ave[j]), 2);
00235     }
00236   }
00237   for(j=0;j<dim;j++) {
00238     fv[j] /= size;
00239   }
00240 
00241   if(free_ave) free(ave);
00242 
00243   while(pc-->0) vpop();
00244   return mat;
00245 }
00246 static pointer VECTOR_ARRAY_MAX_MIN(ctx,n,argv)
00247      register context *ctx;
00248      int n;
00249      register pointer *argv;
00250 {
00251   int i,j,size,dim,pc=0;
00252   eusfloat_t *m, *fvmin, *fvmax;
00253   pointer fmax, fmin, ret;
00254 
00255   //            0,            1,            2
00256   // matrix-array, (max-vector), (min-vector)
00257 
00258   ckarg2(1,3);
00259   if(! ismatrix(argv[0])) {
00260     error(E_NOVECTOR);
00261   }
00262   m = argv[0]->c.ary.entity->c.fvec.fv;
00263   size = rowsize(argv[0]);
00264   dim = colsize(argv[0]);
00265 
00266   if(n == 1 && isfltvector(argv[1])) {
00267     fmax = argv[1];
00268     fmin = makevector(C_FLTVECTOR, dim); vpush(fmin); pc++;
00269   } else if(n > 2 && isfltvector(argv[1]) && isfltvector(argv[2])) {
00270     fmax = argv[1];
00271     fmin = argv[2];
00272   } else {
00273     fmax = makevector(C_FLTVECTOR, dim); vpush(fmax); pc++;
00274     fmin = makevector(C_FLTVECTOR, dim); vpush(fmin); pc++;
00275   }
00276   fvmax = fmax->c.fvec.fv;
00277   fvmin = fmin->c.fvec.fv;
00278 
00279   // set initial value
00280   for(j=0;j<dim;j++) {
00281     eusfloat_t val = *m++;
00282     fvmax[j] = val;
00283     fvmin[j] = val;
00284   }
00285   for(i=1;i<size;i++) {
00286     for(j=0;j<dim;j++) {
00287       eusfloat_t val = *m++;
00288       if (val > fvmax[j])
00289         fvmax[j] = val;
00290       if (val < fvmin[j])
00291         fvmin[j] = val;
00292     }
00293   }
00294 
00295   ret=cons(ctx, fmin, NIL);
00296   vpush(ret);
00297   ret=cons(ctx, fmax, ret);
00298   vpop();
00299   while(pc-->0) vpop();
00300   return ret;
00301 }
00302 
00303 static pointer FVECTOR_REPLACE(ctx,n,argv)
00304      register context *ctx;
00305      int n;
00306      register pointer *argv;
00307 {
00308   register int i,count;
00309   register eusfloat_t *src, *dest;
00310   eusinteger_t ss,ds,se,de;
00311   numunion nu;
00312 
00313   //       0,       1,           2,         3,           4,         5
00314   // dst_vec, src_vec, (start_src), (end_src), (start_dst), (end_dst)
00315 
00316   ckarg2(2,6);
00317   if (!isfltvector(argv[0])) error(E_NOVECTOR);
00318   if (!isfltvector(argv[1])) error(E_NOVECTOR);
00319 
00320   dest = argv[0]->c.fvec.fv;
00321   src  = argv[1]->c.fvec.fv;
00322 
00323   ds = (n==2) ? 0 : ckintval(argv[2]);
00324   de = (n<=3) ? vecsize(argv[0]) : ckintval(argv[3]);
00325   ss = (n<=4) ? 0 : ckintval(argv[4]);
00326   se = (n<=5) ? vecsize(argv[1]) : ckintval(argv[5]);
00327 
00328   count = min(de-ds, se-ss);
00329   dest += ds;
00330   src += ss;
00331 
00332   for(i = 0; i < count; i++) {
00333     *dest++ = *src++;
00334   }
00335 
00336   return argv[0];
00337 }
00338 //
00339 //
00340 pointer C_ISNAN (ctx,n,argv)
00341      register context *ctx;
00342      int n;
00343      register pointer argv[];
00344 {
00345   ckarg(1);
00346 
00347   if ( isflt(argv[0]) ) {
00348     numunion nu;
00349     eusfloat_t f = fltval(argv[0]);
00350     if(isnan(f)) return T;
00351     return NIL;
00352   } else {
00353     return NIL;
00354   }
00355 }
00356 
00357 pointer ___irtgeoc(ctx,n, argv, env)
00358      register context *ctx;int n;pointer *argv;pointer env;
00359 {
00360   defun(ctx,"C-COORDS-TRANSFORM-VECTOR",argv[0],C_COORDS_TRNSFORM_VECTOR);
00361   defun(ctx,"C-MATRIX-ROW",argv[0],C_MATRIX_ROW);
00362   defun(ctx,"VECTOR-ARRAY-MEAN",argv[0],VECTOR_ARRAY_MEAN);
00363   defun(ctx,"VECTOR-ARRAY-VARIANCE",argv[0],VECTOR_ARRAY_VARIANCE);
00364   defun(ctx,"VECTOR-ARRAY-MAX-MIN",argv[0],VECTOR_ARRAY_MAX_MIN);
00365   defun(ctx,"FVECTOR-REPLACE", argv[0], FVECTOR_REPLACE);
00366 
00367   defun(ctx,"C-ISNAN", argv[0], C_ISNAN);
00368 }


jskeus
Author(s): JSK Alumnis
autogenerated on Tue Mar 7 2017 04:04:34