tetrahedronGenerator.hpp

#ifndef TETRAHEDRONGENERATOR_HPP
#define TETRAHEDRONGENERATOR_HPP

#include "pcl/common/distances.h"
#include <fstream>
#include <pcl/impl/point_types.hpp>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>

inline std::vector<pcl::PointXYZ>
calculateTetrahedronVertices(float trisize, float x, float y, float z) {
  // float trisize = 0.06;
  float z_trisize = trisize + 0.04;
  float pi = 3.14;

  pcl::PointXYZ A(x, y, z + 2 * trisize / 3);
  pcl::PointXYZ B(x + trisize * cos(0.0 / 3 * 2 * pi),
                  y + trisize * sin(0.0 / 3 * 2 * pi), z - z_trisize / 3);
  pcl::PointXYZ C(x + trisize * cos(1.0 / 3 * 2 * pi),
                  y + trisize * sin(1.0 / 3 * 2 * pi), z - z_trisize / 3);
  pcl::PointXYZ D(x + trisize * cos(2.0 / 3 * 2 * pi),
                  y + trisize * sin(2.0 / 3 * 2 * pi), z - z_trisize / 3);

  std::vector<pcl::PointXYZ> ret;
  ret.emplace_back(A);
  ret.emplace_back(B);
  ret.emplace_back(C);
  ret.emplace_back(D);

  return ret;
}

inline void createTetrahedronModel(std::string plyFile) {
  // load pcl cloud
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
      new pcl::PointCloud<pcl::PointXYZRGB>());
  pcl::io::loadPLYFile("../pointClouds/" + plyFile, *cloud);

  // create and open output obj file
  std::fstream fout;
  fout.open("../pointClouds/tetrahedron_" +
                plyFile.substr(0, plyFile.length() - 4) + ".obj",
            std::ios::out);

  if (!fout) {
    std::cout << "file cannot be opened" << std::endl;
    return;
  }

  // set trisize according to distance of the points
  float dist = pcl::euclideanDistance(cloud->points[0], cloud->points[1]);
  dist *= 0.2f;

  int i = 1;
  for (pcl::PointXYZRGB &pt : cloud->points) {
    std::vector<pcl::PointXYZ> vertex =
        calculateTetrahedronVertices(dist, pt.x, pt.y, pt.z);

    // write the vertices into the obj file
    for (pcl::PointXYZ &vpt : vertex) {
      fout << "v " << vpt.x << " " << vpt.y << " " << vpt.z << " "
           << pt.r / 255.0 << " " << pt.g / 255.0 << " " << pt.b / 255.0
           << std::endl;
    }

    // write the faces
    fout << "f " << i << " " << i + 1 << " " << i + 2 << std::endl;
    fout << "f " << i << " " << i + 2 << " " << i + 3 << std::endl;
    fout << "f " << i << " " << i + 3 << " " << i + 1 << std::endl;
    fout << "f " << i + 1 << " " << i + 3 << " " << i + 2 << std::endl;
    i += 4;
  }

  fout.close();
}

#endif // !TETRAHEDRONGENERATOR_HPP