Select Git revision
SelectPointsController.cs
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);
}
}
}
}