40 #include <console_bridge/console.h>
102 Sphere::Sphere(
double r) : Shape()
105 throw std::runtime_error(
"Sphere radius must be non-negative.");
110 Cylinder::Cylinder() : Shape()
113 length = radius = 0.0;
116 Cylinder::Cylinder(
double r,
double l) : Shape()
119 throw std::runtime_error(
"Cylinder dimensions must be non-negative.");
125 Cone::Cone() :
Shape()
128 length = radius = 0.0;
134 throw std::runtime_error(
"Cone dimensions must be non-negative.");
143 size[0] = size[1] = size[2] = 0.0;
148 if (
x < 0 ||
y < 0 ||
z < 0)
149 throw std::runtime_error(
"Box dimensions must be non-negative.");
163 triangle_normals =
nullptr;
164 vertex_normals =
nullptr;
170 vertex_count = v_count;
171 vertices =
new double[v_count * 3];
172 triangle_count = t_count;
173 triangles =
new unsigned int[t_count * 3];
174 triangle_normals =
new double[t_count * 3];
175 vertex_normals =
new double[v_count * 3];
196 Plane::Plane(
double pa,
double pb,
double pc,
double pd) : Shape()
239 for (
unsigned int i = 0; i < n; ++i)
242 for (
unsigned int i = 0; i < n; ++i)
249 n = 3 * triangle_count;
250 for (
unsigned int i = 0; i < n; ++i)
252 if (triangle_normals)
253 for (
unsigned int i = 0; i < n; ++i)
263 Plane* Plane::clone()
const
265 return new Plane(a, b, c,
d);
268 OcTree* OcTree::clone()
const
270 return new OcTree(octree);
273 void OcTree::scaleAndPadd(
double ,
double )
275 CONSOLE_BRIDGE_logWarn(
"OcTrees cannot be scaled or padded");
278 void Plane::scaleAndPadd(
double ,
double )
280 CONSOLE_BRIDGE_logWarn(
"Planes cannot be scaled or padded");
283 void Shape::scale(
double scale)
285 scaleAndPadd(scale, 0.0);
288 void Shape::padd(
double padding)
290 scaleAndPadd(1.0, padding);
293 void Sphere::scaleAndPadd(
double scale,
double padding)
295 const auto tmpRadius = radius * scale + padding;
297 throw std::runtime_error(
"Sphere radius must be non-negative.");
301 void Cylinder::scaleAndPadd(
double scaleRadius,
double scaleLength,
double paddRadius,
double paddLength)
303 const auto tmpRadius = radius * scaleRadius + paddRadius;
304 const auto tmpLength = length * scaleLength + 2.0 * paddLength;
305 if (tmpRadius < 0 || tmpLength < 0)
306 throw std::runtime_error(
"Cylinder dimensions must be non-negative.");
311 void Cylinder::scale(
double scaleRadius,
double scaleLength)
313 scaleAndPadd(scaleRadius, scaleLength, 0.0, 0.0);
316 void Cylinder::padd(
double paddRadius,
double paddLength)
318 scaleAndPadd(1.0, 1.0, paddRadius, paddLength);
321 void Cylinder::scaleAndPadd(
double scale,
double padd)
323 scaleAndPadd(scale, scale, padd, padd);
326 void Cone::scaleAndPadd(
double scaleRadius,
double scaleLength,
double paddRadius,
double paddLength)
328 const auto tmpRadius = radius * scaleRadius + paddRadius;
329 const auto tmpLength = length * scaleLength + 2.0 * paddLength;
330 if (tmpRadius < 0 || tmpLength < 0)
331 throw std::runtime_error(
"Cone dimensions must be non-negative.");
336 void Cone::scale(
double scaleRadius,
double scaleLength)
338 scaleAndPadd(scaleRadius, scaleLength, 0.0, 0.0);
341 void Cone::padd(
double paddRadius,
double paddLength)
343 scaleAndPadd(1.0, 1.0, paddRadius, paddLength);
346 void Cone::scaleAndPadd(
double scale,
double padd)
348 scaleAndPadd(scale, scale, padd, padd);
351 void Box::scaleAndPadd(
double scaleX,
double scaleY,
double scaleZ,
double paddX,
double paddY,
double paddZ)
353 const auto tmpSize0 = size[0] * scaleX + paddX * 2.0;
354 const auto tmpSize1 = size[1] * scaleY + paddY * 2.0;
355 const auto tmpSize2 = size[2] * scaleZ + paddZ * 2.0;
356 if (tmpSize0 < 0 || tmpSize1 < 0 || tmpSize2 < 0)
357 throw std::runtime_error(
"Box dimensions must be non-negative.");
363 void Box::scale(
double scaleX,
double scaleY,
double scaleZ)
365 scaleAndPadd(scaleX, scaleY, scaleZ, 0.0, 0.0, 0.0);
368 void Box::padd(
double paddX,
double paddY,
double paddZ)
370 scaleAndPadd(1.0, 1.0, 1.0, paddX, paddY, paddZ);
373 void Box::scaleAndPadd(
double scale,
double padd)
375 scaleAndPadd(scale, scale, scale, padd, padd, padd);
378 void Mesh::scaleAndPadd(
double scaleX,
double scaleY,
double scaleZ,
double paddX,
double paddY,
double paddZ)
381 double sx = 0.0, sy = 0.0, sz = 0.0;
382 for (
unsigned int i = 0; i < vertex_count; ++i)
384 unsigned int i3 = i * 3;
386 sy += vertices[i3 + 1];
387 sz += vertices[i3 + 2];
389 sx /= (double)vertex_count;
390 sy /= (double)vertex_count;
391 sz /= (double)vertex_count;
394 for (
unsigned int i = 0; i < vertex_count; ++i)
396 unsigned int i3 = i * 3;
399 double dx = vertices[i3] - sx;
400 double dy = vertices[i3 + 1] - sy;
401 double dz = vertices[i3 + 2] - sz;
404 double norm = sqrt(dx * dx + dy * dy + dz * dz);
407 vertices[i3] = sx + dx * (scaleX + paddX / norm);
408 vertices[i3 + 1] = sy + dy * (scaleY + paddY / norm);
409 vertices[i3 + 2] = sz + dz * (scaleZ + paddZ / norm);
413 double ndx = ((dx > 0) ? dx + paddX : dx - paddX);
414 double ndy = ((dy > 0) ? dy + paddY : dy - paddY);
415 double ndz = ((dz > 0) ? dz + paddZ : dz - paddZ);
416 vertices[i3] = sx + ndx;
417 vertices[i3 + 1] = sy + ndy;
418 vertices[i3 + 2] = sz + ndz;
423 void Mesh::scale(
double scaleX,
double scaleY,
double scaleZ)
425 scaleAndPadd(scaleX, scaleY, scaleZ, 0.0, 0.0, 0.0);
428 void Mesh::padd(
double paddX,
double paddY,
double paddZ)
430 scaleAndPadd(1.0, 1.0, 1.0, paddX, paddY, paddZ);
433 void Mesh::scaleAndPadd(
double scale,
double padd)
435 scaleAndPadd(scale, scale, scale, padd, padd, padd);
438 void Shape::print(std::ostream& out)
const
440 out <<
this << std::endl;
443 void Sphere::print(std::ostream& out)
const
445 out <<
"Sphere[radius=" << radius <<
"]" << std::endl;
448 void Cylinder::print(std::ostream& out)
const
450 out <<
"Cylinder[radius=" << radius <<
", length=" << length <<
"]" << std::endl;
453 void Cone::print(std::ostream& out)
const
455 out <<
"Cone[radius=" << radius <<
", length=" << length <<
"]" << std::endl;
458 void Box::print(std::ostream& out)
const
460 out <<
"Box[x=length=" << size[0] <<
", y=width=" << size[1] <<
"z=height=" << size[2] <<
"]" << std::endl;
463 void Mesh::print(std::ostream& out)
const
465 out <<
"Mesh[vertices=" << vertex_count <<
", triangles=" << triangle_count <<
"]" << std::endl;
468 void Plane::print(std::ostream& out)
const
470 out <<
"Plane[a=" << a <<
", b=" << b <<
", c=" << c <<
", d=" <<
d <<
"]" << std::endl;
473 void OcTree::print(std::ostream& out)
const
477 double minx, miny, minz, maxx, maxy, maxz;
478 octree->getMetricMin(minx, miny, minz);
479 octree->getMetricMax(maxx, maxy, maxz);
480 out <<
"OcTree[depth = " << octree->getTreeDepth() <<
", resolution = " << octree->getResolution()
481 <<
" inside box (minx=" << minx <<
", miny=" << miny <<
", minz=" << minz <<
", maxx=" << maxx
482 <<
", maxy=" << maxy <<
", maxz=" << maxz <<
")]" << std::endl;
485 out <<
"OcTree[NULL]" << std::endl;
488 bool Shape::isFixed()
const
493 bool OcTree::isFixed()
const
498 bool Plane::isFixed()
const
503 void Mesh::computeTriangleNormals()
505 if (triangle_count && !triangle_normals)
506 triangle_normals =
new double[triangle_count * 3];
509 for (
unsigned int i = 0; i < triangle_count; ++i)
511 unsigned int i3 = i * 3;
512 Eigen::Vector3d s1(vertices[triangles[i3] * 3] - vertices[triangles[i3 + 1] * 3],
513 vertices[triangles[i3] * 3 + 1] - vertices[triangles[i3 + 1] * 3 + 1],
514 vertices[triangles[i3] * 3 + 2] - vertices[triangles[i3 + 1] * 3 + 2]);
515 Eigen::Vector3d s2(vertices[triangles[i3 + 1] * 3] - vertices[triangles[i3 + 2] * 3],
516 vertices[triangles[i3 + 1] * 3 + 1] - vertices[triangles[i3 + 2] * 3 + 1],
517 vertices[triangles[i3 + 1] * 3 + 2] - vertices[triangles[i3 + 2] * 3 + 2]);
518 Eigen::Vector3d normal = s1.cross(s2);
520 triangle_normals[i3] = normal.x();
521 triangle_normals[i3 + 1] = normal.y();
522 triangle_normals[i3 + 2] = normal.z();
526 void Mesh::computeVertexNormals()
528 if (!triangle_normals)
529 computeTriangleNormals();
530 if (vertex_count && !vertex_normals)
531 vertex_normals =
new double[vertex_count * 3];
534 for (
unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx)
536 unsigned int tIdx3 = 3 * tIdx;
537 unsigned int tIdx3_1 = tIdx3 + 1;
538 unsigned int tIdx3_2 = tIdx3 + 2;
540 unsigned int v1 = triangles[tIdx3];
541 unsigned int v2 = triangles[tIdx3_1];
542 unsigned int v3 = triangles[tIdx3_2];
544 avg_normals[v1][0] += triangle_normals[tIdx3];
545 avg_normals[v1][1] += triangle_normals[tIdx3_1];
546 avg_normals[v1][2] += triangle_normals[tIdx3_2];
548 avg_normals[v2][0] += triangle_normals[tIdx3];
549 avg_normals[v2][1] += triangle_normals[tIdx3_1];
550 avg_normals[v2][2] += triangle_normals[tIdx3_2];
552 avg_normals[v3][0] += triangle_normals[tIdx3];
553 avg_normals[v3][1] += triangle_normals[tIdx3_1];
554 avg_normals[v3][2] += triangle_normals[tIdx3_2];
556 for (std::size_t i = 0; i < avg_normals.size(); ++i)
558 if (avg_normals[i].squaredNorm() > 0.0)
559 avg_normals[i].normalize();
560 unsigned int i3 = i * 3;
561 vertex_normals[i3] = avg_normals[i][0];
562 vertex_normals[i3 + 1] = avg_normals[i][1];
563 vertex_normals[i3 + 2] = avg_normals[i][2];
567 void Mesh::mergeVertices(
double threshold)
569 const double thresholdSQR = threshold * threshold;
571 std::vector<unsigned int> vertex_map(vertex_count);
575 for (
unsigned int vIdx = 0; vIdx < vertex_count; ++vIdx)
577 orig_vertices[vIdx][0] = vertices[3 * vIdx];
578 orig_vertices[vIdx][1] = vertices[3 * vIdx + 1];
579 orig_vertices[vIdx][2] = vertices[3 * vIdx + 2];
580 vertex_map[vIdx] = vIdx;
583 for (
unsigned int vIdx1 = 0; vIdx1 < vertex_count; ++vIdx1)
585 if (vertex_map[vIdx1] != vIdx1)
588 vertex_map[vIdx1] = compressed_vertices.size();
589 compressed_vertices.push_back(orig_vertices[vIdx1]);
591 for (
unsigned int vIdx2 = vIdx1 + 1; vIdx2 < vertex_count; ++vIdx2)
593 double distanceSQR = (orig_vertices[vIdx1] - orig_vertices[vIdx2]).squaredNorm();
595 vertex_map[vIdx2] = vertex_map[vIdx1];
599 if (compressed_vertices.size() == orig_vertices.size())
603 for (
unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx)
605 unsigned int i3 = 3 * tIdx;
606 triangles[i3] = vertex_map[triangles[i3]];
607 triangles[i3 + 1] = vertex_map[triangles[i3 + 1]];
608 triangles[i3 + 2] = vertex_map[triangles[i3 + 2]];
611 vertex_count = compressed_vertices.size();
613 vertices =
new double[vertex_count * 3];
615 for (
unsigned int vIdx = 0; vIdx < vertex_count; ++vIdx)
617 unsigned int i3 = 3 * vIdx;
618 vertices[i3] = compressed_vertices[vIdx][0];
619 vertices[i3 + 1] = compressed_vertices[vIdx][1];
620 vertices[i3 + 2] = compressed_vertices[vIdx][2];
623 if (triangle_normals)
624 computeTriangleNormals();
626 computeVertexNormals();