Go to the documentation of this file.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
00032
00033
00034
00035 #include <cstdio>
00036 #include <cmath>
00037 #include <algorithm>
00038 #include <set>
00039 #include "pr2_navigation_self_filter/shapes.h"
00040
00041
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
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
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
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
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
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;
00195
00196 unsigned int numTriangles = *(unsigned int*)pos;
00197 pos += 4;
00198
00199
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
00207 pos += 12;
00208
00209
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
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 }