VTK from Open3D (PointCloud)

#pragma once

#include <vtkCellArray.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkDoubleArray.h>
#include <vtkSmartPointer.h>
#include <vtkPointData.h>
#include <vtkProperty.h>


#include <open3d/Open3D.h>
#include <open3d/geometry/PointCloud.h>
#include <Eigen/Core>

#include "vtkFromO3dCommon.hpp"

namespace vtko3d {

    vtkSmartPointer<vtkPolyData> toCloudPolyData(const open3d::geometry::PointCloud& cloud, USEVALUE usedata=POINT_RGB) {

        // 頂点配列
        vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();

        // 色情報の配列
        vtkSmartPointer<vtkDoubleArray> colors = vtkSmartPointer<vtkDoubleArray>::New();
        colors->SetNumberOfComponents(3);
        colors->SetName("Colors");

        auto normals = vtkSmartPointer<vtkDoubleArray>::New();
        normals->SetNumberOfComponents(3);  // x, y, z
        normals->SetName("Normals");

        // 頂点インデクスの配列
        vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New();


        for (size_t i = 0; i < cloud.points_.size(); i++) {

            double x = cloud.points_[i].x();
            double y = cloud.points_[i].y();
            double z = cloud.points_[i].z();
            points->InsertNextPoint(x, y, z);  // 座標追加

            if (usedata & POINT_RGB) {
                double r = cloud.colors_[i].x();
                double g = cloud.colors_[i].y();
                double b = cloud.colors_[i].z();
                colors->InsertNextTuple3(r, g, b); // 色情報追加
            }
            if (usedata & POINT_NORMAL) {
                double nx = cloud.normals_[i].x();
                double ny = cloud.normals_[i].y();
                double nz = cloud.normals_[i].z();
                normals->InsertNextTuple3(nx, ny, nz); // 法線ベクトル追加
            }

            cells->InsertNextCell(1);  // 要素数
            cells->InsertCellPoint(i); // 頂点インデックスを追加

        }

        // --- vtkPolyData作成 ---
        auto polydata = vtkSmartPointer<vtkPolyData>::New();
        polydata->SetPoints(points);
        polydata->SetVerts(cells);
        if (usedata & POINT_RGB) {
            polydata->GetPointData()->SetScalars(colors);
        }
        if (usedata & POINT_NORMAL) {
            polydata->GetPointData()->SetNormals(normals);
        }

        return polydata;

    }

    vtkSmartPointer<vtkActor> toCloudActor(vtkSmartPointer<vtkPolyDataMapper> mapper, int point_size = 1,std::optional< std::array<double, 3> > single_color = std::nullopt) {
        auto actor = vtkSmartPointer<vtkActor>::New();

        if (single_color.has_value()) {
            mapper->SetScalarVisibility(false); // 頂点ごとの色情報を無視
        }

        actor->SetMapper(mapper);

        if (mapper->GetScalarVisibility() == false) {
            if (single_color.has_value()) {
                actor->GetProperty()->SetColor(single_color.value()[0], single_color.value()[1], single_color.value()[2]); // 単色（R, G, B）を指定
            }
        }

        actor->GetProperty()->SetPointSize(point_size);  // 頂点サイズを5に設定（ピクセル単位）
        actor->GetProperty()->SetLighting(false);
        return actor;
    }

    std::shared_ptr<open3d::geometry::PointCloud> createSampleCloud() {
        auto pointcloud = std::make_shared<open3d::geometry::PointCloud>();
        for (size_t i = 0; i < 100000; i++) {
            double x = (double)rand() / (double)RAND_MAX;
            double y = (double)rand() / (double)RAND_MAX;
            double z = (double)rand() / (double)RAND_MAX;
            pointcloud->points_.emplace_back(x, y, z); // 座標追加
            pointcloud->colors_.emplace_back(x, y, z); // 色情報追加
            pointcloud->normals_.emplace_back(x, y, z); // 法線ベクトル追加
        }
        return pointcloud;
    }