Unity에서 LiDAR Pointcloud에 DBScan을 해보자

using System;
using System.Collections;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using Unity.VisualScripting;
using UnityEngine;
using UnityEngine.Experimental.GlobalIllumination;
using UnityEngine.UI;
using static UnityEditor.Experimental.AssetDatabaseExperimental.AssetDatabaseCounters;

public class DBScan : MonoBehaviour
{
    // DBScan 파라미터 설정
    public float epsilon = 0.2f;
    public int minPoints = 5;

    // Sphere 프리팹
    public GameObject spherePrefab;
    private int poolSize = 10000;
    public List<GameObject> pools = new List<GameObject>();

    public List<UnityEngine.Color> clusterColors = new List<UnityEngine.Color>();

    // 군집 구분을 위한 2중 list 
    public List<List<Vector3>> clusters_now = new List<List<Vector3>>();

    public List<List<Vector3>> clusters_past = new List<List<Vector3>>();


    public List<Vector3> clusterMeans = new List<Vector3>();

    public void Forcluster(int total_num, List<Vector3> pointList)
    {
        // 군집 초기화
        clusterMeans.Clear();
        clusters_now.Clear();
        clusterColors.Clear();

        // 방문 여부 배열
        bool[] visited = new bool[total_num];

        for (int i = 0; i < total_num; i++)
        {
            
            // 이미 방문한 점은 건너뜀
            if (visited[i])
            {
                continue;
            }

            // 새로운 군집 리스트 생성
            List<Vector3> cluster = new List<Vector3>();

            // 군집 확장
            ExpandCluster(i, pointList, cluster, visited);

            // 군집의 크기가 설정 갯수보다 큰 경우에만 최종 군집 설정
            if (cluster.Count >= minPoints) 
            {

                UnityEngine.Color clusterColor = UnityEngine.Random.ColorHSV();
                clusterColors.Add(clusterColor);

                clusters_now.Add(cluster);

                // 클러스터의 평균값 계산하여 리스트에 추가
                Vector3 mean = CalculateClusterMean(cluster);
                clusterMeans.Add(mean);
            }

        }
    }


    private void ExpandCluster(int pointIndex, List<Vector3> pointList, List<Vector3> cluster, bool[] visited)
    {
        // 현재 점 방문 처리
        visited[pointIndex] = true;
        // cluster에 값 등록
        cluster.Add(pointList[pointIndex]);
        
        // 주변 점 탐색
        List<int> neighbors = FindNeighbors(pointIndex, pointList, visited);
        foreach (int neighborIndex in neighbors)
        {
            
            // 이웃 점이 방문되지 않았다면
            if (!visited[neighborIndex])
            {
                // 이웃 점을 군집에 추가
                ExpandCluster(neighborIndex, pointList, cluster, visited);

            }
        }
    }

    private List<int> FindNeighbors(int pointIndex, List<Vector3> pointList, bool[] visited)
    {
        List<int> neighbors = new List<int>();
        HashSet<int> addedIndices = new HashSet<int>(); // 이미 이웃으로 추가된 점의 인덱스를 추적
        
        

        // 현재 점과 거리가 epsilon보다 작은 다른 점들을 이웃으로 간주
        for (int j = 0; j < pointList.Count; j++)
        {
            if (j != pointIndex && !visited[j] && !addedIndices.Contains(j))
            {

                float distance = Vector3.Distance(pointList[pointIndex], pointList[j]);

                if (distance < epsilon)
                {
                    neighbors.Add(j); // 이웃으로 간주되는 점의 인덱스 추가
                    addedIndices.Add(j); // 이미 추가된 점을 추적
                }
            }
        }

        return neighbors;
    }

    private Vector3 CalculateClusterMean(List<Vector3> cluster)
    {
        // 클러스터 내의 점들의 값을 더할 변수 초기화
        Vector3 sum = Vector3.zero;

        // 클러스터 내의 각 점들의 값을 더함
        foreach (Vector3 point in cluster)
        {
            sum += point;
        }

        // 클러스터의 크기로 나누어 평균값 계산
        Vector3 mean = sum / cluster.Count;

        return mean;
    }


    // Start is called before the first frame update
    void Start()
    {
        for (int i = 0; i < poolSize; i++)
        {
            GameObject bullet = Instantiate(spherePrefab);
            bullet.gameObject.SetActive(false);
            pools.Add(bullet);
        }

    }


    // Update is called once per frame
    void Update()
    {
        GameObject forPointsObject = GameObject.Find("RosTcpConnector");

        // For_points 스크립트 컴포넌트를 가져옵니다.
        Ground_seg forPointsScript = forPointsObject.GetComponent<Ground_seg>();


        // pointList 가져오기
        List<Vector3> pointsList = forPointsScript.pointList;
        Forcluster(pointsList.Count, pointsList);

        // 시각화 포인트 갯수 만큼 pools 반복을 위해 q 변수 선언
        int q = 0;



        for (int i = 0; i < pools.Count; i++)
        {
            pools[i].SetActive(false);
        }

        for (int k = 0; k < clusters_now.Count; k++)
        {
            for (int j = 0; j < clusters_now[k].Count; j++)
            {
                // 클러스터 내의 포인트들 시각화
                UnityEngine.Color pointColor = clusterColors[k];
                pools[q].transform.position = clusters_now[k][j];
                pools[q].transform.localScale = new Vector3(0.05f, 0.05f, 0.05f);
                pools[q].GetComponent<Renderer>().material.color = pointColor;
                pools[q].SetActive(true);
                q++;
            }

            // 클러스터 평균값 시각화
            pools[q].transform.position = clusterMeans[k];
            pools[q].transform.localScale = new Vector3(0.1f, 0.1f, 0.1f); // 평균값의 크기 설정
            pools[q].SetActive(true); // 클러스터 평균값 오브젝트 활성화
            q++; // 다음 pools 리스트의 인덱스로 이동
        }


    }


}