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
00031 #define GL3_PROTOTYPES 1
00032 #include <GL3/gl3.h>
00033 #include <GL/freeglut.h>
00034 #include <realtime_urdf_filter/renderable.h>
00035 #include <resource_retriever/retriever.h>
00036 #include <assimp/assimp.hpp>
00037 #include <assimp/aiScene.h>
00038 #include <assimp/aiPostProcess.h>
00039 #include <assimp/IOStream.h>
00040 #include <assimp/IOSystem.h>
00041 #include <ros/assert.h>
00042
00043 namespace realtime_urdf_filter
00044 {
00045
00046 void Renderable::setLinkName (std::string n)
00047 {
00048 name = n;
00049 }
00050
00051 void Renderable::applyTransform ()
00052 {
00053 glPushMatrix ();
00054
00055 tf::Transform transform (link_to_fixed);
00056 transform *= link_offset;
00057 btScalar glTf[16];
00058 transform.getOpenGLMatrix(glTf);
00059 glMultMatrixd((GLdouble*)glTf);
00060 }
00061
00062 void Renderable::unapplyTransform ()
00063 {
00064 glPopMatrix ();
00065 }
00066
00067
00068 RenderableSphere::RenderableSphere (float radius)
00069 : radius(radius)
00070 {}
00071
00072 void RenderableSphere::render ()
00073 {
00074 applyTransform ();
00075 glutSolidSphere(radius, 10, 10);
00076 unapplyTransform ();
00077 }
00078
00079
00080 RenderableCylinder::RenderableCylinder (float radius, float length)
00081 : radius(radius), length(length)
00082 {}
00083
00084 void RenderableCylinder::render ()
00085 {
00086 applyTransform ();
00087 glTranslatef (0, 0, -length/2);
00088
00089 unapplyTransform ();
00090 }
00091
00092
00093 RenderableBox::RenderableBox (float dimx, float dimy, float dimz)
00094 : dimx(dimx), dimy(dimy), dimz(dimz)
00095 {
00096 createBoxVBO ();
00097 }
00098
00099 void RenderableBox::render ()
00100 {
00101 applyTransform ();
00102
00103 glColor3f (color.r, color.g, color.b);
00104
00105
00106 glEnableClientState (GL_VERTEX_ARRAY);
00107 glEnableClientState (GL_NORMAL_ARRAY);
00108
00109 glBindBuffer (GL_ARRAY_BUFFER, vbo);
00110 glVertexPointer (3, GL_FLOAT, 6*sizeof(GLfloat), (char *) NULL);
00111 glNormalPointer (GL_FLOAT, 6*sizeof(GLfloat), ((char *) NULL) + (3 * sizeof(GLfloat)) );
00112
00113
00114 glDrawArrays (GL_QUADS, 0, 24);
00115
00116
00117 glDisableClientState( GL_VERTEX_ARRAY );
00118 glDisableClientState( GL_NORMAL_ARRAY );
00119
00120
00121
00122 unapplyTransform ();
00123 }
00124
00125 void RenderableBox::createBoxVBO ()
00126 {
00127 const GLfloat boxvertices[] =
00128 {0.5f * dimx, 0.5f * dimy,-0.5f * dimz, 0.0f, 1.0f, 0.0f,
00129 -0.5f * dimx, 0.5f * dimy,-0.5f * dimz, 0.0f, 1.0f, 0.0f,
00130 -0.5f * dimx, 0.5f * dimy, 0.5f * dimz, 0.0f, 1.0f, 0.0f,
00131 0.5f * dimx, 0.5f * dimy, 0.5f * dimz, 0.0f, 1.0f, 0.0f,
00132
00133 0.5f * dimx,-0.5f * dimy, 0.5f * dimz, 0.0f,-1.0f, 0.0f,
00134 -0.5f * dimx,-0.5f * dimy, 0.5f * dimz, 0.0f,-1.0f, 0.0f,
00135 -0.5f * dimx,-0.5f * dimy,-0.5f * dimz, 0.0f,-1.0f, 0.0f,
00136 0.5f * dimx,-0.5f * dimy,-0.5f * dimz, 0.0f,-1.0f, 0.0f,
00137
00138 0.5f * dimx, 0.5f * dimy, 0.5f * dimz, 0.0f, 0.0f, 1.0f,
00139 -0.5f * dimx, 0.5f * dimy, 0.5f * dimz, 0.0f, 0.0f, 1.0f,
00140 -0.5f * dimx,-0.5f * dimy, 0.5f * dimz, 0.0f, 0.0f, 1.0f,
00141 0.5f * dimx,-0.5f * dimy, 0.5f * dimz, 0.0f, 0.0f, 1.0f,
00142
00143 0.5f * dimx,-0.5f * dimy,-0.5f * dimz, 0.0f, 0.0f,-1.0f,
00144 -0.5f * dimx,-0.5f * dimy,-0.5f * dimz, 0.0f, 0.0f,-1.0f,
00145 -0.5f * dimx, 0.5f * dimy,-0.5f * dimz, 0.0f, 0.0f,-1.0f,
00146 0.5f * dimx, 0.5f * dimy,-0.5f * dimz, 0.0f, 0.0f,-1.0f,
00147
00148 -0.5f * dimx, 0.5f * dimy, 0.5f * dimz,-1.0f, 0.0f, 0.0f,
00149 -0.5f * dimx, 0.5f * dimy,-0.5f * dimz,-1.0f, 0.0f, 0.0f,
00150 -0.5f * dimx,-0.5f * dimy,-0.5f * dimz,-1.0f, 0.0f, 0.0f,
00151 -0.5f * dimx,-0.5f * dimy, 0.5f * dimz,-1.0f, 0.0f, 0.0f,
00152
00153 0.5f * dimx, 0.5f * dimy,-0.5f * dimz, 1.0f, 0.0f, 0.0f,
00154 0.5f * dimx, 0.5f * dimy, 0.5f * dimz, 1.0f, 0.0f, 0.0f,
00155 0.5f * dimx,-0.5f * dimy, 0.5f * dimz, 1.0f, 0.0f, 0.0f,
00156 0.5f * dimx,-0.5f * dimy,-0.5f * dimz, 1.0f, 0.0f, 0.0f};
00157
00158
00159 glGenBuffers (1, &vbo);
00160 glBindBuffer (GL_ARRAY_BUFFER, vbo);
00161 glBufferData (GL_ARRAY_BUFFER, 24 * 6 * sizeof(GLfloat), boxvertices, GL_STATIC_DRAW);
00162 }
00163
00164
00165 class ResourceIOStream : public Assimp::IOStream
00166 {
00167 public:
00168 ResourceIOStream(const resource_retriever::MemoryResource& res)
00169 : res_(res)
00170 , pos_(res.data.get())
00171 {}
00172
00173 ~ResourceIOStream()
00174 {}
00175
00176 size_t Read(void* buffer, size_t size, size_t count)
00177 {
00178 size_t to_read = size * count;
00179 if (pos_ + to_read > res_.data.get() + res_.size)
00180 {
00181 to_read = res_.size - (pos_ - res_.data.get());
00182 }
00183
00184 memcpy(buffer, pos_, to_read);
00185 pos_ += to_read;
00186
00187 return to_read;
00188 }
00189
00190 size_t Write( const void* buffer, size_t size, size_t count) { ROS_BREAK(); return 0; }
00191
00192 aiReturn Seek( size_t offset, aiOrigin origin)
00193 {
00194 uint8_t* new_pos = 0;
00195 switch (origin)
00196 {
00197 case aiOrigin_SET:
00198 new_pos = res_.data.get() + offset;
00199 break;
00200 case aiOrigin_CUR:
00201 new_pos = pos_ + offset;
00202 break;
00203 case aiOrigin_END:
00204 new_pos = res_.data.get() + res_.size - offset;
00205 break;
00206 default:
00207 ROS_BREAK();
00208 }
00209
00210 if (new_pos < res_.data.get() || new_pos > res_.data.get() + res_.size)
00211 {
00212 return aiReturn_FAILURE;
00213 }
00214
00215 pos_ = new_pos;
00216 return aiReturn_SUCCESS;
00217 }
00218
00219 size_t Tell() const
00220 {
00221 return pos_ - res_.data.get();
00222 }
00223
00224 size_t FileSize() const
00225 {
00226 return res_.size;
00227 }
00228
00229 void Flush() {}
00230
00231 private:
00232 resource_retriever::MemoryResource res_;
00233 uint8_t* pos_;
00234 };
00235
00236 class ResourceIOSystem : public Assimp::IOSystem
00237 {
00238 public:
00239 ResourceIOSystem()
00240 {
00241 }
00242
00243 ~ResourceIOSystem()
00244 {
00245 }
00246
00247
00248 bool Exists(const char* file) const
00249 {
00250
00251
00252
00253 resource_retriever::MemoryResource res;
00254 try
00255 {
00256 res = retriever_.get(file);
00257 }
00258 catch (resource_retriever::Exception& e)
00259 {
00260 return false;
00261 }
00262
00263 return true;
00264 }
00265
00266
00267 char getOsSeparator() const
00268 {
00269 return '/';
00270 }
00271
00272
00273 Assimp::IOStream* Open(const char* file, const char* mode)
00274 {
00275 ROS_ASSERT(mode == std::string("r") || mode == std::string("rb"));
00276
00277
00278
00279 resource_retriever::MemoryResource res;
00280 try
00281 {
00282 res = retriever_.get(file);
00283 }
00284 catch (resource_retriever::Exception& e)
00285 {
00286 return 0;
00287 }
00288
00289 return new ResourceIOStream(res);
00290 }
00291
00292 void Close(Assimp::IOStream* stream) { delete stream; }
00293
00294 private:
00295 mutable resource_retriever::Retriever retriever_;
00296 };
00297
00298 RenderableMesh::RenderableMesh (std::string meshname) :
00299 meshname_(meshname)
00300 {
00301 Assimp::Importer importer;
00302 importer.SetIOHandler(new ResourceIOSystem());
00303 const aiScene* scene = importer.ReadFile(meshname, aiProcess_PreTransformVertices|aiProcess_SortByPType|aiProcess_GenNormals|aiProcess_Triangulate|aiProcess_GenUVCoords|aiProcess_FlipUVs);
00304 if (!scene)
00305 {
00306 ROS_ERROR("Could not load resource [%s]: %s", meshname.c_str(), importer.GetErrorString());
00307 return;
00308 }
00309
00310 fromAssimpScene(scene);
00311 }
00312
00313 RenderableMesh::SubMesh::SubMesh ()
00314 {
00315 vbo = INVALID_VALUE;
00316 ibo = INVALID_VALUE;
00317 num_indices = 0;
00318 }
00319
00320 RenderableMesh::SubMesh::~SubMesh ()
00321 {
00322 if (vbo != INVALID_VALUE)
00323 glDeleteBuffers (1, &vbo);
00324 if (ibo != INVALID_VALUE)
00325 glDeleteBuffers (1, &ibo);
00326 }
00327
00328 void RenderableMesh::SubMesh::init (const std::vector<Vertex>& vertices,
00329 const std::vector<unsigned int>& indices)
00330 {
00331 num_indices = indices.size ();
00332 glGenBuffers (1, &vbo);
00333 glBindBuffer (GL_ARRAY_BUFFER, vbo);
00334 glBufferData (GL_ARRAY_BUFFER, sizeof(Vertex) * vertices.size (), &vertices[0], GL_STATIC_DRAW);
00335
00336 glGenBuffers (1, &ibo);
00337 glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, ibo);
00338 glBufferData (GL_ELEMENT_ARRAY_BUFFER, sizeof(unsigned int) * num_indices, &indices[0], GL_STATIC_DRAW);
00339 }
00340
00341 void RenderableMesh::fromAssimpScene (const aiScene* scene)
00342 {
00343 meshes.resize (scene->mNumMeshes);
00344 for (unsigned int i = 0; i < meshes.size (); ++i)
00345 {
00346 const aiMesh* mesh = scene->mMeshes[i];
00347 initMesh (i, mesh, scene->mRootNode);
00348 }
00349 }
00350
00351 void RenderableMesh::initMesh (unsigned int index, const aiMesh* mesh, const aiNode* node)
00352 {
00353
00354
00355 std::vector<Vertex> vertices;
00356 std::vector<unsigned int> indices;
00357
00358
00359 if (!node) {
00360 return;
00361 }
00362
00363 ROS_DEBUG_STREAM("Parsing mesh: "<<meshname_);
00364 aiMatrix4x4 transform = node->mTransformation;
00365 aiMatrix3x3 rotation(transform);
00366
00367 ROS_INFO_STREAM(" transform: "<<std::endl
00368 <<std::fixed
00369 <<"[ "<<*transform[0]<<" \t"<<*transform[1]<<" \t"<<*transform[2]<<" \t"<<*transform[3]<<std::endl
00370 <<" "<<*transform[4]<<" \t"<<*transform[5]<<" \t"<<*transform[6]<<" \t"<<*transform[7]<<std::endl
00371 <<" "<<*transform[8]<<" \t"<<*transform[9]<<" \t"<<*transform[10]<<" \t"<<*transform[11]<<std::endl
00372 <<" "<<*transform[12]<<" \t"<<*transform[13]<<" \t"<<*transform[14]<<" \t"<<*transform[15]<<std::endl);
00373 aiNode *pnode = node->mParent;
00374
00375
00376
00377 for (unsigned int i = 0; i < mesh->mNumVertices; ++i)
00378 {
00379 aiVector3D pos = mesh->mVertices[i];
00380 aiVector3D n = mesh->mNormals[i];
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390 Vertex v(pos.x, pos.y, pos.z, n.x, n.y, n.z);
00391 vertices.push_back (v);
00392 }
00393
00394 for (unsigned int i = 0 ; i < mesh->mNumFaces ; ++i)
00395 {
00396 const aiFace& face = mesh->mFaces[i];
00397 assert(face.mNumIndices == 3);
00398 indices.push_back(face.mIndices[0]);
00399 indices.push_back(face.mIndices[1]);
00400 indices.push_back(face.mIndices[2]);
00401 }
00402
00403 meshes[index].init (vertices, indices);
00404 }
00405
00406 void RenderableMesh::setScale (float x, float y, float z)
00407 {
00408 scale_x = x;
00409 scale_y = y;
00410 scale_z = z;
00411 }
00412
00413 void RenderableMesh::render ()
00414 {
00415 applyTransform ();
00416 glScalef (scale_x, scale_y, scale_z);
00417 glEnableVertexAttribArray (0);
00418 glEnableVertexAttribArray (2);
00419
00420 for (unsigned int i = 0 ; i < meshes.size() ; i++)
00421 {
00422 glBindBuffer (GL_ARRAY_BUFFER, meshes[i].vbo);
00423 glVertexAttribPointer (0, 3, GL_FLOAT, GL_FALSE, sizeof (Vertex), 0);
00424 glVertexAttribPointer (2, 3, GL_FLOAT, GL_FALSE, sizeof (Vertex), (const GLvoid*) (sizeof(float)*3));
00425
00426 glBindBuffer (GL_ELEMENT_ARRAY_BUFFER, meshes[i].ibo);
00427
00428
00429
00430
00431
00432
00433
00434
00435 glDrawElements (GL_TRIANGLES, meshes[i].num_indices, GL_UNSIGNED_INT, 0);
00436 }
00437
00438 glDisableVertexAttribArray (0);
00439 glDisableVertexAttribArray (2);
00440 unapplyTransform ();
00441 }
00442
00443
00444 }
00445
00446