stl_loader.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "stl_loader.h"
00031 #include <ros/console.h>
00032 
00033 #include <OgreManualObject.h>
00034 
00035 namespace ogre_tools
00036 {
00037 
00038 STLLoader::STLLoader()
00039 {
00040 
00041 }
00042 
00043 STLLoader::~STLLoader()
00044 {
00045 
00046 }
00047 
00048 bool STLLoader::load(const std::string& path)
00049 {
00050   FILE* input = fopen( path.c_str(), "r" );
00051   if ( !input )
00052   {
00053     ROS_ERROR( "Could not open '%s' for read", path.c_str() );
00054     return false;
00055   }
00056 
00057   /* from wikipedia:
00058    * Because ASCII STL files can become very large, a binary version of STL exists. A binary STL file has an 80 character header
00059    * (which is generally ignored - but which should never begin with 'solid' because that will lead most software to assume that
00060    * this is an ASCII STL file). Following the header is a 4 byte unsigned integer indicating the number of triangular facets in
00061    * the file. Following that is data describing each triangle in turn. The file simply ends after the last triangle.
00062    *
00063    * Each triangle is described by twelve 32-bit-floating point numbers: three for the normal and then three for the X/Y/Z coordinate
00064    * of each vertex - just as with the ASCII version of STL. After the twelve floats there is a two byte unsigned 'short' integer that
00065    * is the 'attribute byte count' - in the standard format, this should be zero because most software does not understand anything else.
00066    *
00067    * Floating point numbers are represented as IEEE floating point numbers and the endianness is assumed to be little endian although this
00068    * is not stated in documentation.
00069    */
00070 
00071   // find the file size
00072   fseek( input, 0, SEEK_END );
00073   long fileSize = ftell( input );
00074   rewind( input );
00075 
00076   std::vector<uint8_t> buffer_vec(fileSize);
00077   uint8_t* buffer = &buffer_vec[0];
00078 
00079   long num_bytes_read = fread( buffer, 1, fileSize, input );
00080   if ( num_bytes_read != fileSize )
00081   {
00082     ROS_ERROR("STLLoader::load( \"%s\" ) only read %ld bytes out of total %ld.",
00083               path.c_str(), num_bytes_read, fileSize);
00084     fclose( input );
00085     return false;
00086   }
00087   fclose( input );
00088 
00089   return this->load(buffer, num_bytes_read, path);
00090 }
00091 
00092 bool STLLoader::load(uint8_t* buffer, const size_t num_bytes, const std::string& origin)
00093 {
00094   // check for ascii since we can only load binary types with this class
00095   std::string buffer_str = std::string(reinterpret_cast<char *>(buffer), num_bytes);
00096 
00097   if (buffer_str.substr(0, 5) == std::string("solid"))
00098   {
00099     // file says that it is ascii, but why should we trust it?
00100 
00101     // check for "endsolid" as well
00102     if (buffer_str.find("endsolid", 5) != std::string::npos)
00103     {
00104       ROS_ERROR_STREAM("The STL file '" << origin << "' is malformed. It "
00105                        "starts with the word 'solid' and also contains the "
00106                        "word 'endsolid', indicating that it's an ASCII STL "
00107                        "file, but rviz can only load binary STL files so it "
00108                        "will not be loaded. Please convert it to a "
00109                        "binary STL file.");
00110       return false;
00111     }
00112 
00113     // chastise the user for malformed files
00114     ROS_WARN_STREAM("The STL file '" << origin << "' is malformed. It starts "
00115                     "with the word 'solid', indicating that it's an ASCII "
00116                     "STL file, but it does not contain the word 'endsolid' so "
00117                     "it is either a malformed ASCII STL file or it is actually "
00118                     "a binary STL file. Trying to interpret it as a binary "
00119                     "STL file instead.");
00120   }
00121 
00122   // make sure there's enough data for a binary STL header and triangle count
00123   static const size_t binary_stl_header_len = 84;
00124   if (num_bytes <= binary_stl_header_len)
00125   {
00126     ROS_ERROR_STREAM("The STL file '" << origin << "' is malformed. It "
00127                      "appears to be a binary STL file but does not contain "
00128                      "enough data for the 80 byte header and 32-bit integer "
00129                      "triangle count.");
00130     return false;
00131   }
00132 
00133   // one last check to make sure that the size matches the number of triangles
00134   unsigned int num_triangles = *(reinterpret_cast<uint32_t *>(buffer + 80));
00135   static const size_t number_of_bytes_per_triangle = 50;
00136   size_t expected_size = binary_stl_header_len + num_triangles * number_of_bytes_per_triangle;
00137   if (num_bytes < expected_size)
00138   {
00139     ROS_ERROR_STREAM("The STL file '" << origin << "' is malformed. According "
00140                      "to the binary STL header it should have '" <<
00141                      num_triangles << "' triangles, but it has too little" <<
00142                      " data for that to be the case.");
00143     return false;
00144   }
00145   else if (num_bytes > expected_size)
00146   {
00147     ROS_WARN_STREAM("The STL file '" << origin << "' is malformed. According "
00148                     "to the binary STL header it should have '" <<
00149                     num_triangles << "' triangles, but it has too much" <<
00150                     " data for that to be the case. The extra data will be" <<
00151                     " ignored.");
00152   }
00153 
00154   // load the binary STL data
00155   return this->load_binary(buffer);
00156 }
00157 
00158 bool STLLoader::load_binary(uint8_t* buffer)
00159 {
00160   uint8_t* pos = buffer;
00161 
00162   pos += 80; // skip the 80 byte header
00163 
00164   unsigned int numTriangles = *(unsigned int*)pos;
00165   pos += 4;
00166 
00167   for ( unsigned int currentTriangle = 0; currentTriangle < numTriangles; ++currentTriangle )
00168   {
00169     Triangle tri;
00170 
00171     tri.normal_.x = *(float*)pos;
00172     pos += 4;
00173     tri.normal_.y = *(float*)pos;
00174     pos += 4;
00175     tri.normal_.z = *(float*)pos;
00176     pos += 4;
00177 
00178     tri.vertices_[0].x = *(float*)pos;
00179     pos += 4;
00180     tri.vertices_[0].y = *(float*)pos;
00181     pos += 4;
00182     tri.vertices_[0].z = *(float*)pos;
00183     pos += 4;
00184 
00185     tri.vertices_[1].x = *(float*)pos;
00186     pos += 4;
00187     tri.vertices_[1].y = *(float*)pos;
00188     pos += 4;
00189     tri.vertices_[1].z = *(float*)pos;
00190     pos += 4;
00191 
00192     tri.vertices_[2].x = *(float*)pos;
00193     pos += 4;
00194     tri.vertices_[2].y = *(float*)pos;
00195     pos += 4;
00196     tri.vertices_[2].z = *(float*)pos;
00197     pos += 4;
00198 
00199     // Blender was writing a large number into this short... am I misinterpreting what the attribute byte count is supposed to do?
00200     //unsigned short attributeByteCount = *(unsigned short*)pos;
00201     pos += 2;
00202 
00203     //pos += attributeByteCount;
00204 
00205     if (tri.normal_.squaredLength() < 0.001)
00206     {
00207       Ogre::Vector3 side1 = tri.vertices_[0] - tri.vertices_[1];
00208       Ogre::Vector3 side2 = tri.vertices_[1] - tri.vertices_[2];
00209       tri.normal_ = side1.crossProduct(side2);
00210     }
00211     tri.normal_.normalise();
00212 
00213     triangles_.push_back(tri);
00214   }
00215 
00216   return true;
00217 }
00218 
00219 void calculateUV(const Ogre::Vector3& vec, float& u, float& v)
00220 {
00221   Ogre::Vector3 pos(vec);
00222   pos.normalise();
00223   u = acos( pos.y / pos.length() );
00224 
00225   float val = pos.x / ( sin( u ) );
00226   v = acos( val );
00227 
00228   u /= Ogre::Math::PI;
00229   v /= Ogre::Math::PI;
00230 }
00231 
00232 
00233 Ogre::MeshPtr STLLoader::toMesh(const std::string& name)
00234 {
00235   Ogre::ManualObject* object = new Ogre::ManualObject( "the one and only" );
00236   object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST );
00237 
00238   unsigned int vertexCount = 0;
00239   V_Triangle::const_iterator it = triangles_.begin();
00240   V_Triangle::const_iterator end = triangles_.end();
00241   for (; it != end; ++it )
00242   {
00243     if( vertexCount >= 2004 )
00244     {
00245       // Subdivide large meshes into submeshes with at most 2004
00246       // vertices to prevent problems on some graphics cards.
00247       object->end();
00248       object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST );
00249       vertexCount = 0;
00250     }
00251 
00252     const STLLoader::Triangle& tri = *it;
00253 
00254     float u, v;
00255     u = v = 0.0f;
00256     object->position( tri.vertices_[0] );
00257     object->normal( tri.normal_);
00258     calculateUV( tri.vertices_[0], u, v );
00259     object->textureCoord( u, v );
00260 
00261     object->position( tri.vertices_[1] );
00262     object->normal( tri.normal_);
00263     calculateUV( tri.vertices_[1], u, v );
00264     object->textureCoord( u, v );
00265 
00266     object->position( tri.vertices_[2] );
00267     object->normal( tri.normal_);
00268     calculateUV( tri.vertices_[2], u, v );
00269     object->textureCoord( u, v );
00270 
00271     object->triangle( vertexCount + 0, vertexCount + 1, vertexCount + 2 );
00272 
00273     vertexCount += 3;
00274   }
00275 
00276   object->end();
00277 
00278   Ogre::MeshPtr mesh = object->convertToMesh( name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
00279   mesh->buildEdgeList();
00280 
00281   delete object;
00282 
00283   return mesh;
00284 }
00285 
00286 }


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Jun 6 2019 18:02:16