1
2#ifdef SEGLEN
3//Update step cell functor::seg_len
4void step_cell_seglen(AuxArgs aux_args, int data_ptr, uchar llid, float d)
5{
6#ifdef ATOMIC_OPT
7    // --------- faster and less accurate method... --------------------------
8    //keep track of cells being hit
9    aux_args.cell_ptrs[llid] = data_ptr;
10    aux_args.cached_aux[llid] = (float4) 0.0f;  //leaders retain the mean obs and the cell length
11    barrier(CLK_LOCAL_MEM_FENCE);
12
13    //segment workgroup
14    load_data_mutable_opt(aux_args.ray_bundle_array,aux_args.cell_ptrs);
15
16    //back to normal mean of mean obs...
17    seg_len_obs_functor(d, aux_args.obs, aux_args.ray_bundle_array, aux_args.cached_aux);
18    barrier(CLK_LOCAL_MEM_FENCE);
19
20    //set aux data here (for each leader.. )
21    if (aux_args.ray_bundle_array[llid].y==1)
22    {
23        //scale!
24        int seg_int = convert_int_rte(aux_args.cached_aux[llid].x * SEGLEN_FACTOR);
25        int cum_obs = convert_int_rte(aux_args.cached_aux[llid].y * SEGLEN_FACTOR);
26
27        //atomically update the cells
28        atom_add(&aux_args.seg_len[data_ptr], seg_int);
29        atom_add(&aux_args.mean_obs[data_ptr], cum_obs);
30    }
31    //reset cell_ptrs to negative one every time (prevents invisible layer bug)
32    aux_args.cell_ptrs[llid] = -1;
33    //------------------------------------------------------------------------
34#elif ATOMIC_FLOAT
35    //SLOW and accurate method
36    AtomicAdd((__global float*) (&aux_args.seg_len[data_ptr]), d);
37    AtomicAdd((__global float*) (&aux_args.mean_obs[data_ptr]), d * aux_args.obs );
38#else
39    //SLOW and accurate method
40    int seg_int = convert_int_rte(d * SEGLEN_FACTOR);
41    atom_add(&aux_args.seg_len[data_ptr], seg_int);
42    int cum_obs = convert_int_rte(d * aux_args.obs * SEGLEN_FACTOR);
43    atom_add(&aux_args.mean_obs[data_ptr], cum_obs);
44
45#ifdef DEBUG
46    (*aux_args.ray_len) += d;
47#endif
48
49#endif
50}
51#endif // SEGLEN
52
53#ifdef PREINF
54//preinf step cell functor
55//
56// pre(x)(I) = pre(x-1)(I) + vis(x)*p(I|x in S)*P(x in S)
57//
58void step_cell_preinf(AuxArgs aux_args, int data_ptr, uchar llid, float d)
59{
60    //keep track of cells being hit
61    ////cell data, i.e., alpha and app model is needed for some passes
62    float  alpha    = aux_args.alpha[data_ptr];
63    CONVERT_FUNC_FLOAT8(mixture,aux_args.mog[data_ptr])/NORM;
64    float  weight3  = (1.0f-mixture.s2-mixture.s5);
65
66#ifdef ATOMIC_FLOAT
67    float cum_len = as_float(aux_args.seg_len[data_ptr]);
68    float mean_obs= as_float(aux_args.mean_obs[data_ptr]) / cum_len;
69#else
70    int cum_int = aux_args.seg_len[data_ptr];
71    int mean_int = aux_args.mean_obs[data_ptr];
72    float mean_obs = convert_float(mean_int) / convert_float(cum_int);
73    float cum_len = convert_float(cum_int) / SEGLEN_FACTOR;
74#endif
75
76    //calculate pre_infinity term
77    // pre_infinity_opt is in ray_bundle_library_opt.cl
78    pre_infinity_opt( d*aux_args.linfo->block_len,
79                      cum_len*aux_args.linfo->block_len,
80                      mean_obs,
81                      aux_args.vis_inf,
82                      aux_args.pre_inf,
83                      alpha,
84                      mixture,
85                      weight3);
86}
87#endif // PREINF
88
89#ifdef BAYES
90//bayes step cell functor
91void step_cell_bayes(AuxArgs aux_args, int data_ptr, uchar llid, float d)
92{
93#ifdef ATOMIC_OPT
94    //keep track of cells being hit
95    aux_args.cell_ptrs[llid] = data_ptr;
96    barrier(CLK_LOCAL_MEM_FENCE);
97    load_data_mutable_opt(aux_args.ray_bundle_array, aux_args.cell_ptrs);
98
99    //if this current thread is a segment leader...
100    //cell data, i.e., alpha and app model is needed for some passes
101    float  alpha    = aux_args.alpha[data_ptr];
102    //float8 mixture  = convert_float8(aux_args.mog[data_ptr])/(float)NORM;
103    CONVERT_FUNC_FLOAT8(mixture,aux_args.mog[data_ptr])/NORM;
104    float weight3   = (1.0f-mixture.s2-mixture.s5);
105
106    //load aux data
107    float cum_len  = convert_float(aux_args.seg_len[data_ptr]);//SEGLEN_FACTOR;
108    float mean_obs = convert_float(aux_args.mean_obs[data_ptr]);//SEGLEN_FACTOR;
109    mean_obs = mean_obs/cum_len;
110    //aux_args.linfo->block_len;
111    float cell_beta = 0.0f;
112    float cell_vis  = 0.0f;
113    barrier(CLK_LOCAL_MEM_FENCE);
114
115    //calculate bayes ratio
116    bayes_ratio_functor(d,aux_args.linfo->block_len,
117                        mean_obs,
118                        aux_args.ray_pre,
119                        aux_args.ray_vis,
120                        aux_args.norm,
121                        &cell_beta,
122                        &cell_vis,
123                        aux_args.ray_bundle_array,
124                        aux_args.cell_ptrs,
125                        aux_args.cached_vis,
126                        alpha,
127                        mixture,
128                        weight3);
129
130    //set aux data here (for each leader.. )
131    if (aux_args.ray_bundle_array[llid].y==1)
132    {
133        int beta_int = convert_int_rte(cell_beta * SEGLEN_FACTOR);
134        atom_add(&aux_args.beta_array[data_ptr], beta_int);
135        int vis_int  = convert_int_rte(cell_vis * SEGLEN_FACTOR);
136        atom_add(&aux_args.vis_array[data_ptr], vis_int);
137    }
138#else
139    //slow beta calculation ----------------------------------------------------
140    float  alpha    = aux_args.alpha[data_ptr];
141    CONVERT_FUNC_FLOAT8(mixture,aux_args.mog[data_ptr])/NORM;
142    float weight3   = (1.0f-mixture.s2-mixture.s5);
143
144#ifdef ATOMIC_FLOAT
145    float cum_len = as_float(aux_args.seg_len[data_ptr]);
146    float mean_obs= as_float(aux_args.mean_obs[data_ptr]) / cum_len;
147#else
148    int cum_int = aux_args.seg_len[data_ptr];
149    int mean_int = aux_args.mean_obs[data_ptr];
150    float mean_obs = convert_float(mean_int) / convert_float(cum_int);
151    float cum_len = convert_float(cum_int) / SEGLEN_FACTOR;
152#endif
153
154    float ray_beta, vis_cont;
155    bayes_ratio_ind( d,
156                     alpha,
157                     mixture,
158                     weight3,
159                     aux_args.linfo->block_len,
160                     mean_obs,
161                     aux_args.norm,
162                     aux_args.ray_pre,
163                     aux_args.ray_vis,
164                     &ray_beta,
165                     &vis_cont);
166
167#ifdef ATOMIC_FLOAT
168    AtomicAdd((__global float*) (&aux_args.beta_array[data_ptr]), ray_beta);
169    AtomicAdd((__global float*) (&aux_args.vis_array[data_ptr]),  (vis_cont/aux_args.linfo->block_len) );
170#else
171    //discretize and store beta and vis contribution
172    int beta_int = convert_int_rte(ray_beta * SEGLEN_FACTOR);
173    atom_add(&aux_args.beta_array[data_ptr], beta_int);
174    int vis_int  = convert_int_rte((vis_cont) * SEGLEN_FACTOR);
175    atom_add(&aux_args.vis_array[data_ptr], vis_int);
176#endif
177
178
179    //-------------------------------------------------------------------------- */
180#endif
181
182    //reset cell_ptrs to -1 every time
183    aux_args.cell_ptrs[llid] = -1;
184}
185#endif // BAYES
186
187#ifdef UPDATE_HIST
188//bayes step cell functor
189void step_cell_update_hist(AuxArgs aux_args, int data_ptr, uchar llid, float d)
190{
191    //slow beta calculation ----------------------------------------------------
192    float  alpha    = aux_args.alpha[data_ptr];
193
194    //load aux data
195    float cum_len  = convert_float(aux_args.seg_len[data_ptr])/SEGLEN_FACTOR;
196    float obs = aux_args.obs;
197
198    float vis=*(aux_args.vis);
199    float upcount=d/cum_len* vis;
200
201    int index1=(int)floor(obs*4);
202    index1=min(index1,3);
203
204    //discretize and store beta and vis contribution
205    int upcount_int = convert_int_rte(upcount * SEGLEN_FACTOR);
206    atom_add(&aux_args.hist[8*data_ptr+index1], upcount_int);
207
208    int index2=(int)floor((obs-0.125)*3);
209    vis=vis*exp(-alpha*d*aux_args.linfo->block_len);
210    *(aux_args.vis)=vis;
211
212    if (index2>=0 && index2<=2)
213        atom_add(&aux_args.hist[8*data_ptr+4+index2], upcount_int);
214      //atom_add(&aux_args.hist[8*data_ptr+7],        upcount_int);
215}
216#endif // UPDATE_HIST
217
218#ifdef CUMLEN
219void step_cell_cumlen(AuxArgs aux_args, int data_ptr, uchar llid, float d)
220{
221    //SLOW and accurate method
222    int seg_int = convert_int_rte(d * SEGLEN_FACTOR);
223    atom_add(&aux_args.seg_len[data_ptr], seg_int);
224}
225#endif // CUMLEN
226
227
228#ifdef INGEST_HEIGHT_MAP
229void step_cell_ingest_height_map(AuxArgs aux_args, int data_ptr, float d)
230{
231    float alpha = - (log(1-0.999))/d;
232    aux_args.alpha[data_ptr] = alpha;
233}
234void step_cell_ingest_height_space_map(AuxArgs aux_args, int data_ptr, float d)
235{
236    float alpha = - (log(1-0.001))/d;
237    aux_args.alpha[data_ptr] = alpha;
238}
239#endif // INGEST_HEIGHT_MAP
240
241
242#ifdef INGEST_BUCKEYE_DEM
243#define Z_SIGMA 0.2
244#define NEAR_ZERO 1e-5
245
246void step_cell_ingest_buckeye_dem(AuxArgs aux_args, int data_ptr, float d0, float d1)
247{
248
249    float b = aux_args.belief[data_ptr];
250    float u = aux_args.uncertainty[data_ptr];
251
252    // probability first return lies within cell
253    const float norm_const = 1.0/(sqrt(2.0)*Z_SIGMA);
254    //const float norm_const2 = 1.0/(sqrt(2.0)*2*Z_SIGMA);
255    // null case (first_depth > last_depth)
256    //const float null_case_prob = 1.0 - 0.5 * (1.0 + erf((aux_args.last_depth - aux_args.first_depth)*norm_const2));
257    // P(d0 < first_depth < d1)
258    const float P1 = 0.5 * (erf((d1 - aux_args.first_depth)*norm_const) - erf((d0 - aux_args.first_depth)*norm_const));
259    // P(d0 < last_depth < d1)
260    const float P2 = 0.5 * (erf((d1 - aux_args.last_depth)*norm_const) - erf((d0 - aux_args.last_depth)*norm_const));
261
262
263    const float b_obs =  P1 + P2 - P1*P2;
264    // P(d1 < first_depth)
265    const float d_obs1 = 1.0 - 0.5 * (1.0 + erf((d1 - aux_args.first_depth)*norm_const));
266    const float d_obs2 = 1.0 - 0.5 * (1.0 + erf((d1 - aux_args.last_depth)*norm_const));
267    const float d_obs =  d_obs1*d_obs2;
268    const float u_obs = 1.0 - b_obs - d_obs;
269
270    const float denom = u + u_obs - u*u_obs;
271
272    // Perform Cumalitive Fusion on two subjective logic opinions
273    if ((u_obs > NEAR_ZERO) || (u > NEAR_ZERO)) {
274      b = (b*u_obs + b_obs*u)/denom;
275      u = u*u_obs/denom;
276    }
277    else {
278      b = 0.5*(b_obs + b);
279      u = 0.0;
280    }
281    aux_args.belief[data_ptr] = b;
282    aux_args.uncertainty[data_ptr] = u;
283
284}
285#endif // INGEST_BUCKEYE_DEM
286