roseus_c_util.c
Go to the documentation of this file.
00001 #if Solaris2 && GCC
00002 #pragma init (register_roseus_c_util)
00003 #endif
00004 #include <stdio.h>
00005 #include <stdlib.h>
00006 #include <unistd.h>
00007 #include <string.h>
00008 #include <math.h>
00009 #include <sys/time.h>
00010 #include "eus.h"
00011 
00012 extern pointer ___roseus_c_util();
00013 //static void register_roseus_c_util()
00014 //  { add_module_initializer("___roseus_c_util", ___roseus_c_util);}
00015 
00016 #define colsize(p) (intval(p->c.ary.dim[1]))
00017 #define rowsize(p) (intval(p->c.ary.dim[0]))
00018 #define ismatrix(p) ((isarray(p) && \
00019                       p->c.ary.rank==makeint(2) && \
00020                       elmtypeof(p->c.ary.entity)==ELM_FLOAT))
00021 #define ckvsize(a,b) ((a->c.vec.size==b->c.vec.size)?vecsize(a):(int)error(E_VECINDEX))
00022 
00023 pointer CONV_MSG2_PC (ctx,n,argv)
00024      register context *ctx;
00025      int n;
00026      register pointer argv[];
00027 {
00028   int i, nanflag;
00029   int step, size;
00030   int pos_x, pos_y, pos_z;
00031   int pos_nx, pos_ny, pos_nz;
00032   int pos_rgb;
00033   eusfloat_t *mat, *nmat, *cmat;
00034   byte *data;
00035 
00036   /* data step size mat x y z nmat nx ny nz cmat rgb nan-remove-flag*/
00037   /*    0    1    2   3 4 5 6    7  8  9 10   11  12 13 */
00038   ckarg(14);
00039 
00040   if ((!ismatrix(argv[3])) && NIL != argv[3]) {
00041     error(E_TYPEMISMATCH);
00042   }
00043   if ((!ismatrix(argv[7])) && NIL != argv[7]) {
00044     error(E_TYPEMISMATCH);
00045   }
00046   if ((!ismatrix(argv[11])) && NIL != argv[11]) {
00047     error(E_TYPEMISMATCH);
00048   }
00049 
00050   if (!isstring(argv[0])) error(E_TYPEMISMATCH);
00051 
00052   /*
00053   if ((colsize(argv[0]) != 3) ||
00054       (colsize(argv[1]) != 3) ||
00055       (vecsize(argv[2]) != 3) ||
00056       (colsize(argv[3]) != 3) ||
00057       (rowsize(argv[3]) != 3))
00058     error(E_VECINDEX);
00059   */
00060 
00061   step = ckintval(argv[1]);
00062   size = ckintval(argv[2]);
00063 
00064   nanflag = ckintval(argv[13]);
00065 
00066   data = argv[0]->c.str.chars;
00067   if ( NIL != argv[3] ) {
00068     mat = argv[3]->c.ary.entity->c.fvec.fv;
00069     pos_x = ckintval(argv[4]);
00070     pos_y = ckintval(argv[5]);
00071     pos_z = ckintval(argv[6]);
00072   } else {
00073     mat = NULL;
00074     pos_x = pos_y = pos_z = 0;
00075   }
00076   if ( NIL != argv[7] ) {
00077     nmat = argv[7]->c.ary.entity->c.fvec.fv;
00078     pos_nx = ckintval(argv[8]);
00079     pos_ny = ckintval(argv[9]);
00080     pos_nz = ckintval(argv[10]);
00081   } else {
00082     nmat = NULL;
00083     pos_nx = pos_ny = pos_nz = 0;
00084   }
00085   if ( NIL != argv[11] ) {
00086     cmat = argv[11]->c.ary.entity->c.fvec.fv;
00087     pos_rgb = ckintval(argv[12]);
00088   } else {
00089     cmat = NULL;
00090     pos_rgb = 0;
00091   }
00092 
00093   for( i = 0; i < size; i++, data += step ) {
00094     if (mat != NULL) {
00095       *mat++ = ( *((float *)(data + pos_x)) ) * 1000.0;
00096       *mat++ = ( *((float *)(data + pos_y)) ) * 1000.0;
00097       *mat++ = ( *((float *)(data + pos_z)) ) * 1000.0;
00098     }
00099     if (nmat != NULL) {
00100       *nmat++ = *((float *)(data + pos_nx));
00101       *nmat++ = *((float *)(data + pos_ny));
00102       *nmat++ = *((float *)(data + pos_nz));
00103     }
00104     if (cmat != NULL) {
00105       int rgb = *((int *) (data + pos_rgb));
00106       *cmat++ = ( ((rgb >> 16) & 0x000000FF) / 255.0 );
00107       *cmat++ = ( ((rgb >> 8) & 0x000000FF) / 255.0 );
00108       *cmat++ = ( (rgb & 0x000000FF) / 255.0 );
00109     }
00110   }
00111 
00112   if (nanflag > 0) {
00113     // replace nan
00114     if ( mat != NULL ) { mat--; }
00115     if ( cmat != NULL ) { cmat--; }
00116     for( i = 0; i < size; i++ ) {
00117       if ( mat != NULL ) {
00118         if ( isnan(*mat) ) {
00119           *mat-- = 0.0;
00120           *mat-- = 0.0;
00121           *mat-- = 0.0;
00122         } else {
00123           mat -= 3;
00124         }
00125       }
00126       if ( cmat != NULL ) {
00127         if ( isnan(*cmat) ) {
00128           *cmat-- = 0.0;
00129           *cmat-- = 0.0;
00130           *cmat-- = 0.0;
00131         } else {
00132           cmat -= 3;
00133         }
00134       }
00135     }
00136   }
00137 
00138   return NIL;
00139 }
00140 
00141 pointer CONV_PC_MSG2 (ctx,n,argv)
00142      register context *ctx;
00143      int n;
00144      register pointer argv[];
00145 {
00146   int i, step, size;
00147   eusfloat_t *mat, *nmat, *cmat;
00148   byte *data;
00149   /* raw_data  size psize parray carray narray */
00150   /*        0     1     2      3      4      5 */
00151   ckarg(6);
00152 
00153   if (!isstring(argv[0])) error(E_TYPEMISMATCH);
00154 
00155   if ((!ismatrix(argv[3])) && NIL != argv[3]) {
00156     error(E_TYPEMISMATCH);
00157   }
00158   if ((!ismatrix(argv[4])) && NIL != argv[4]) {
00159     error(E_TYPEMISMATCH);
00160   }
00161   if ((!ismatrix(argv[5])) && NIL != argv[5]) {
00162     error(E_TYPEMISMATCH);
00163   }
00164 
00165   size = ckintval(argv[1]);
00166   step = ckintval(argv[2]);
00167 
00168   data = argv[0]->c.str.chars;
00169 
00170   if ( NIL != argv[3] ) {
00171     mat = argv[3]->c.ary.entity->c.fvec.fv;
00172   } else {
00173     mat = NULL;
00174   }
00175   if ( NIL != argv[4] ) {
00176     cmat = argv[4]->c.ary.entity->c.fvec.fv;
00177   } else {
00178     cmat = NULL;
00179   }
00180   if ( NIL != argv[5] ) {
00181     nmat = argv[5]->c.ary.entity->c.fvec.fv;
00182   } else {
00183     nmat = NULL;
00184   }
00185 
00186   for( i = 0; i < size; i++, data += step ) {
00187     float *tmp = (float *)data;
00188     if (mat != NULL) {
00189       *tmp++ = (*mat++) * 0.001;
00190       *tmp++ = (*mat++) * 0.001;
00191       *tmp++ = (*mat++) * 0.001;
00192     }
00193     if (cmat != NULL) {
00194       int r = (floor ((*cmat++) * 255));
00195       int g = (floor ((*cmat++) * 255));
00196       int b = (floor ((*cmat++) * 255));
00197 
00198       *((int *)tmp) = ((r & 0x000000FF) << 16) | ((g & 0x000000FF) << 8) | (b & 0x000000FF);
00199       tmp++;
00200     }
00201     if (nmat != NULL) {
00202       *tmp++ = *nmat++;
00203       *tmp++ = *nmat++;
00204       *tmp++ = *nmat++;
00205       *tmp++ = 0.0; // curvature
00206     }
00207   }
00208 
00209   return NIL;
00210 }
00211 
00212 #include "defun.h"
00213 pointer ___roseus_c_util(ctx,n, argv, env)
00214 register context *ctx;int n;pointer *argv;pointer env;
00215 {
00216   defun(ctx,"CONVERT-MSG2-POINTCLOUD", argv[0], CONV_MSG2_PC,NULL);
00217   defun(ctx,"CONVERT-POINTCLOUD-MSG2", argv[0], CONV_PC_MSG2,NULL);
00218   return NULL;
00219 }
00220 


roseus
Author(s): Kei Okada
autogenerated on Wed Apr 3 2019 02:46:38