Skip to content
Snippets Groups Projects
Select Git revision
  • 057a1392932f6052f47a769097ea7ca3c32f4516
  • master default protected
2 results

tetrahedronGenerator.hpp

Blame
  • tetrahedronGenerator.hpp 2.38 KiB
    #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