$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include <cstdio> 00036 #include <cmath> 00037 #include <algorithm> 00038 #include <set> 00039 #include "pr2_navigation_self_filter/shapes.h" 00040 00041 // \author Ioan Sucan ; based on stl_to_mesh 00042 00043 namespace shapes 00044 { 00045 00046 namespace detail 00047 { 00048 struct myVertex 00049 { 00050 tf::Vector3 point; 00051 unsigned int index; 00052 }; 00053 00054 struct ltVertexValue 00055 { 00056 bool operator()(const myVertex &p1, const myVertex &p2) const 00057 { 00058 const tf::Vector3 &v1 = p1.point; 00059 const tf::Vector3 &v2 = p2.point; 00060 if (v1.getX() < v2.getX()) 00061 return true; 00062 if (v1.getX() > v2.getX()) 00063 return false; 00064 if (v1.getY() < v2.getY()) 00065 return true; 00066 if (v1.getY() > v2.getY()) 00067 return false; 00068 if (v1.getZ() < v2.getZ()) 00069 return true; 00070 return false; 00071 } 00072 }; 00073 00074 struct ltVertexIndex 00075 { 00076 bool operator()(const myVertex &p1, const myVertex &p2) const 00077 { 00078 return p1.index < p2.index; 00079 } 00080 }; 00081 } 00082 00083 shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &vertices, const std::vector<unsigned int> &triangles) 00084 { 00085 unsigned int nt = triangles.size() / 3; 00086 shapes::Mesh *mesh = new shapes::Mesh(vertices.size(), nt); 00087 for (unsigned int i = 0 ; i < vertices.size() ; ++i) 00088 { 00089 mesh->vertices[3 * i ] = vertices[i].getX(); 00090 mesh->vertices[3 * i + 1] = vertices[i].getY(); 00091 mesh->vertices[3 * i + 2] = vertices[i].getZ(); 00092 } 00093 00094 std::copy(triangles.begin(), triangles.end(), mesh->triangles); 00095 00096 // compute normals 00097 for (unsigned int i = 0 ; i < nt ; ++i) 00098 { 00099 tf::Vector3 s1 = vertices[triangles[i * 3 ]] - vertices[triangles[i * 3 + 1]]; 00100 tf::Vector3 s2 = vertices[triangles[i * 3 + 1]] - vertices[triangles[i * 3 + 2]]; 00101 tf::Vector3 normal = s1.cross(s2); 00102 normal.normalize(); 00103 mesh->normals[3 * i ] = normal.getX(); 00104 mesh->normals[3 * i + 1] = normal.getY(); 00105 mesh->normals[3 * i + 2] = normal.getZ(); 00106 } 00107 return mesh; 00108 } 00109 00110 shapes::Mesh* createMeshFromVertices(const std::vector<tf::Vector3> &source) 00111 { 00112 if (source.size() < 3) 00113 return NULL; 00114 00115 std::set<detail::myVertex, detail::ltVertexValue> vertices; 00116 std::vector<unsigned int> triangles; 00117 00118 for (unsigned int i = 0 ; i < source.size() / 3 ; ++i) 00119 { 00120 // check if we have new vertices 00121 detail::myVertex vt; 00122 00123 vt.point = source[3 * i]; 00124 std::set<detail::myVertex, detail::ltVertexValue>::iterator p1 = vertices.find(vt); 00125 if (p1 == vertices.end()) 00126 { 00127 vt.index = vertices.size(); 00128 vertices.insert(vt); 00129 } 00130 else 00131 vt.index = p1->index; 00132 triangles.push_back(vt.index); 00133 00134 vt.point = source[3 * i + 1]; 00135 std::set<detail::myVertex, detail::ltVertexValue>::iterator p2 = vertices.find(vt); 00136 if (p2 == vertices.end()) 00137 { 00138 vt.index = vertices.size(); 00139 vertices.insert(vt); 00140 } 00141 else 00142 vt.index = p2->index; 00143 triangles.push_back(vt.index); 00144 00145 vt.point = source[3 * i + 2]; 00146 std::set<detail::myVertex, detail::ltVertexValue>::iterator p3 = vertices.find(vt); 00147 if (p3 == vertices.end()) 00148 { 00149 vt.index = vertices.size(); 00150 vertices.insert(vt); 00151 } 00152 else 00153 vt.index = p3->index; 00154 00155 triangles.push_back(vt.index); 00156 } 00157 00158 // sort our vertices 00159 std::vector<detail::myVertex> vt; 00160 vt.insert(vt.begin(), vertices.begin(), vertices.end()); 00161 std::sort(vt.begin(), vt.end(), detail::ltVertexIndex()); 00162 00163 // copy the data to a mesh structure 00164 unsigned int nt = triangles.size() / 3; 00165 00166 shapes::Mesh *mesh = new shapes::Mesh(vt.size(), nt); 00167 for (unsigned int i = 0 ; i < vt.size() ; ++i) 00168 { 00169 mesh->vertices[3 * i ] = vt[i].point.getX(); 00170 mesh->vertices[3 * i + 1] = vt[i].point.getY(); 00171 mesh->vertices[3 * i + 2] = vt[i].point.getZ(); 00172 } 00173 00174 std::copy(triangles.begin(), triangles.end(), mesh->triangles); 00175 00176 // compute normals 00177 for (unsigned int i = 0 ; i < nt ; ++i) 00178 { 00179 tf::Vector3 s1 = vt[triangles[i * 3 ]].point - vt[triangles[i * 3 + 1]].point; 00180 tf::Vector3 s2 = vt[triangles[i * 3 + 1]].point - vt[triangles[i * 3 + 2]].point; 00181 tf::Vector3 normal = s1.cross(s2); 00182 normal.normalize(); 00183 mesh->normals[3 * i ] = normal.getX(); 00184 mesh->normals[3 * i + 1] = normal.getY(); 00185 mesh->normals[3 * i + 2] = normal.getZ(); 00186 } 00187 00188 return mesh; 00189 } 00190 00191 shapes::Mesh* createMeshFromBinaryStlData(const char *data, unsigned int size) 00192 { 00193 const char* pos = data; 00194 pos += 80; // skip the 80 byte header 00195 00196 unsigned int numTriangles = *(unsigned int*)pos; 00197 pos += 4; 00198 00199 // make sure we have read enough data 00200 if ((long)(50 * numTriangles + 84) <= size) 00201 { 00202 std::vector<tf::Vector3> vertices; 00203 00204 for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle) 00205 { 00206 // skip the normal 00207 pos += 12; 00208 00209 // read vertices 00210 tf::Vector3 v1(0,0,0); 00211 tf::Vector3 v2(0,0,0); 00212 tf::Vector3 v3(0,0,0); 00213 00214 v1.setX(*(float*)pos); 00215 pos += 4; 00216 v1.setY(*(float*)pos); 00217 pos += 4; 00218 v1.setZ(*(float*)pos); 00219 pos += 4; 00220 00221 v2.setX(*(float*)pos); 00222 pos += 4; 00223 v2.setY(*(float*)pos); 00224 pos += 4; 00225 v2.setZ(*(float*)pos); 00226 pos += 4; 00227 00228 v3.setX(*(float*)pos); 00229 pos += 4; 00230 v3.setY(*(float*)pos); 00231 pos += 4; 00232 v3.setZ(*(float*)pos); 00233 pos += 4; 00234 00235 // skip attribute 00236 pos += 2; 00237 00238 vertices.push_back(v1); 00239 vertices.push_back(v2); 00240 vertices.push_back(v3); 00241 } 00242 00243 return createMeshFromVertices(vertices); 00244 } 00245 00246 return NULL; 00247 } 00248 00249 shapes::Mesh* createMeshFromBinaryStl(const char *filename) 00250 { 00251 FILE* input = fopen(filename, "r"); 00252 if (!input) 00253 return NULL; 00254 00255 fseek(input, 0, SEEK_END); 00256 long fileSize = ftell(input); 00257 fseek(input, 0, SEEK_SET); 00258 00259 char* buffer = new char[fileSize]; 00260 size_t rd = fread(buffer, fileSize, 1, input); 00261 00262 fclose(input); 00263 00264 shapes::Mesh *result = NULL; 00265 00266 if (rd == 1) 00267 result = createMeshFromBinaryStlData(buffer, fileSize); 00268 00269 delete[] buffer; 00270 00271 return result; 00272 } 00273 00274 }