00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "mesh_loader.h"
00031 #include <resource_retriever/retriever.h>
00032
00033 #include <boost/filesystem.hpp>
00034
00035 #include "ogre_helpers/stl_loader.h"
00036
00037 #include <OgreMeshManager.h>
00038 #include <OgreTextureManager.h>
00039 #include <OgreMaterialManager.h>
00040 #include <OgreTexture.h>
00041 #include <OgrePass.h>
00042 #include <OgreTechnique.h>
00043 #include <OgreMaterial.h>
00044 #include <OgreTextureUnitState.h>
00045 #include <OgreMeshSerializer.h>
00046 #include <OgreSubMesh.h>
00047 #include <OgreHardwareBufferManager.h>
00048 #include <OgreSharedPtr.h>
00049 #include <OgreTechnique.h>
00050
00051 #include <tinyxml.h>
00052
00053
00054 #include <ros/assert.h>
00055
00056 #if defined(ASSIMP_UNIFIED_HEADER_NAMES)
00057 #include <assimp/Importer.hpp>
00058 #include <assimp/scene.h>
00059 #include <assimp/postprocess.h>
00060 #include <assimp/IOStream.hpp>
00061 #include <assimp/IOSystem.hpp>
00062 #else
00063 #include <assimp/assimp.hpp>
00064 #include <assimp/aiScene.h>
00065 #include <assimp/aiPostProcess.h>
00066 #include <assimp/IOStream.h>
00067 #include <assimp/IOSystem.h>
00068 #endif
00069
00070 namespace fs = boost::filesystem;
00071
00072 namespace rviz
00073 {
00074
00075 class ResourceIOStream : public Assimp::IOStream
00076 {
00077 public:
00078 ResourceIOStream(const resource_retriever::MemoryResource& res)
00079 : res_(res)
00080 , pos_(res.data.get())
00081 {}
00082
00083 ~ResourceIOStream()
00084 {}
00085
00086 size_t Read(void* buffer, size_t size, size_t count)
00087 {
00088 size_t to_read = size * count;
00089 if (pos_ + to_read > res_.data.get() + res_.size)
00090 {
00091 to_read = res_.size - (pos_ - res_.data.get());
00092 }
00093
00094 memcpy(buffer, pos_, to_read);
00095 pos_ += to_read;
00096
00097 return to_read;
00098 }
00099
00100 size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
00101
00102 aiReturn Seek( size_t offset, aiOrigin origin)
00103 {
00104 uint8_t* new_pos = 0;
00105 switch (origin)
00106 {
00107 case aiOrigin_SET:
00108 new_pos = res_.data.get() + offset;
00109 break;
00110 case aiOrigin_CUR:
00111 new_pos = pos_ + offset;
00112 break;
00113 case aiOrigin_END:
00114 new_pos = res_.data.get() + res_.size - offset;
00115 break;
00116 default:
00117 ROS_BREAK();
00118 }
00119
00120 if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
00121 {
00122 return aiReturn_FAILURE;
00123 }
00124
00125 pos_ = new_pos;
00126 return aiReturn_SUCCESS;
00127 }
00128
00129 size_t Tell() const
00130 {
00131 return pos_ - res_.data.get();
00132 }
00133
00134 size_t FileSize() const
00135 {
00136 return res_.size;
00137 }
00138
00139 void Flush() {}
00140
00141 private:
00142 resource_retriever::MemoryResource res_;
00143 uint8_t* pos_;
00144 };
00145
00146 class ResourceIOSystem : public Assimp::IOSystem
00147 {
00148 public:
00149 ResourceIOSystem()
00150 {
00151 }
00152
00153 ~ResourceIOSystem()
00154 {
00155 }
00156
00157
00158 bool Exists(const char* file) const
00159 {
00160
00161
00162
00163 resource_retriever::MemoryResource res;
00164 try
00165 {
00166 res = retriever_.get(file);
00167 }
00168 catch (resource_retriever::Exception& e)
00169 {
00170 return false;
00171 }
00172
00173 return true;
00174 }
00175
00176
00177 char getOsSeparator() const
00178 {
00179 return '/';
00180 }
00181
00182
00183 Assimp::IOStream* Open(const char* file, const char* mode = "rb")
00184 {
00185 ROS_ASSERT(mode == std::string("r") || mode == std::string("rb"));
00186
00187
00188
00189 resource_retriever::MemoryResource res;
00190 try
00191 {
00192 res = retriever_.get(file);
00193 }
00194 catch (resource_retriever::Exception& e)
00195 {
00196 return 0;
00197 }
00198
00199 return new ResourceIOStream(res);
00200 }
00201
00202 void Close(Assimp::IOStream* stream);
00203
00204 private:
00205 mutable resource_retriever::Retriever retriever_;
00206 };
00207
00208 void ResourceIOSystem::Close(Assimp::IOStream* stream)
00209 {
00210 delete stream;
00211 }
00212
00213
00218 void buildMesh( const aiScene* scene, const aiNode* node,
00219 const Ogre::MeshPtr& mesh,
00220 Ogre::AxisAlignedBox& aabb, float& radius, const float scale,
00221 std::vector<Ogre::MaterialPtr>& material_table )
00222 {
00223 if (!node)
00224 {
00225 return;
00226 }
00227
00228 aiMatrix4x4 transform = node->mTransformation;
00229 aiNode *pnode = node->mParent;
00230 while (pnode)
00231 {
00232
00233
00234 if (pnode->mParent != NULL)
00235 transform = pnode->mTransformation * transform;
00236 pnode = pnode->mParent;
00237 }
00238
00239 aiMatrix3x3 rotation(transform);
00240 aiMatrix3x3 inverse_transpose_rotation(rotation);
00241 inverse_transpose_rotation.Inverse();
00242 inverse_transpose_rotation.Transpose();
00243
00244 for (uint32_t i = 0; i < node->mNumMeshes; i++)
00245 {
00246 aiMesh* input_mesh = scene->mMeshes[node->mMeshes[i]];
00247
00248 Ogre::SubMesh* submesh = mesh->createSubMesh();
00249 submesh->useSharedVertices = false;
00250 submesh->vertexData = new Ogre::VertexData();
00251 Ogre::VertexData* vertex_data = submesh->vertexData;
00252 Ogre::VertexDeclaration* vertex_decl = vertex_data->vertexDeclaration;
00253
00254 size_t offset = 0;
00255
00256 vertex_decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
00257 offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00258
00259
00260 if (input_mesh->HasNormals())
00261 {
00262 vertex_decl->addElement(0, offset, Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
00263 offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3);
00264 }
00265
00266
00267 if (input_mesh->HasTextureCoords(0))
00268 {
00269 vertex_decl->addElement(0, offset, Ogre::VET_FLOAT2, Ogre::VES_TEXTURE_COORDINATES, 0);
00270 offset += Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT2);
00271 }
00272
00273
00274
00275
00276 vertex_data->vertexCount = input_mesh->mNumVertices;
00277 Ogre::HardwareVertexBufferSharedPtr vbuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(vertex_decl->getVertexSize(0),
00278 vertex_data->vertexCount,
00279 Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY,
00280 false);
00281
00282 vertex_data->vertexBufferBinding->setBinding(0, vbuf);
00283 float* vertices = static_cast<float*>(vbuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
00284
00285
00286 for (uint32_t j = 0; j < input_mesh->mNumVertices; j++)
00287 {
00288 aiVector3D p = input_mesh->mVertices[j];
00289 p *= transform;
00290 p *= scale;
00291 *vertices++ = p.x;
00292 *vertices++ = p.y;
00293 *vertices++ = p.z;
00294
00295 Ogre::Vector3 v(p.x, p.y, p.z);
00296 aabb.merge(v);
00297 float dist = v.length();
00298 if (dist > radius)
00299 {
00300 radius = dist;
00301 }
00302
00303 if (input_mesh->HasNormals())
00304 {
00305 aiVector3D n = inverse_transpose_rotation * input_mesh->mNormals[j];
00306 n.Normalize();
00307 *vertices++ = n.x;
00308 *vertices++ = n.y;
00309 *vertices++ = n.z;
00310 }
00311
00312 if (input_mesh->HasTextureCoords(0))
00313 {
00314 *vertices++ = input_mesh->mTextureCoords[0][j].x;
00315 *vertices++ = input_mesh->mTextureCoords[0][j].y;
00316 }
00317 }
00318
00319
00320 submesh->indexData->indexCount = 0;
00321 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
00322 {
00323 aiFace& face = input_mesh->mFaces[j];
00324 submesh->indexData->indexCount += face.mNumIndices;
00325 }
00326
00327
00328 if( vertex_data->vertexCount < (1<<16) )
00329 {
00330
00331 submesh->indexData->indexBuffer = Ogre::HardwareBufferManager::getSingleton().createIndexBuffer(
00332 Ogre::HardwareIndexBuffer::IT_16BIT,
00333 submesh->indexData->indexCount,
00334 Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY,
00335 false);
00336
00337 Ogre::HardwareIndexBufferSharedPtr ibuf = submesh->indexData->indexBuffer;
00338 uint16_t* indices = static_cast<uint16_t*>(ibuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
00339
00340
00341 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
00342 {
00343 aiFace& face = input_mesh->mFaces[j];
00344 for (uint32_t k = 0; k < face.mNumIndices; ++k)
00345 {
00346 *indices++ = face.mIndices[k];
00347 }
00348 }
00349
00350 ibuf->unlock();
00351 }
00352 else
00353 {
00354
00355
00356
00357
00358
00359 submesh->indexData->indexBuffer = Ogre::HardwareBufferManager::getSingleton().createIndexBuffer(
00360 Ogre::HardwareIndexBuffer::IT_32BIT,
00361 submesh->indexData->indexCount,
00362 Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY,
00363 false);
00364
00365 Ogre::HardwareIndexBufferSharedPtr ibuf = submesh->indexData->indexBuffer;
00366 uint32_t* indices = static_cast<uint32_t*>(ibuf->lock(Ogre::HardwareBuffer::HBL_DISCARD));
00367
00368
00369 for (uint32_t j = 0; j < input_mesh->mNumFaces; j++)
00370 {
00371 aiFace& face = input_mesh->mFaces[j];
00372 for (uint32_t k = 0; k < face.mNumIndices; ++k)
00373 {
00374 *indices++ = face.mIndices[k];
00375 }
00376 }
00377
00378 ibuf->unlock();
00379 }
00380 vbuf->unlock();
00381
00382 submesh->setMaterialName(material_table[input_mesh->mMaterialIndex]->getName());
00383 }
00384
00385 for (uint32_t i=0; i < node->mNumChildren; ++i)
00386 {
00387 buildMesh(scene, node->mChildren[i], mesh, aabb, radius, scale, material_table);
00388 }
00389 }
00390
00391 void loadTexture(const std::string& resource_path)
00392 {
00393 if (!Ogre::TextureManager::getSingleton().resourceExists(resource_path))
00394 {
00395 resource_retriever::Retriever retriever;
00396 resource_retriever::MemoryResource res;
00397 try
00398 {
00399 res = retriever.get(resource_path);
00400 }
00401 catch (resource_retriever::Exception& e)
00402 {
00403 ROS_ERROR("%s", e.what());
00404 }
00405
00406 if (res.size != 0)
00407 {
00408 Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
00409 Ogre::Image image;
00410 std::string extension = fs::extension(fs::path(resource_path));
00411
00412 if (extension[0] == '.')
00413 {
00414 extension = extension.substr(1, extension.size() - 1);
00415 }
00416
00417 try
00418 {
00419 image.load(stream, extension);
00420 Ogre::TextureManager::getSingleton().loadImage(resource_path, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
00421 }
00422 catch (Ogre::Exception& e)
00423 {
00424 ROS_ERROR("Could not load texture [%s]: %s", resource_path.c_str(), e.what());
00425 }
00426 }
00427 }
00428 }
00429
00430
00437 void loadMaterials(const std::string& resource_path,
00438 const aiScene* scene,
00439 std::vector<Ogre::MaterialPtr>& material_table_out )
00440 {
00441 for (uint32_t i = 0; i < scene->mNumMaterials; i++)
00442 {
00443 std::stringstream ss;
00444 ss << resource_path << "Material" << i;
00445 Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(ss.str(), ROS_PACKAGE_NAME, true);
00446 material_table_out.push_back(mat);
00447
00448 Ogre::Technique* tech = mat->getTechnique(0);
00449 Ogre::Pass* pass = tech->getPass(0);
00450
00451 aiMaterial *amat = scene->mMaterials[i];
00452
00453 Ogre::ColourValue diffuse(1.0, 1.0, 1.0, 1.0);
00454 Ogre::ColourValue specular(1.0, 1.0, 1.0, 1.0);
00455 Ogre::ColourValue ambient(0.5, 0.5, 0.5, 1.0);
00456
00457 for (uint32_t j=0; j < amat->mNumProperties; j++)
00458 {
00459 aiMaterialProperty *prop = amat->mProperties[j];
00460 std::string propKey = prop->mKey.data;
00461
00462 if (propKey == "$tex.file")
00463 {
00464 aiString texName;
00465 aiTextureMapping mapping;
00466 uint32_t uvIndex;
00467 amat->GetTexture(aiTextureType_DIFFUSE,0, &texName, &mapping, &uvIndex);
00468
00469
00470 std::string texture_path = fs::path(resource_path).parent_path().string() + "/" + texName.data;
00471 loadTexture(texture_path);
00472 Ogre::TextureUnitState* tu = pass->createTextureUnitState();
00473 tu->setTextureName(texture_path);
00474 }
00475 else if (propKey == "$clr.diffuse")
00476 {
00477 aiColor3D clr;
00478 amat->Get(AI_MATKEY_COLOR_DIFFUSE, clr);
00479 diffuse = Ogre::ColourValue(clr.r, clr.g, clr.b);
00480 }
00481 else if (propKey == "$clr.ambient")
00482 {
00483 aiColor3D clr;
00484 amat->Get(AI_MATKEY_COLOR_AMBIENT, clr);
00485
00486
00487 if (clr.r > 0 && clr.g > 0 && clr.b > 0)
00488 {
00489 ambient = Ogre::ColourValue(clr.r, clr.g, clr.b);
00490 }
00491 }
00492 else if (propKey == "$clr.specular")
00493 {
00494 aiColor3D clr;
00495 amat->Get(AI_MATKEY_COLOR_SPECULAR, clr);
00496 specular = Ogre::ColourValue(clr.r, clr.g, clr.b);
00497 }
00498 else if (propKey == "$clr.emissive")
00499 {
00500 aiColor3D clr;
00501 amat->Get(AI_MATKEY_COLOR_EMISSIVE, clr);
00502 mat->setSelfIllumination(clr.r, clr.g, clr.b);
00503 }
00504 else if (propKey == "$clr.opacity")
00505 {
00506 float o;
00507 amat->Get(AI_MATKEY_OPACITY, o);
00508 diffuse.a = o;
00509 }
00510 else if (propKey == "$mat.shininess")
00511 {
00512 float s;
00513 amat->Get(AI_MATKEY_SHININESS, s);
00514 mat->setShininess(s);
00515 }
00516 else if (propKey == "$mat.shadingm")
00517 {
00518 int model;
00519 amat->Get(AI_MATKEY_SHADING_MODEL, model);
00520 switch(model)
00521 {
00522 case aiShadingMode_Flat:
00523 mat->setShadingMode(Ogre::SO_FLAT);
00524 break;
00525 case aiShadingMode_Phong:
00526 mat->setShadingMode(Ogre::SO_PHONG);
00527 break;
00528 case aiShadingMode_Gouraud:
00529 default:
00530 mat->setShadingMode(Ogre::SO_GOURAUD);
00531 break;
00532 }
00533 }
00534 }
00535
00536 int mode = aiBlendMode_Default;
00537 amat->Get(AI_MATKEY_BLEND_FUNC, mode);
00538 switch(mode)
00539 {
00540 case aiBlendMode_Additive:
00541 mat->setSceneBlending(Ogre::SBT_ADD);
00542 break;
00543 case aiBlendMode_Default:
00544 default:
00545 {
00546 if (diffuse.a < 0.99)
00547 {
00548 pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00549 }
00550 else
00551 {
00552 pass->setSceneBlending(Ogre::SBT_REPLACE);
00553 }
00554 }
00555 break;
00556 }
00557
00558 mat->setAmbient(ambient);
00559 mat->setDiffuse(diffuse);
00560 specular.a = diffuse.a;
00561 mat->setSpecular(specular);
00562 }
00563 }
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578 float getMeshUnitRescale(const std::string& resource_path)
00579 {
00580 static std::map<std::string, float> rescale_cache;
00581
00582
00583
00584
00585 TiXmlDocument xmlDoc;
00586 float unit_scale(1.0);
00587 resource_retriever::Retriever retriever;
00588 resource_retriever::MemoryResource res;
00589 try
00590 {
00591 res = retriever.get(resource_path);
00592 }
00593 catch (resource_retriever::Exception& e)
00594 {
00595 ROS_ERROR("%s", e.what());
00596 return unit_scale;
00597 }
00598
00599 if (res.size == 0)
00600 {
00601 return unit_scale;
00602 }
00603
00604
00605
00606 const char * data = reinterpret_cast<const char * > (res.data.get());
00607 xmlDoc.Parse(data);
00608
00609
00610 if(!xmlDoc.Error())
00611 {
00612 TiXmlElement * colladaXml = xmlDoc.FirstChildElement("COLLADA");
00613 if(colladaXml)
00614 {
00615 TiXmlElement *assetXml = colladaXml->FirstChildElement("asset");
00616 if(assetXml)
00617 {
00618 TiXmlElement *unitXml = assetXml->FirstChildElement("unit");
00619 if (unitXml && unitXml->Attribute("meter"))
00620 {
00621
00622 if(unitXml->QueryFloatAttribute("meter", &unit_scale) != 0)
00623 ROS_WARN_STREAM("getMeshUnitRescale::Failed to convert unit element meter attribute to determine scaling. unit element: "
00624 << *unitXml);
00625 }
00626 }
00627 }
00628 }
00629 return unit_scale;
00630 }
00631
00632
00633
00634 Ogre::MeshPtr meshFromAssimpScene(const std::string& name, const aiScene* scene)
00635 {
00636 if (!scene->HasMeshes())
00637 {
00638 ROS_ERROR("No meshes found in file [%s]", name.c_str());
00639 return Ogre::MeshPtr();
00640 }
00641
00642 std::vector<Ogre::MaterialPtr> material_table;
00643 loadMaterials(name, scene, material_table);
00644
00645 Ogre::MeshPtr mesh = Ogre::MeshManager::getSingleton().createManual(name, ROS_PACKAGE_NAME);
00646
00647 Ogre::AxisAlignedBox aabb(Ogre::AxisAlignedBox::EXTENT_NULL);
00648 float radius = 0.0f;
00649 float scale = getMeshUnitRescale(name);
00650 buildMesh(scene, scene->mRootNode, mesh, aabb, radius, scale, material_table);
00651
00652 mesh->_setBounds(aabb);
00653 mesh->_setBoundingSphereRadius(radius);
00654 mesh->buildEdgeList();
00655
00656 mesh->load();
00657
00658 return mesh;
00659 }
00660
00661 Ogre::MeshPtr loadMeshFromResource(const std::string& resource_path)
00662 {
00663 if (Ogre::MeshManager::getSingleton().resourceExists(resource_path))
00664 {
00665 return Ogre::MeshManager::getSingleton().getByName(resource_path);
00666 }
00667 else
00668 {
00669 fs::path model_path(resource_path);
00670 #if BOOST_FILESYSTEM_VERSION == 3
00671 std::string ext = model_path.extension().string();
00672 #else
00673 std::string ext = model_path.extension();
00674 #endif
00675 if (ext == ".mesh" || ext == ".MESH")
00676 {
00677 resource_retriever::Retriever retriever;
00678 resource_retriever::MemoryResource res;
00679 try
00680 {
00681 res = retriever.get(resource_path);
00682 }
00683 catch (resource_retriever::Exception& e)
00684 {
00685 ROS_ERROR("%s", e.what());
00686 return Ogre::MeshPtr();
00687 }
00688
00689 if (res.size == 0)
00690 {
00691 return Ogre::MeshPtr();
00692 }
00693
00694 Ogre::MeshSerializer ser;
00695 Ogre::DataStreamPtr stream(new Ogre::MemoryDataStream(res.data.get(), res.size));
00696 Ogre::MeshPtr mesh = Ogre::MeshManager::getSingleton().createManual(resource_path, "rviz");
00697 ser.importMesh(stream, mesh.get());
00698
00699 return mesh;
00700 }
00701 else if (ext == ".stl" || ext == ".STL" || ext == ".stlb" || ext == ".STLB")
00702 {
00703 resource_retriever::Retriever retriever;
00704 resource_retriever::MemoryResource res;
00705 try
00706 {
00707 res = retriever.get(resource_path);
00708 }
00709 catch (resource_retriever::Exception& e)
00710 {
00711 ROS_ERROR("%s", e.what());
00712 return Ogre::MeshPtr();
00713 }
00714
00715 if (res.size == 0)
00716 {
00717 return Ogre::MeshPtr();
00718 }
00719
00720 ogre_tools::STLLoader loader;
00721 if (!loader.load(res.data.get(), res.size, resource_path))
00722 {
00723 ROS_ERROR("Failed to load file [%s]", resource_path.c_str());
00724 return Ogre::MeshPtr();
00725 }
00726
00727 return loader.toMesh(resource_path);
00728 }
00729 else
00730 {
00731 Assimp::Importer importer;
00732 importer.SetIOHandler(new ResourceIOSystem());
00733 const aiScene* scene = importer.ReadFile(resource_path, aiProcess_SortByPType|aiProcess_GenNormals|aiProcess_Triangulate|aiProcess_GenUVCoords|aiProcess_FlipUVs);
00734 if (!scene)
00735 {
00736 ROS_ERROR("Could not load resource [%s]: %s", resource_path.c_str(), importer.GetErrorString());
00737 return Ogre::MeshPtr();
00738 }
00739
00740 return meshFromAssimpScene(resource_path, scene);
00741 }
00742 }
00743
00744 return Ogre::MeshPtr();
00745 }
00746
00747 }