$search
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 <OGRE/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 long fileSize = ftell( input ); 00074 fseek( input, 0, SEEK_SET ); 00075 00076 uint8_t* buffer = new uint8_t[ fileSize ]; 00077 fread( buffer, fileSize, 1, input ); 00078 fclose( input ); 00079 00080 bool success = load(buffer); 00081 delete [] buffer; 00082 00083 return success; 00084 } 00085 00086 bool STLLoader::load(uint8_t* buffer) 00087 { 00088 uint8_t* pos = buffer; 00089 pos += 80; // skip the 80 byte header 00090 00091 unsigned int numTriangles = *(unsigned int*)pos; 00092 pos += 4; 00093 00094 for ( unsigned int currentTriangle = 0; currentTriangle < numTriangles; ++currentTriangle ) 00095 { 00096 Triangle tri; 00097 00098 tri.normal_.x = *(float*)pos; 00099 pos += 4; 00100 tri.normal_.y = *(float*)pos; 00101 pos += 4; 00102 tri.normal_.z = *(float*)pos; 00103 pos += 4; 00104 00105 tri.vertices_[0].x = *(float*)pos; 00106 pos += 4; 00107 tri.vertices_[0].y = *(float*)pos; 00108 pos += 4; 00109 tri.vertices_[0].z = *(float*)pos; 00110 pos += 4; 00111 00112 tri.vertices_[1].x = *(float*)pos; 00113 pos += 4; 00114 tri.vertices_[1].y = *(float*)pos; 00115 pos += 4; 00116 tri.vertices_[1].z = *(float*)pos; 00117 pos += 4; 00118 00119 tri.vertices_[2].x = *(float*)pos; 00120 pos += 4; 00121 tri.vertices_[2].y = *(float*)pos; 00122 pos += 4; 00123 tri.vertices_[2].z = *(float*)pos; 00124 pos += 4; 00125 00126 // Blender was writing a large number into this short... am I misinterpreting what the attribute byte count is supposed to do? 00127 //unsigned short attributeByteCount = *(unsigned short*)pos; 00128 pos += 2; 00129 00130 //pos += attributeByteCount; 00131 00132 if (tri.normal_.squaredLength() < 0.001) 00133 { 00134 Ogre::Vector3 side1 = tri.vertices_[0] - tri.vertices_[1]; 00135 Ogre::Vector3 side2 = tri.vertices_[1] - tri.vertices_[2]; 00136 tri.normal_ = side1.crossProduct(side2); 00137 tri.normal_.normalise(); 00138 } 00139 00140 triangles_.push_back(tri); 00141 } 00142 00143 return true; 00144 } 00145 00146 void calculateUV(const Ogre::Vector3& vec, float& u, float& v) 00147 { 00148 Ogre::Vector3 pos(vec); 00149 pos.normalise(); 00150 u = acos( pos.y / pos.length() ); 00151 00152 float val = pos.x / ( sin( u ) ); 00153 v = acos( val ); 00154 00155 u /= Ogre::Math::PI; 00156 v /= Ogre::Math::PI; 00157 } 00158 00159 00160 Ogre::MeshPtr STLLoader::toMesh(const std::string& name) 00161 { 00162 Ogre::ManualObject* object = new Ogre::ManualObject( "the one and only" ); 00163 object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST ); 00164 00165 unsigned int vertexCount = 0; 00166 V_Triangle::const_iterator it = triangles_.begin(); 00167 V_Triangle::const_iterator end = triangles_.end(); 00168 for (; it != end; ++it ) 00169 { 00170 const STLLoader::Triangle& tri = *it; 00171 00172 float u, v; 00173 u = v = 0.0f; 00174 object->position( tri.vertices_[0] ); 00175 object->normal( tri.normal_); 00176 calculateUV( tri.vertices_[0], u, v ); 00177 object->textureCoord( u, v ); 00178 00179 object->position( tri.vertices_[1] ); 00180 object->normal( tri.normal_); 00181 calculateUV( tri.vertices_[1], u, v ); 00182 object->textureCoord( u, v ); 00183 00184 object->position( tri.vertices_[2] ); 00185 object->normal( tri.normal_); 00186 calculateUV( tri.vertices_[2], u, v ); 00187 object->textureCoord( u, v ); 00188 00189 object->triangle( vertexCount + 0, vertexCount + 1, vertexCount + 2 ); 00190 00191 vertexCount += 3; 00192 } 00193 00194 object->end(); 00195 00196 Ogre::MeshPtr mesh = object->convertToMesh( name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); 00197 mesh->buildEdgeList(); 00198 00199 delete object; 00200 00201 return mesh; 00202 } 00203 00204 }