roseus_c_util.c
Go to the documentation of this file.
1 #if Solaris2 && GCC
2 #pragma init (register_roseus_c_util)
3 #endif
4 #include <stdio.h>
5 #include <stdlib.h>
6 #include <unistd.h>
7 #include <string.h>
8 #include <math.h>
9 #include <sys/time.h>
10 #include "eus.h"
11 
12 extern pointer ___roseus_c_util();
13 //static void register_roseus_c_util()
14 // { add_module_initializer("___roseus_c_util", ___roseus_c_util);}
15 
16 #define colsize(p) (intval(p->c.ary.dim[1]))
17 #define rowsize(p) (intval(p->c.ary.dim[0]))
18 #define ismatrix(p) ((isarray(p) && \
19  p->c.ary.rank==makeint(2) && \
20  elmtypeof(p->c.ary.entity)==ELM_FLOAT))
21 #define ckvsize(a,b) ((a->c.vec.size==b->c.vec.size)?vecsize(a):(int)error(E_VECINDEX))
22 
24  register context *ctx;
25  int n;
26  register pointer argv[];
27 {
28  int i, nanflag;
29  int step, size;
30  int pos_x, pos_y, pos_z;
31  int pos_nx, pos_ny, pos_nz;
32  int pos_rgb;
33  eusfloat_t *mat, *nmat, *cmat;
34  byte *data;
35 
36  /* data step size mat x y z nmat nx ny nz cmat rgb nan-remove-flag*/
37  /* 0 1 2 3 4 5 6 7 8 9 10 11 12 13 */
38  ckarg(14);
39 
40  if ((!ismatrix(argv[3])) && NIL != argv[3]) {
42  }
43  if ((!ismatrix(argv[7])) && NIL != argv[7]) {
45  }
46  if ((!ismatrix(argv[11])) && NIL != argv[11]) {
48  }
49 
50  if (!isstring(argv[0])) error(E_TYPEMISMATCH);
51 
52  /*
53  if ((colsize(argv[0]) != 3) ||
54  (colsize(argv[1]) != 3) ||
55  (vecsize(argv[2]) != 3) ||
56  (colsize(argv[3]) != 3) ||
57  (rowsize(argv[3]) != 3))
58  error(E_VECINDEX);
59  */
60 
61  step = ckintval(argv[1]);
62  size = ckintval(argv[2]);
63 
64  nanflag = ckintval(argv[13]);
65 
66  data = argv[0]->c.str.chars;
67  if ( NIL != argv[3] ) {
68  mat = argv[3]->c.ary.entity->c.fvec.fv;
69  pos_x = ckintval(argv[4]);
70  pos_y = ckintval(argv[5]);
71  pos_z = ckintval(argv[6]);
72  } else {
73  mat = NULL;
74  pos_x = pos_y = pos_z = 0;
75  }
76  if ( NIL != argv[7] ) {
77  nmat = argv[7]->c.ary.entity->c.fvec.fv;
78  pos_nx = ckintval(argv[8]);
79  pos_ny = ckintval(argv[9]);
80  pos_nz = ckintval(argv[10]);
81  } else {
82  nmat = NULL;
83  pos_nx = pos_ny = pos_nz = 0;
84  }
85  if ( NIL != argv[11] ) {
86  cmat = argv[11]->c.ary.entity->c.fvec.fv;
87  pos_rgb = ckintval(argv[12]);
88  } else {
89  cmat = NULL;
90  pos_rgb = 0;
91  }
92 
93  for( i = 0; i < size; i++, data += step ) {
94  if (mat != NULL) {
95  *mat++ = ( *((float *)(data + pos_x)) ) * 1000.0;
96  *mat++ = ( *((float *)(data + pos_y)) ) * 1000.0;
97  *mat++ = ( *((float *)(data + pos_z)) ) * 1000.0;
98  }
99  if (nmat != NULL) {
100  *nmat++ = *((float *)(data + pos_nx));
101  *nmat++ = *((float *)(data + pos_ny));
102  *nmat++ = *((float *)(data + pos_nz));
103  }
104  if (cmat != NULL) {
105  int rgb = *((int *) (data + pos_rgb));
106  *cmat++ = ( ((rgb >> 16) & 0x000000FF) / 255.0 );
107  *cmat++ = ( ((rgb >> 8) & 0x000000FF) / 255.0 );
108  *cmat++ = ( (rgb & 0x000000FF) / 255.0 );
109  }
110  }
111 
112  if (nanflag > 0) {
113  // replace nan
114  if ( mat != NULL ) { mat--; }
115  if ( cmat != NULL ) { cmat--; }
116  for( i = 0; i < size; i++ ) {
117  if ( mat != NULL ) {
118  if ( isnan(*mat) ) {
119  *mat-- = 0.0;
120  *mat-- = 0.0;
121  *mat-- = 0.0;
122  } else {
123  mat -= 3;
124  }
125  }
126  if ( cmat != NULL ) {
127  if ( isnan(*cmat) ) {
128  *cmat-- = 0.0;
129  *cmat-- = 0.0;
130  *cmat-- = 0.0;
131  } else {
132  cmat -= 3;
133  }
134  }
135  }
136  }
137 
138  return NIL;
139 }
140 
142  register context *ctx;
143  int n;
144  register pointer argv[];
145 {
146  int i, step, size;
147  eusfloat_t *mat, *nmat, *cmat;
148  byte *data;
149  /* raw_data size psize parray carray narray */
150  /* 0 1 2 3 4 5 */
151  ckarg(6);
152 
153  if (!isstring(argv[0])) error(E_TYPEMISMATCH);
154 
155  if ((!ismatrix(argv[3])) && NIL != argv[3]) {
157  }
158  if ((!ismatrix(argv[4])) && NIL != argv[4]) {
160  }
161  if ((!ismatrix(argv[5])) && NIL != argv[5]) {
163  }
164 
165  size = ckintval(argv[1]);
166  step = ckintval(argv[2]);
167 
168  data = argv[0]->c.str.chars;
169 
170  if ( NIL != argv[3] ) {
171  mat = argv[3]->c.ary.entity->c.fvec.fv;
172  } else {
173  mat = NULL;
174  }
175  if ( NIL != argv[4] ) {
176  cmat = argv[4]->c.ary.entity->c.fvec.fv;
177  } else {
178  cmat = NULL;
179  }
180  if ( NIL != argv[5] ) {
181  nmat = argv[5]->c.ary.entity->c.fvec.fv;
182  } else {
183  nmat = NULL;
184  }
185 
186  for( i = 0; i < size; i++, data += step ) {
187  float *tmp = (float *)data;
188  if (mat != NULL) {
189  *tmp++ = (*mat++) * 0.001;
190  *tmp++ = (*mat++) * 0.001;
191  *tmp++ = (*mat++) * 0.001;
192  }
193  if (cmat != NULL) {
194  int r = (floor ((*cmat++) * 255));
195  int g = (floor ((*cmat++) * 255));
196  int b = (floor ((*cmat++) * 255));
197 
198  *((int *)tmp) = ((r & 0x000000FF) << 16) | ((g & 0x000000FF) << 8) | (b & 0x000000FF);
199  tmp++;
200  }
201  if (nmat != NULL) {
202  *tmp++ = *nmat++;
203  *tmp++ = *nmat++;
204  *tmp++ = *nmat++;
205  *tmp++ = 0.0; // curvature
206  }
207  }
208 
209  return NIL;
210 }
211 
212 #include "defun.h"
214 register context *ctx;int n;pointer *argv;pointer env;
215 {
216  defun(ctx,"CONVERT-MSG2-POINTCLOUD", argv[0], CONV_MSG2_PC,NULL);
217  defun(ctx,"CONVERT-POINTCLOUD-MSG2", argv[0], CONV_PC_MSG2,NULL);
218  return NULL;
219 }
220 
ROS_INFO ROS_ERROR int n
Definition: roseus.cpp:818
ckarg(2)
#define ismatrix(p)
Definition: roseus_c_util.c:18
ROS_INFO ROS_ERROR int pointer * argv
Definition: roseus.cpp:819
pointer CONV_MSG2_PC(context *ctx, int n, argv)
Definition: roseus_c_util.c:23
pointer error(enum errorcode ec,...) pointer error(va_alist) va_dcl
pointer ___roseus_c_util()
pointer defun(context *, char *, pointer, pointer(*)(), char *)
unsigned int step
#define NULL
E_TYPEMISMATCH
unsigned char byte
double eusfloat_t
pointer NIL
pointer CONV_PC_MSG2(context *ctx, int n, argv)


roseus
Author(s): Kei Okada
autogenerated on Thu Apr 4 2019 02:55:35