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

SelectPointsController.cs

Blame
  • SelectPointsController.cs 9.49 KiB
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;
    using UnityEngine.InputSystem;
    using MathNet.Numerics.LinearAlgebra.Factorization;
    using MathNet.Numerics.LinearAlgebra;
    using Unity.Mathematics;
    using static PointCloudDataReader;
    using System.Threading.Tasks;
    
    public class SelectPointsController : MonoBehaviour
    {
        public static SelectPointsController Instance { get; private set; }
    
        public List<GameObject> pointClouds;
    
        public InputActionReference selectPointsEventAction;
        private InputLoader _inputSelectPoints;
    
        public GameObject spawnPrefab;
        public int maxNumberOfPoints = 3000;
    
        private List<GameObject> _blockSelections;
    
        private List<Vector3> _debugPoints;
    
        private List<Vector3> _recalculatedPoints;
    
        private void Awake()
        {
            if (Instance != null && Instance != this)
            {
                Destroy(this);
            }
            else
            {
                Instance = this;
            }
    
            _inputSelectPoints = new(selectPointsEventAction, SelectPointsHandler);
            _blockSelections = new();
            _debugPoints = new();
        }
    
        private void SelectPointsHandler(InputAction.CallbackContext context)
        {
            Vector3[] cloudPositions = new Vector3[pointClouds.Count];
            Quaternion[] cloudRotations = new Quaternion[pointClouds.Count];
            List<Point[]> points = new();
            for (int i = 0; i < pointClouds.Count; i++)
            {
                cloudPositions[i] = pointClouds[i].transform.position;
                cloudRotations[i] = pointClouds[i].transform.rotation;
                points.Add(pointClouds[i].GetComponent<PointCloudDataReader>().PointData);
            }
            Bounds[] selectionBounds = new Bounds[_blockSelections.Count];
            for (int i = 0; i < _blockSelections.Count; i++)
            {
                selectionBounds[i] = _blockSelections[i].GetComponent<MeshCollider>().bounds;
            }
            _ = FitPlaneToSelectedPointsAsync(cloudPositions, cloudRotations, points, selectionBounds);
        }
    
        private async Task FitPlaneToSelectedPointsAsync(Vector3[] cloudPositions, Quaternion[] cloudRotations, List<Point[]> cloudPoints, Bounds[] selectionBounds)
        {
            Debug.Log("task");
            Task<List<Vector3>> task = Task.Run(() =>
            {
                Debug.Log("running task");
                List<Vector3> selectedPoints = new();
                for (int i1 = 0; i1 < cloudPoints.Count; i1++)
                {
                    // GameObject cloud = pointClouds[i1];
                    // Transform cloudTransform = cloud.transform;
                    // Point[] points = cloud.GetComponent<PointCloudDataReader>().PointData;
                    Point[] points = cloudPoints[i1];
                    for (int i = 0; i < points.Length; i++)
                    {
                        Vector3 point = points[i].position;
                        point.x *= -1f;
                        // foreach (GameObject blockSelection in _blockSelections)
                        foreach (Bounds bound in selectionBounds)
                        {
                            // MeshCollider collider = blockSelection.GetComponent<MeshCollider>();
                            // Vector3 ptGlobal = cloudTransform.rotation * point + cloudTransform.position;
                            Vector3 ptGlobal = cloudRotations[i1] * point + cloudPositions[i1];
                            // if (collider.bounds.Contains(ptGlobal))
                            if (bound.Contains(ptGlobal))
                            {
                                selectedPoints.Add(ptGlobal);
                            }
                        }
                    }
                }
                Debug.Log("finished collecting points");
                return selectedPoints;
            });
            CalculatePlane(await task);
        }
    
        // source: https://stackoverflow.com/questions/10900141/fast-plane-fitting-to-many-points
        private void CalculatePlane(List<Vector3> points)
        {
            Debug.Log("calculating plane");
            if (points.Count < 3)
            {
                Debug.Log("not enough points to calculate plane");
                return;
            }
    
            if (points.Count > maxNumberOfPoints)
            {
                double dropChance = maxNumberOfPoints / (double)points.Count;
                System.Random rnd = new();
                List<Vector3> newPoints = new();
                foreach (Vector3 v in points)
                {
                    if (rnd.NextDouble() < dropChance)
                    {
                        newPoints.Add(v);
                    }
    
                    if (newPoints.Count >= maxNumberOfPoints) break;
                }
                points = newPoints;
            }
    
            Matrix<float> G = Matrix<float>.Build.Dense(points.Count, 4);
            for (int i = 0; i < points.Count; i++)
            {
                // Vector3 pt = points[i].normalized;
                Vector3 pt = points[i];
                Vector<float> point = Vector<float>.Build.Dense(new float[] { pt.x, pt.y, pt.z, 1 });
                G.SetRow(i, point);
            }
    
            Svd<float> svd = G.Svd(true);
            Matrix<float> vt = svd.VT;
            // vt = vt.Transpose();
            Vector<float> planeEq = vt.Row(3);
    
            Vector3 planeNormal = new(planeEq[0], planeEq[1], planeEq[2]);
            // planeNormal = pointClouds[0].transform.rotation * planeNormal;
            planeNormal.Normalize();
    
            float d = planeNormal.x * points[0].x + planeNormal.y * points[0].y + planeNormal.z * points[0].z;
    
            Plane plane = new(planeNormal, -d);
            Debug.Log(plane);
    
            // _debugPoints.Clear();
            // for (int i = 0; i < 200; i++)
            // {
            //     Vector3 randomPoint = new(UnityEngine.Random.Range(-8f, 8f), UnityEngine.Random.Range(-8f, 8f), UnityEngine.Random.Range(-8f, 8f));
            //     _debugPoints.Add(ProjectPointOntoPlane(plane, randomPoint));
            // }
    
            Vector<float> colAvg = G.ColumnSums();
            colAvg /= G.RowCount;
            Vector3 pointAvg = new(colAvg[0], colAvg[1], colAvg[2]);
            SpawnPlane(ProjectPointOntoPlane(plane, pointAvg), planeNormal);
            UIController.Instance.NewStatusMessage("Plane calculation completed.");
        }
    
        public GameObject SpawnPlane(Vector3 pos, Vector3 normal)
        {
            GameObject spawnedObject = Instantiate(spawnPrefab, pos, Quaternion.identity);
            spawnedObject.transform.parent = this.transform;
            spawnedObject.transform.LookAt(spawnedObject.transform.position + normal);
    
            ProgramController.Instance.RegisterQuad(spawnedObject);
            return spawnedObject;
        }
    
        public void LoadPlanes(List<Vector3[]> planes)
        {
            foreach (Vector3[] data in planes)
            {
                GameObject spawnedObject = SpawnPlane(data[0], data[1]);
                Vector3[] vertexData = new Vector3[data.Length - 2];
                Array.Copy(data, 2, vertexData, 0, data.Length - 2);
    
                spawnedObject.GetComponent<FittedQuad>().SetAnchors(vertexData);
            }
        }
    
        private Vector3 ProjectPointOntoPlane(Plane plane, Vector3 point)
        {
            float distance = plane.GetDistanceToPoint(point);
            Vector3 projectedPoint = point - plane.normal * distance;
    
            return projectedPoint;
        }
    
        // private void OnDrawGizmosSelected()
        // {
        //     Gizmos.color = Color.red;
        //     foreach (Vector3 pt in _debugPoints)
        //     {
        //         Gizmos.DrawSphere(pt, 0.05f);
        //     }
        // }
    
        public void AddSelection(GameObject selection)
        {
            _blockSelections.Add(selection);
        }
    
        public void ClearSelection()
        {
            foreach (GameObject selection in _blockSelections)
            {
                Destroy(selection);
            }
            _blockSelections.Clear();
        }
    
        public void RemoveSelection(GameObject selection)
        {
            _blockSelections.Remove(selection);
            Destroy(selection);
        }
    
        // Start is called before the first frame update
        void Start()
        {
            // CalculateRealPoints();
            EditState.Instance.RegisterListener(EditChangeHandler);
            // SpawnPlane(new Vector3(0, 0, 0), Vector3.left);
            // SpawnPlane(new Vector3(-1, 0, 0), Vector3.up);
        }
    
        private void EditChangeHandler()
        {
            // if (EditState.Instance.ActiveEditState != EditState.EditType.Select)
            // {
            //     ClearSelection();
            // }
        }
    
        private void CalculateRealPoints()
        {
            _recalculatedPoints = new();
    
            float epsilon = 0.005f;
    
            Mesh pointCloudMesh = pointClouds[0].GetComponent<MeshFilter>().mesh;
            Vector3[] cloudVertices = pointCloudMesh.vertices;
            Vector3[] cloudNormals = pointCloudMesh.normals;
            Vector3 cloudNormal = cloudNormals[0];
            List<Vector3> normalsPerFace = new()
            {
                cloudNormals[0] * pointCloudMesh.vertices[0].magnitude
            };
            for (int i = 1; i < cloudVertices.Length; i++)
            {
                Vector3 pt = cloudVertices[i];
                float distance = Vector3.Distance(cloudNormals[i], cloudNormal);
                if (distance < epsilon)
                {
                    normalsPerFace.Add(cloudNormals[i] * pt.magnitude);
                }
                else
                {
                    Vector3 normalAverage = normalsPerFace[0];
                    if (normalsPerFace.Count > 1)
                    {
                        for (int j = 1; j < normalsPerFace.Count; j++)
                        {
                            normalAverage += normalsPerFace[j];
                        }
    
                        normalAverage /= normalsPerFace.Count;
                    }
    
                    _recalculatedPoints.Add(normalAverage);
    
                    cloudNormal = cloudNormals[i];
                    normalsPerFace.Clear();
                    normalsPerFace.Add(cloudNormals[i] * pt.magnitude);
                }
    
            }
        }
    }