165 triangles =
new unsigned int[t_count * 3];
209 return new Sphere(radius);
214 return new Cylinder(radius, length);
219 return new Cone(radius, length);
224 return new Box(size[0], size[1], size[2]);
229 Mesh* dest =
new Mesh(vertex_count, triangle_count);
230 unsigned int n = 3 * vertex_count;
231 for (
unsigned int i = 0; i < n; ++i)
234 for (
unsigned int i = 0; i < n; ++i)
241 n = 3 * triangle_count;
242 for (
unsigned int i = 0; i < n; ++i)
243 dest->triangles[i] = triangles[i];
244 if (triangle_normals)
245 for (
unsigned int i = 0; i < n; ++i)
246 dest->triangle_normals[i] = triangle_normals[i];
249 delete[] dest->triangle_normals;
250 dest->triangle_normals = NULL;
257 return new Plane(a, b, c,
d);
287 radius = radius * scale + padding;
292 radius = radius * scale + padding;
293 length = length * scale + 2.0 * padding;
298 radius = radius * scale + padding;
299 length = length * scale + 2.0 * padding;
304 double p2 = padding * 2.0;
305 size[0] = size[0] * scale + p2;
306 size[1] = size[1] * scale + p2;
307 size[2] = size[2] * scale + p2;
313 double sx = 0.0, sy = 0.0, sz = 0.0;
314 for (
unsigned int i = 0; i < vertex_count; ++i)
316 unsigned int i3 = i * 3;
318 sy += vertices[i3 + 1];
319 sz += vertices[i3 + 2];
321 sx /= (double)vertex_count;
322 sy /= (double)vertex_count;
323 sz /= (double)vertex_count;
326 for (
unsigned int i = 0; i < vertex_count; ++i)
328 unsigned int i3 = i * 3;
331 double dx = vertices[i3] - sx;
332 double dy = vertices[i3 + 1] - sy;
333 double dz = vertices[i3 + 2] - sz;
336 double norm = sqrt(dx * dx + dy * dy + dz * dz);
339 double fact = scale + padding / norm;
340 vertices[i3] = sx + dx * fact;
341 vertices[i3 + 1] = sy + dy * fact;
342 vertices[i3 + 2] = sz + dz * fact;
346 double ndx = ((dx > 0) ? dx + padding : dx - padding);
347 double ndy = ((dy > 0) ? dy + padding : dy - padding);
348 double ndz = ((dz > 0) ? dz + padding : dz - padding);
349 vertices[i3] = sx + ndx;
350 vertices[i3 + 1] = sy + ndy;
351 vertices[i3 + 2] = sz + ndz;
358 out <<
this << std::endl;
363 out <<
"Sphere[radius=" << radius <<
"]" << std::endl;
368 out <<
"Cylinder[radius=" << radius <<
", length=" << length <<
"]" << std::endl;
373 out <<
"Cone[radius=" << radius <<
", length=" << length <<
"]" << std::endl;
378 out <<
"Box[x=length=" << size[0] <<
", y=width=" << size[1] <<
"z=height=" << size[2] <<
"]" << std::endl;
383 out <<
"Mesh[vertices=" << vertex_count <<
", triangles=" << triangle_count <<
"]" << std::endl;
388 out <<
"Plane[a=" << a <<
", b=" << b <<
", c=" << c <<
", d=" <<
d <<
"]" << std::endl;
395 double minx, miny, minz, maxx, maxy, maxz;
396 octree->getMetricMin(minx, miny, minz);
397 octree->getMetricMax(maxx, maxy, maxz);
398 out <<
"OcTree[depth = " <<
octree->getTreeDepth() <<
", resolution = " <<
octree->getResolution()
399 <<
" inside box (minx=" << minx <<
", miny=" << miny <<
", minz=" << minz <<
", maxx=" << maxx
400 <<
", maxy=" << maxy <<
", maxz=" << maxz <<
")]" << std::endl;
403 out <<
"OcTree[NULL]" << std::endl;
423 if (triangle_count && !triangle_normals)
424 triangle_normals =
new double[triangle_count * 3];
427 for (
unsigned int i = 0; i < triangle_count; ++i)
429 unsigned int i3 = i * 3;
430 Eigen::Vector3d s1(vertices[triangles[i3] * 3] - vertices[triangles[i3 + 1] * 3],
431 vertices[triangles[i3] * 3 + 1] - vertices[triangles[i3 + 1] * 3 + 1],
432 vertices[triangles[i3] * 3 + 2] - vertices[triangles[i3 + 1] * 3 + 2]);
433 Eigen::Vector3d s2(vertices[triangles[i3 + 1] * 3] - vertices[triangles[i3 + 2] * 3],
434 vertices[triangles[i3 + 1] * 3 + 1] - vertices[triangles[i3 + 2] * 3 + 1],
435 vertices[triangles[i3 + 1] * 3 + 2] - vertices[triangles[i3 + 2] * 3 + 2]);
436 Eigen::Vector3d normal = s1.cross(s2);
438 triangle_normals[i3] = normal.x();
439 triangle_normals[i3 + 1] = normal.y();
440 triangle_normals[i3 + 2] = normal.z();
446 if (!triangle_normals)
447 computeTriangleNormals();
448 if (vertex_count && !vertex_normals)
449 vertex_normals =
new double[vertex_count * 3];
452 for (
unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx)
454 unsigned int tIdx3 = 3 * tIdx;
455 unsigned int tIdx3_1 = tIdx3 + 1;
456 unsigned int tIdx3_2 = tIdx3 + 2;
458 unsigned int v1 = triangles[tIdx3];
459 unsigned int v2 = triangles[tIdx3_1];
460 unsigned int v3 = triangles[tIdx3_2];
462 avg_normals[v1][0] += triangle_normals[tIdx3];
463 avg_normals[v1][1] += triangle_normals[tIdx3_1];
464 avg_normals[v1][2] += triangle_normals[tIdx3_2];
466 avg_normals[v2][0] += triangle_normals[tIdx3];
467 avg_normals[v2][1] += triangle_normals[tIdx3_1];
468 avg_normals[v2][2] += triangle_normals[tIdx3_2];
470 avg_normals[v3][0] += triangle_normals[tIdx3];
471 avg_normals[v3][1] += triangle_normals[tIdx3_1];
472 avg_normals[v3][2] += triangle_normals[tIdx3_2];
474 for (std::size_t i = 0; i < avg_normals.size(); ++i)
476 if (avg_normals[i].squaredNorm() > 0.0)
477 avg_normals[i].normalize();
478 unsigned int i3 = i * 3;
479 vertex_normals[i3] = avg_normals[i][0];
480 vertex_normals[i3 + 1] = avg_normals[i][1];
481 vertex_normals[i3 + 2] = avg_normals[i][2];
487 const double thresholdSQR = threshold * threshold;
489 std::vector<unsigned int> vertex_map(vertex_count);
493 for (
unsigned int vIdx = 0; vIdx < vertex_count; ++vIdx)
495 orig_vertices[vIdx][0] = vertices[3 * vIdx];
496 orig_vertices[vIdx][1] = vertices[3 * vIdx + 1];
497 orig_vertices[vIdx][2] = vertices[3 * vIdx + 2];
498 vertex_map[vIdx] = vIdx;
501 for (
unsigned int vIdx1 = 0; vIdx1 < vertex_count; ++vIdx1)
503 if (vertex_map[vIdx1] != vIdx1)
506 vertex_map[vIdx1] = compressed_vertices.size();
507 compressed_vertices.push_back(orig_vertices[vIdx1]);
509 for (
unsigned int vIdx2 = vIdx1 + 1; vIdx2 < vertex_count; ++vIdx2)
511 double distanceSQR = (orig_vertices[vIdx1] - orig_vertices[vIdx2]).squaredNorm();
512 if (distanceSQR <= thresholdSQR)
513 vertex_map[vIdx2] = vertex_map[vIdx1];
517 if (compressed_vertices.size() == orig_vertices.size())
521 for (
unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx)
523 unsigned int i3 = 3 * tIdx;
524 triangles[i3] = vertex_map[triangles[i3]];
525 triangles[i3 + 1] = vertex_map[triangles[i3 + 1]];
526 triangles[i3 + 2] = vertex_map[triangles[i3 + 2]];
529 vertex_count = compressed_vertices.size();
531 vertices =
new double[vertex_count * 3];
533 for (
unsigned int vIdx = 0; vIdx < vertex_count; ++vIdx)
535 unsigned int i3 = 3 * vIdx;
536 vertices[i3] = compressed_vertices[vIdx][0];
537 vertices[i3 + 1] = compressed_vertices[vIdx][1];
538 vertices[i3 + 2] = compressed_vertices[vIdx][2];
541 if (triangle_normals)
542 computeTriangleNormals();
544 computeVertexNormals();
#define CONSOLE_BRIDGE_logWarn(fmt,...)
Definition of various shapes. No properties such as position are included. These are simply the descr...
double radius
The radius of the cylinder.
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
void scale(double scale)
Scale this shape by a factor.
void mergeVertices(double threshold)
Merge vertices that are very close to each other, up to a threshold.
virtual Shape * clone() const
Create a copy of this shape.
virtual bool isFixed() const
Return a flag indicating whether this shape can be scaled and/or padded.
Definition of a cylinder Length is along z axis. Origin is at center of mass.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
virtual Shape * clone() const
Create a copy of this shape.
double length
The length (height) of the cone.
double size[3]
x, y, z dimensions of the box (axis-aligned)
ShapeType type
The type of the shape.
virtual Shape * clone() const
Create a copy of this shape.
static double distanceSQR(const Eigen::Vector3d &p, const Eigen::Vector3d &origin, const Eigen::Vector3d &dir)
Compute the square of the distance between a ray and a point Note: this requires 'dir' to be normaliz...
double radius
The radius of the cone.
static const std::string STRING_NAME
The type of the shape, as a string.
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
double length
The length of the cylinder.
virtual void scaleAndPadd(double scale, double padd)
Scale and padd this shape.
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
virtual void scaleAndPadd(double scale, double padd)
Scale and padd this shape.
Definition of a plane with equation ax + by + cz + d = 0.
double a
The plane equation is ax + by + cz + d = 0.
double * vertex_normals
The normal to each vertex; unit vector represented as (x,y,z); If missing from the mesh...
std::ostream & operator<<(std::ostream &ss, ShapeType type)
std::shared_ptr< const octomap::OcTree > octree
Definition of a cone Tip is on positive z axis. Center of base is on negative z axis. Origin is halway between tip and center of base.
unsigned int vertex_count
The number of available vertices.
virtual void scaleAndPadd(double scale, double padd)
Scale and padd this shape.
void padd(double padding)
Add padding to this shape.
A basic definition of a shape. Shapes are considered centered at origin.
unsigned int triangle_count
The number of triangles formed with the vertices.
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
virtual bool isFixed() const
Return a flag indicating whether this shape can be scaled and/or padded.
ShapeType
A list of known shape types.
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
virtual void scaleAndPadd(double scale, double padd)
Scale and padd this shape.
double * triangle_normals
The normal to each triangle; unit vector represented as (x,y,z); If missing from the mesh...
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
void computeVertexNormals()
Compute vertex normals by averaging from adjacent triangle normals.
virtual void scaleAndPadd(double scale, double padd)
Scale and padd this shape.
virtual Shape * clone() const
Create a copy of this shape.
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
void computeTriangleNormals()
Compute the normals of each triangle from its vertices via cross product.
static const std::string STRING_NAME
The type of the shape, as a string.
static const std::string STRING_NAME
The type of the shape, as a string.
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin.
Definition of a box Aligned with the XYZ axes.
virtual bool isFixed() const
Return a flag indicating whether this shape can be scaled and/or padded.
virtual Shape * clone() const
Create a copy of this shape.
static const std::string STRING_NAME
The type of the shape, as a string.
double radius
The radius of the sphere.
virtual void scaleAndPadd(double scale, double padd)
Scale and padd this shape.
static const std::string STRING_NAME
The type of the shape, as a string.
virtual void print(std::ostream &out=std::cout) const
Print information about this shape.
static const std::string STRING_NAME
The type of the shape, as a string.
virtual Shape * clone() const
Create a copy of this shape.
virtual Shape * clone() const
Create a copy of this shape.
virtual void scaleAndPadd(double scale, double padd)
Scale and padd this shape.
static const std::string STRING_NAME
The type of the shape, as a string.