1 // This is brl/bseg/boxm2/cpp/pro/processes/boxm2_cpp_get_index_from_3d_point_process.cxx
2 #include <fstream>
3 #include <bprb/bprb_func_process.h>
4 //:
5 // \file
6 // \brief A process for converting a 3d point into an octree blockid/index
7 //
8 // \author Andy Neff
9 // \date Oct 15 2015
10
11 #include <boxm2/io/boxm2_cache.h>
12 #include <boxm2/io/boxm2_stream_cache.h>
13 #include <boxm2/boxm2_scene.h>
14 #include <boxm2/boxm2_block.h>
15 #include <boxm2/boxm2_data_base.h>
16 #include <boxm2/boxm2_util.h>
17 //brdb stuff
18 #include <boct/boct_bit_tree.h>
19
20
21 namespace boxm2_cpp_get_index_from_3d_point_process_globals
22 {
23 constexpr unsigned n_inputs_ = 5;
24 constexpr unsigned n_outputs_ = 4;
25 }
26
27 namespace boxm2_cpp_get_3d_point_from_index_process_globals
28 {
29 constexpr unsigned n_inputs_ = 6;
30 constexpr unsigned n_outputs_ = 7;
31 }
32
boxm2_cpp_get_index_from_3d_point_process_cons(bprb_func_process & pro)33 bool boxm2_cpp_get_index_from_3d_point_process_cons(bprb_func_process& pro)
34 {
35 using namespace boxm2_cpp_get_index_from_3d_point_process_globals;
36
37 bool ok=false;
38
39 //process takes 7 inputs
40 std::vector<std::string> input_types_(n_inputs_);
41 input_types_[0] = "boxm2_scene_sptr";
42 input_types_[1] = "boxm2_cache_sptr";
43 input_types_[2] = "float"; //x
44 input_types_[3] = "float"; //y
45 input_types_[4] = "float"; //z
46 ok = pro.set_input_types(input_types_);
47 if (!ok) return ok;
48
49 std::vector<std::string> output_types_(n_outputs_);
50 output_types_[0]="int";
51 output_types_[1]="int";
52 output_types_[2]="int";
53 output_types_[3]="int";
54 ok = pro.set_output_types(output_types_);
55 if (!ok) return ok;
56
57 return true;
58 }
59
boxm2_cpp_get_3d_point_from_index_process_cons(bprb_func_process & pro)60 bool boxm2_cpp_get_3d_point_from_index_process_cons(bprb_func_process& pro)
61 {
62 using namespace boxm2_cpp_get_3d_point_from_index_process_globals;
63
64 bool ok=false;
65
66 //process takes 7 inputs
67 std::vector<std::string> input_types_(n_inputs_);
68 input_types_[0] = "boxm2_scene_sptr";
69 input_types_[1] = "boxm2_cache_sptr";
70 input_types_[2] = "int"; //i
71 input_types_[3] = "int"; //j
72 input_types_[4] = "int"; //k
73 input_types_[5] = "int"; //index
74 ok = pro.set_input_types(input_types_);
75 if (!ok) return ok;
76
77 std::vector<std::string> output_types_(n_outputs_);
78 output_types_[0]="float";
79 output_types_[1]="float";
80 output_types_[2]="float";
81 output_types_[3]="float";
82 output_types_[4]="float";
83 output_types_[5]="float";
84 output_types_[6]="int";
85 ok = pro.set_output_types(output_types_);
86 if (!ok) return ok;
87
88 return true;
89 }
90
boxm2_cpp_get_index_from_3d_point_process(bprb_func_process & pro)91 bool boxm2_cpp_get_index_from_3d_point_process(bprb_func_process& pro)
92 {
93 using namespace boxm2_cpp_get_index_from_3d_point_process_globals;
94
95 if ( pro.n_inputs() < n_inputs_ ) {
96 std::cout << pro.name() << ": The input number should be " << n_inputs_<< std::endl;
97 return false;
98 }
99 //get the inputs
100 unsigned i = 0;
101 boxm2_scene_sptr scene =pro.get_input<boxm2_scene_sptr>(i++);
102 boxm2_cache_sptr cache= pro.get_input<boxm2_cache_sptr>(i++);
103 auto x =pro.get_input<float>(i++);
104 auto y =pro.get_input<float>(i++);
105 auto z =pro.get_input<float>(i++);
106
107 vgl_point_3d<double> local;
108 boxm2_block_id id;
109 if (!scene->contains(vgl_point_3d<double>(x, y, z), id, local))
110 return false;
111
112 int index_x=(int)std::floor(local.x());
113 int index_y=(int)std::floor(local.y());
114 int index_z=(int)std::floor(local.z());
115 boxm2_block * blk=cache->get_block(scene, id);
116 boxm2_block_metadata mdata = scene->get_block_metadata_const(id);
117 vnl_vector_fixed<unsigned char,16> treebits=blk->trees()(index_x,index_y,index_z);
118 boct_bit_tree tree(treebits.data_block(),mdata.max_level_);
119 int bit_index=tree.traverse(local);
120 int index=tree.get_data_index(bit_index,false);
121
122 pro.set_output_val<int>(0, id.i());
123 pro.set_output_val<int>(1, id.j());
124 pro.set_output_val<int>(2, id.k());
125 pro.set_output_val<int>(3, index);
126
127 return true;
128 }
129
boxm2_cpp_get_3d_point_from_index_process(bprb_func_process & pro)130 bool boxm2_cpp_get_3d_point_from_index_process(bprb_func_process& pro)
131 {
132 using namespace boxm2_cpp_get_3d_point_from_index_process_globals;
133
134 if ( pro.n_inputs() < n_inputs_ ) {
135 std::cout << pro.name() << ": The input number should be " << n_inputs_<< std::endl;
136 return false;
137 }
138 //get the inputs
139 unsigned i = 0;
140 boxm2_scene_sptr scene =pro.get_input<boxm2_scene_sptr>(i++);
141 boxm2_cache_sptr cache= pro.get_input<boxm2_cache_sptr>(i++);
142 int bi =pro.get_input<int>(i++);
143 int bj =pro.get_input<int>(i++);
144 int bk =pro.get_input<int>(i++);
145 int index =pro.get_input<int>(i++);
146
147 boxm2_block_id id(bi, bj, bk);
148 boxm2_block * blk = cache->get_block(scene,id);
149 boxm2_block_metadata mdata = scene->get_block_metadata_const(id);
150 vgl_box_3d<double> bbox = mdata.bbox();
151 vgl_vector_3d<unsigned int> num_subblocks = mdata.sub_block_num_;
152 vgl_point_3d<double> block_origin = mdata.local_origin_;
153
154 // int ix_min = std::max(0,int(std::floor((bbox.min_x() - block_origin.x())/mdata.sub_block_dim_.x())));
155 // int iy_min = std::max(0,int(std::floor((bbox.min_y() - block_origin.y())/mdata.sub_block_dim_.y())));
156 // int iz_min = std::max(0,int(std::floor((bbox.min_z() - block_origin.z())/mdata.sub_block_dim_.z())));
157 // int ix_max = std::min(int(num_subblocks.x()-1),int(std::floor((bbox.max_x() - block_origin.x())/mdata.sub_block_dim_.x())));
158 // int iy_max = std::min(int(num_subblocks.y()-1),int(std::floor((bbox.max_y() - block_origin.y())/mdata.sub_block_dim_.y())));
159 // int iz_max = std::min(int(num_subblocks.z()-1),int(std::floor((bbox.max_z() - block_origin.z())/mdata.sub_block_dim_.z())));
160
161 const boxm2_array_3d<vnl_vector_fixed<unsigned char, 16> > &trees = blk->trees();
162
163 vgl_point_3d<double> subblock_origin;
164
165 for (size_t ix=0; ix<mdata.sub_block_num_.x(); ++ix)
166 {
167 for (size_t iy=0; iy<mdata.sub_block_num_.y(); ++iy)
168 {
169 for (size_t iz=0; iz<mdata.sub_block_num_.z(); ++iz)
170 {
171 vnl_vector_fixed<unsigned char, 16> tree = trees(ix, iy, iz);
172
173 boct_bit_tree bit_tree((unsigned char*) tree.data_block(), mdata.max_level_);
174
175 //if (bit_tree.num_cells()>1)
176 // std::cout << bit_tree.num_cells() << " cells" << std::endl;
177
178 if (bit_tree.num_cells() <= index)
179 {
180 index -= bit_tree.num_cells();
181 //std::cout << bit_tree.num_cells() << " cells" << std::endl;
182 }
183 else
184 {
185 //std::cout << "subblock index: " << index << std::endl;
186
187 std::vector<int> cell_indexes = bit_tree.get_cell_bits();
188
189 int cell_index = cell_indexes[index];
190
191 //std::cout << "bit index: " << cell_index << std::endl;
192
193 //iterate through leaves of the tree
194 //std::vector<int> leafBits = bit_tree.get_leaf_bits();
195 subblock_origin = block_origin + vgl_vector_3d<double>(mdata.sub_block_dim_.x()*ix,
196 mdata.sub_block_dim_.y()*iy,
197 mdata.sub_block_dim_.z()*iz);
198
199 vgl_point_3d<double> cell_center = bit_tree.cell_center(cell_index);
200 cell_center.x() = cell_center.x() * mdata.sub_block_dim_.x() + subblock_origin.x();
201 cell_center.y() = cell_center.y() * mdata.sub_block_dim_.y() + subblock_origin.y();
202 cell_center.z() = cell_center.z() * mdata.sub_block_dim_.z() + subblock_origin.z();
203
204 pro.set_output_val<float>(0, cell_center.x());
205 pro.set_output_val<float>(1, cell_center.y());
206 pro.set_output_val<float>(2, cell_center.z());
207 pro.set_output_val<float>(3, bit_tree.cell_len(cell_index) * mdata.sub_block_dim_.x());
208 pro.set_output_val<float>(4, bit_tree.cell_len(cell_index) * mdata.sub_block_dim_.y());
209 pro.set_output_val<float>(5, bit_tree.cell_len(cell_index) * mdata.sub_block_dim_.z());
210 pro.set_output_val<int>(6, bit_tree.is_leaf(cell_index));
211
212 return true;
213 }
214 }
215 }
216 }
217
218 return false;
219 }
220