Example: math_polyhedron_intersection_example

C++ example source code:

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/math/TObject3D.h>
#include <mrpt/opengl/CAngularObservationMesh.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CPolyhedron.h>

#include <iostream>

using namespace std;
using namespace mrpt;
using namespace mrpt::gui;
using namespace mrpt::opengl;
using namespace mrpt::poses;
using namespace mrpt::math;
using mrpt::opengl::CAngularObservationMesh;
using namespace mrpt::system;

const double GRID_R = 1.0;
const double GRID_G = 1.0;
const double GRID_B = 1.0;

// Comment to increase randomness of poses
#define PAIRED_RANDOM_POSES

#define USE_THREADS

inline double MYRAND1(size_t prec = 64)
{
  return static_cast<double>(rand() % prec) / static_cast<double>(prec - 1);
}

void randomColor(const CRenderizable::Ptr& obj, double alpha)
{
  obj->setColor(MYRAND1(), MYRAND1(), MYRAND1(), alpha);
}

#ifdef USE_THREADS
class PIThreadParam
{
 public:
  const pair<CPolyhedron::Ptr, CPolyhedron::Ptr>* polys{nullptr};
  vector<TSegment3D> intersection;
  PIThreadParam(const pair<CPolyhedron::Ptr, CPolyhedron::Ptr>& p) : polys(&p), intersection() {}
  PIThreadParam() : intersection() {}
  inline static PIThreadParam createObject(const pair<CPolyhedron::Ptr, CPolyhedron::Ptr>& p)
  {
    return PIThreadParam(p);
  }
};

void piThreadFunction(PIThreadParam& p)
{
  vector<TObject3D> ints;
  CPolyhedron::getIntersection(p.polys->first, p.polys->second, ints);
  for (const auto& o : ints)
    if (o.isSegment()) p.intersection.emplace_back(o.getAs<TSegment3D>());
}

inline std::thread piCreateThread(PIThreadParam& p)
{
  return std::thread(&piThreadFunction, std::ref(p));
}

class AggregatorFunctor
{
 public:
  vector<TSegment3D>& sgms;
  AggregatorFunctor(vector<TSegment3D>& s) : sgms(s) {}
  inline void operator()(const PIThreadParam& p)
  {
    sgms.insert(sgms.end(), p.intersection.begin(), p.intersection.end());
  }
};
#endif

CSetOfLines::Ptr getIntersections(const vector<pair<CPolyhedron::Ptr, CPolyhedron::Ptr>>& v)
{
  vector<TSegment3D> sgms;
#ifdef USE_THREADS
  vector<PIThreadParam> pars(v.size());
  vector<std::thread> threads(v.size());
  transform(v.begin(), v.end(), pars.begin(), &PIThreadParam::createObject);
  transform(pars.begin(), pars.end(), threads.begin(), &piCreateThread);
  for_each(threads.begin(), threads.end(), [](std::thread& t) { t.join(); });
  for_each(pars.begin(), pars.end(), AggregatorFunctor(sgms));
#else
  vector<TObject3D> ints, TMP;
  for (vector<pair<CPolyhedron::Ptr, CPolyhedron::Ptr>>::const_iterator it = v.begin();
       it != v.end(); ++it)
  {
    CPolyhedron::getIntersection(it->first, it->second, TMP);
    ints.insert(ints.end(), TMP.begin(), TMP.end());
  }
  TObject3D::getSegments(ints, sgms);
#endif
  CSetOfLines::Ptr lns = CSetOfLines::Create(sgms);
  lns->setLineWidth(9);
  randomColor(lns, 1.0);
  return lns;
}

inline double randomAngle(size_t prec = 64) { return MYRAND1(prec) * M_PI; }
inline double randomZ(double space = 25, size_t prec = 64) { return space * (MYRAND1(prec) - 0.5); }

pair<CPolyhedron::Ptr, CPolyhedron::Ptr> addPairOfPolys(
    CPolyhedron::Ptr p1, CPolyhedron::Ptr p2, CSetOfObjects::Ptr& objs, double x, double y)
{
  p1->makeConvexPolygons();
  p2->makeConvexPolygons();
#ifdef PAIRED_RANDOM_POSES
  CPose3D pose = CPose3D(x, y, randomZ(), randomAngle(), randomAngle(), randomAngle());
  p1->setPose(pose);
  p2->setPose(pose);
#else
  CPose3D pose1 = CPose3D(x, y, randomZ(), randomAngle(), randomAngle(), randomAngle());
  CPose3D pose2 = CPose3D(x, y, randomZ(), randomAngle(), randomAngle(), randomAngle());
  p1->setPose(pose1);
  p2->setPose(pose2);
#endif
  randomColor(p1, 0.5);
  randomColor(p2, 0.5);
  objs->insert(p1);
  objs->insert(p2);
  return make_pair(p1, p2);
}

void display()
{
  CDisplayWindow3D window("Polyhedra Intersection demo", 640, 480);
  window.resize(640, 480);
  Scene::Ptr scene1 = Scene::Create();
  opengl::CGridPlaneXY::Ptr plane1 = CGridPlaneXY::Create(-25, 25, -25, 25, 0, 1);
  plane1->setColor(GRID_R, GRID_G, GRID_B);
  scene1->insert(plane1);
  scene1->insert(CAxis::Create(-5, -5, -5, 5, 5, 5, 2.5, 3, true));
  CSetOfObjects::Ptr objs = CSetOfObjects::Create();
  vector<pair<CPolyhedron::Ptr, CPolyhedron::Ptr>> polys;
  polys.reserve(16);
  // Addition of polyhedra. Add more polyhedra at wish, but try to avoid
  // intersections with other pairs, for better visualization.
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateHexahedron(10), CPolyhedron::CreateOctahedron(10), objs, -12.5, -12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateIcosahedron(10), CPolyhedron::CreateDodecahedron(10), objs, -12.5, 12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateRhombicuboctahedron(10), CPolyhedron::CreateCuboctahedron(10), objs, 12.5,
      12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateArchimedeanRegularAntiprism(4, 9),
      CPolyhedron::CreateRegularDoublePyramid(9, 10, 15, 6), objs, 12.5, -12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateCuboctahedron(10), CPolyhedron::CreateRhombicDodecahedron(10), objs, -37.5,
      -37.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateRhombicuboctahedron(10), CPolyhedron::CreateDeltoidalIcositetrahedron(10),
      objs, -37.5, -12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateIcosidodecahedron(10), CPolyhedron::CreateRhombicTriacontahedron(10), objs,
      -37.5, 12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateRhombicosidodecahedron(10),
      CPolyhedron::CreateDeltoidalHexecontahedron(10), objs, -37.5, 37.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateTruncatedTetrahedron(10), CPolyhedron::CreateTriakisTetrahedron(10), objs,
      -12.5, -37.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateTruncatedHexahedron(10), CPolyhedron::CreateTriakisOctahedron(10), objs,
      -12.5, 37.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateTruncatedOctahedron(10), CPolyhedron::CreateTetrakisHexahedron(10), objs,
      12.5, -37.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateTruncatedDodecahedron(10), CPolyhedron::CreateTriakisIcosahedron(10), objs,
      12.5, 37.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateTruncatedIcosahedron(10), CPolyhedron::CreatePentakisDodecahedron(10),
      objs, 37.5, -37.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateRandomPolyhedron(10), CPolyhedron::CreateRandomPolyhedron(10), objs, 37.5,
      -12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateDodecahedron(10), CPolyhedron::CreateDeltoidalHexecontahedron(10), objs,
      37.5, 12.5));
  polys.push_back(addPairOfPolys(
      CPolyhedron::CreateTriakisIcosahedron(10), CPolyhedron::CreatePentakisDodecahedron(10), objs,
      37.5, 37.5));
  objs << getIntersections(polys);

  scene1->insert(objs);
  window.get3DSceneAndLock() = scene1;
  window.unlockAccess3DScene();
  window.setCameraElevationDeg(25.0f);
  window.forceRepaint();
  window.waitForKey();
}

int main()
{
  srand((unsigned int)mrpt::system::extractDayTimeFromTimestamp(mrpt::Clock::now()));
  try
  {
    display();
    return 0;
  }
  catch (exception& e)
  {
    cout << "Error: " << e.what() << '.' << endl;
    return -1;
  }
  catch (...)
  {
    cout << "Unknown Error.\n";
    return -1;
  }
}