00001 #include "../include/object_loader.h"
00002 #include "../include/debug.h"
00003 #include <math.h>
00004 #include <assert.h>
00005
00006 namespace ICR
00007 {
00008 std::vector<double*> points_buffer;
00009 std::vector<double*> normals_buffer;
00010 TargetObject* callback_obj_ptr=NULL;
00011 bool face_callback_run=false;
00012 Eigen::VectorXi map_normal2vertex;
00013
00014
00015 ObjectLoader::ObjectLoader() : object_(new TargetObject()),object_loaded_(false){}
00016
00017 ObjectLoader::ObjectLoader(ObjectLoader const& src) : object_(src.object_), object_loaded_(src.object_loaded_) {}
00018
00019 ObjectLoader& ObjectLoader::operator=(ObjectLoader const& src)
00020 {
00021 if (this !=&src)
00022 {
00023 object_=src.object_;
00024 object_loaded_=src.object_loaded_;
00025 }
00026 return *this;
00027 }
00028
00029 std::ostream& operator<<(std::ostream& stream, ObjectLoader const& obj_loader)
00030 {
00031 stream <<'\n'<<"OBJECT LOADER: "<<'\n'
00032 <<"Is object loaded: "<<std::boolalpha<<obj_loader.object_loaded_<<'\n'
00033 <<"Loaded object name: "<<obj_loader.object_->getName()<<'\n'<<'\n';
00034
00035 return stream;
00036 }
00037
00038 ObjectLoader::~ObjectLoader(){}
00039
00040 const TargetObjectPtr ObjectLoader::getObject()const{return object_;}
00041
00042 TargetObjectPtr ObjectLoader::getObject(){return object_;}
00043
00044 bool ObjectLoader::objectLoaded()const{return object_loaded_;}
00045
00046 void ObjectLoader::loadObject(std::string const& file,std::string const& name)
00047 {
00048 if(object_loaded_)
00049 object_.reset(new TargetObject());
00050
00051 assert(points_buffer.size()==0);
00052 assert(normals_buffer.size()==0);
00053 assert(!face_callback_run);
00054 assert(callback_obj_ptr==NULL);
00055
00056 callback_obj_ptr=object_.get();
00057 obj::obj_parser obj_parser;
00058 obj_parser.geometric_vertex_callback(geometric_vertex_callback);
00059 obj_parser.vertex_normal_callback(vertex_normal_callback);
00060 obj_parser.face_callbacks(triangular_face_geometric_vertices_callback,
00061 triangular_face_geometric_vertices_texture_vertices_callback,
00062 triangular_face_geometric_vertices_vertex_normals_callback,
00063 triangular_face_geometric_vertices_texture_vertices_vertex_normals_callback,
00064 quadrilateral_face_geometric_vertices_callback,
00065 quadrilateral_face_geometric_vertices_texture_vertices_callback,
00066 quadrilateral_face_geometric_vertices_vertex_normals_callback,
00067 quadrilateral_face_geometric_vertices_texture_vertices_vertex_normals_callback,
00068 polygonal_face_geometric_vertices_begin_callback,
00069 polygonal_face_geometric_vertices_vertex_callback,
00070 polygonal_face_geometric_vertices_end_callback,
00071 polygonal_face_geometric_vertices_texture_vertices_begin_callback,
00072 polygonal_face_geometric_vertices_texture_vertices_vertex_callback,
00073 polygonal_face_geometric_vertices_texture_vertices_end_callback,
00074 polygonal_face_geometric_vertices_vertex_normals_begin_callback,
00075 polygonal_face_geometric_vertices_vertex_normals_vertex_callback,
00076 polygonal_face_geometric_vertices_vertex_normals_end_callback,
00077 polygonal_face_geometric_vertices_texture_vertices_vertex_normals_begin_callback,
00078 polygonal_face_geometric_vertices_texture_vertices_vertex_normals_vertex_callback,
00079 polygonal_face_geometric_vertices_texture_vertices_vertex_normals_end_callback);
00080 obj_parser.parse(file);
00081
00082 if(points_buffer.size() < normals_buffer.size())
00083 {
00084 std::cout<<"Error in ObjectLoader - Provide an .obj file with non-smoothed vertex normals. The mapping from vertices to vertex normals has to be unique. Exiting..." <<std::endl;
00085 exit(1);
00086 }
00087
00088 object_->num_cp_=points_buffer.size();
00089
00090
00091 for(uint cp_id=0; cp_id < object_->num_cp_; cp_id++)
00092 {
00093 object_->contact_points_[cp_id]->filterDuplicateNeighbors();
00094 delete points_buffer[cp_id];
00095 }
00096 for(uint vn_id=0; vn_id < normals_buffer.size(); vn_id++)
00097 delete normals_buffer[vn_id];
00098
00099 object_->name_=name;
00100 object_loaded_=true;
00101
00102 points_buffer.clear();
00103 normals_buffer.clear();
00104 callback_obj_ptr=NULL;
00105 face_callback_run=false;
00106
00107 #ifdef DEBUG_OBJECT_LOADER //write the loaded object
00108 remove("../debug/points.txt");
00109 remove("../debug/normals.txt");
00110 remove("../debug/neighbors.txt");
00111 FILE* cp=fopen ("../debug/points.txt","a");
00112 FILE* vn=fopen ("../debug/normals.txt","a");
00113 FILE* nb=fopen ("../debug/neighbors.txt","a");
00114 if(!vn |!cp |!nb )
00115 {
00116 std::cout<<"Error in ObjectLoader: Couldn't write to file. Exiting..."<<std::endl;
00117 exit(1);
00118 }
00119 Eigen::Vector3d cp_vtx;
00120 Eigen::Vector3d cp_vtx_normal;
00121 for(uint id=0; id < object_->num_cp_;id++)
00122 {
00123 cp_vtx=*object_->getContactPoint(id)->getVertex();
00124 cp_vtx_normal=*object_->getContactPoint(id)->getVertexNormal();
00125 fprintf(cp, "% f % f % f \n", cp_vtx(0),cp_vtx(1),cp_vtx(2));
00126 fprintf(vn, "% f % f % f \n", cp_vtx_normal(0),cp_vtx_normal(1),cp_vtx_normal(2));
00127
00128 for (ConstIndexListIterator it= object_->getContactPoint(id)->getNeighborItBegin(); it !=object_->getContactPoint(id)->getNeighborItEnd(); it++)
00129 fprintf(nb, "% d",*it+1);
00130
00131 fprintf(nb, "\n");
00132 }
00133 fclose (cp); fclose (vn); fclose (nb);
00134 #endif
00135 }
00136
00137 void ObjectLoader::geometric_vertex_callback(obj::float_type x, obj::float_type y, obj::float_type z)
00138 {
00139 points_buffer.push_back(new double[3]{x,y,z});
00140 }
00141
00142 void ObjectLoader::vertex_normal_callback(obj::float_type x, obj::float_type y, obj::float_type z)
00143 {
00144 assert(fabs(sqrt(x*x+y*y+z*z) -1) <=EPSILON_UNIT_NORMAL);
00145 normals_buffer.push_back(new double[3]{x,y,z});
00146 }
00147
00148 void ObjectLoader::triangular_face_geometric_vertices_vertex_normals_callback(const obj::index_2_tuple_type& t_1, const obj::index_2_tuple_type& t_2, const obj::index_2_tuple_type& t_3)
00149 {
00150
00151 if(!face_callback_run)
00152 {
00153 callback_obj_ptr->contact_points_.resize(points_buffer.size(),NULL);
00154 map_normal2vertex.resize(points_buffer.size());
00155 map_normal2vertex.fill(NOT_VISITED);
00156 }
00157 face_callback_run=true;
00158
00159
00160
00161 uint facet[3]={(uint)(std::tr1::get<0>(t_1)-1),(uint)(std::tr1::get<0>(t_2)-1),(uint)(std::tr1::get<0>(t_3)-1)};
00162 uint vertex_normals[3]={(uint)(std::tr1::get<1>(t_1)-1),(uint)(std::tr1::get<1>(t_2)-1),(uint)(std::tr1::get<1>(t_3)-1)};
00163
00164
00165 for(uint i=0;i<3;i++)
00166 {
00167 if(map_normal2vertex(facet[i])==NOT_VISITED)
00168 {
00169 map_normal2vertex(facet[i])=(int)vertex_normals[i];
00170 callback_obj_ptr->contact_points_[facet[i]]=new ContactPoint(points_buffer[facet[i]],normals_buffer[vertex_normals[i]],facet[i]);
00171 }
00172 else
00173 if(map_normal2vertex(facet[i]) != (int)vertex_normals[i])
00174 {
00175 std::cout<<"Error in ObjectLoader - Provide an .obj file with a unique mapping from vertices to vertex normals. Exiting..." <<std::endl;
00176 exit(1);
00177 }
00178
00179
00180
00181 for(uint j=0; j < 3; j++)
00182 if (i!=j)
00183 callback_obj_ptr->contact_points_[facet[i]]->addNeighbor(facet[j]);
00184 }
00185
00186 }
00187
00188 void ObjectLoader::triangular_face_geometric_vertices_callback(obj::index_type,obj::index_type,obj::index_type )
00189 {
00190 std::cout<<"\n"<<"Error in ObjectLoader - triangular_face_geometric_vertices_callback is only a dummy implementation. Provide an .obj file with vertex normals. Exiting..."<<std::endl;
00191 exit(1);
00192 }
00193
00194 void ObjectLoader::triangular_face_geometric_vertices_texture_vertices_callback(const obj::index_2_tuple_type&, const obj::index_2_tuple_type& , const obj::index_2_tuple_type& )
00195 {
00196 std::cout<<"\n"<<"Error in ObjectLoader - triangular_face_geometric_vertices_texture_vertices_callback is only a dummy implementation. Provide an .obj file without texture vertices. Exiting..."<<std::endl;
00197 exit(1);
00198 }
00199
00200 void ObjectLoader::triangular_face_geometric_vertices_texture_vertices_vertex_normals_callback(const obj::index_3_tuple_type& , const obj::index_3_tuple_type& , const obj::index_3_tuple_type& )
00201 {
00202 std::cout<<"\n"<<"Error in ObjectLoader - triangular_face_geometric_vertices_texture_vertices_vertex_normals_callback is only a dummy implementation. Provide an .obj file without texture vertices. Exiting..." <<std::endl;
00203 exit(1);
00204 }
00205
00206 void ObjectLoader::quadrilateral_face_geometric_vertices_callback(obj::index_type ,obj::index_type ,obj::index_type ,obj::index_type)
00207 {
00208 std::cout<<"\n"<<"Error in ObjectLoader - quadrilateral_face_geometric_vertices_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00209 exit(1);
00210 }
00211
00212 void ObjectLoader::quadrilateral_face_geometric_vertices_texture_vertices_callback(const obj::index_2_tuple_type&, const obj::index_2_tuple_type&, const obj::index_2_tuple_type&, const obj::index_2_tuple_type&)
00213 {
00214 std::cout<<"\n"<<"Error in ObjectLoader - quadrilateral_face_geometric_vertices_texture_vertices_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00215 exit(1);
00216 }
00217
00218 void ObjectLoader::quadrilateral_face_geometric_vertices_vertex_normals_callback(const obj::index_2_tuple_type&, const obj::index_2_tuple_type&, const obj::index_2_tuple_type&, const obj::index_2_tuple_type& )
00219 {
00220 std::cout<<"\n"<<"Error in ObjectLoader - quadrilateral_face_geometric_vertices_vertex_normals_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00221 exit(1);
00222 }
00223
00224 void ObjectLoader::quadrilateral_face_geometric_vertices_texture_vertices_vertex_normals_callback(const obj::index_3_tuple_type& , const obj::index_3_tuple_type&, const obj::index_3_tuple_type& , const obj::index_3_tuple_type& )
00225 {
00226 std::cout<<"\n"<<"Error in ObjectLoader - quadrilateral_face_geometric_vertices_texture_vertices_vertex_normals_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00227 exit(1);
00228 }
00229
00230 void ObjectLoader::polygonal_face_geometric_vertices_begin_callback(obj::index_type ,obj::index_type,obj::index_type)
00231 {
00232 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_begin_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00233 exit(1);
00234 }
00235
00236 void ObjectLoader::polygonal_face_geometric_vertices_vertex_callback(obj::index_type)
00237 {
00238 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_vertex_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00239 exit(1);
00240 }
00241
00242 void ObjectLoader::polygonal_face_geometric_vertices_end_callback()
00243 {
00244 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_end_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00245 exit(1);
00246 }
00247
00248 void ObjectLoader::polygonal_face_geometric_vertices_texture_vertices_begin_callback(const obj::index_2_tuple_type& , const obj::index_2_tuple_type&, const obj::index_2_tuple_type&)
00249 {
00250 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_texture_vertices_begin_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00251 exit(1);
00252 }
00253
00254 void ObjectLoader::polygonal_face_geometric_vertices_texture_vertices_vertex_callback(const obj::index_2_tuple_type&)
00255 {
00256 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_texture_vertices_vertex_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00257 exit(1);
00258 }
00259
00260 void ObjectLoader::polygonal_face_geometric_vertices_texture_vertices_end_callback()
00261 {
00262 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_texture_vertices_end_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00263 exit(1);
00264 }
00265
00266 void ObjectLoader::polygonal_face_geometric_vertices_vertex_normals_begin_callback(const obj::index_2_tuple_type&, const obj::index_2_tuple_type&, const obj::index_2_tuple_type&)
00267 {
00268 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_vertex_normals_begin_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00269 exit(1);
00270 }
00271
00272 void ObjectLoader::polygonal_face_geometric_vertices_vertex_normals_vertex_callback(const obj::index_2_tuple_type&)
00273 {
00274 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_vertex_normals_vertex_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00275 exit(1);
00276 }
00277
00278 void ObjectLoader::polygonal_face_geometric_vertices_vertex_normals_end_callback()
00279 {
00280 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_vertex_normals_end_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00281 exit(1);
00282 }
00283
00284 void ObjectLoader::polygonal_face_geometric_vertices_texture_vertices_vertex_normals_begin_callback(const obj::index_3_tuple_type&, const obj::index_3_tuple_type&, const obj::index_3_tuple_type&)
00285 {
00286 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_texture_vertices_vertex_normals_begin_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00287 exit(1);
00288 }
00289
00290 void ObjectLoader::polygonal_face_geometric_vertices_texture_vertices_vertex_normals_vertex_callback(const obj::index_3_tuple_type&)
00291 {
00292 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_texture_vertices_vertex_normals_vertex_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00293 exit(1);
00294 }
00295
00296 void ObjectLoader::polygonal_face_geometric_vertices_texture_vertices_vertex_normals_end_callback()
00297 {
00298 std::cout<<"\n"<<"Error in ObjectLoader - polygonal_face_geometric_vertices_texture_vertices_vertex_normals_end_callback is only a dummy implementation. Provide an .obj file with a triangulated mesh. Exiting..."<<std::endl;
00299 exit(1);
00300 }
00301
00302
00303 }
00304