public void onPclData(NetMQMessage m)
{
//UnityEngine.Debug.Log("onPclData" + m[0].BufferSize);
//This contains the pointcloud data
PCLMsg pcl = new PCLMsg(m[0].Buffer);
pcl.fillMesh(m[1].Buffer);
//pcl_frames.Enqueue(pcl);
pcl_frames.Enqueue(pcl);
clientUnity.currentlyreceiving = true;
}
void Update()
{
//If the app didn't click and a done mapping arrived, go to save plan
if (done != true && MapperModule.state == MapperModule.MapperState.DONE)
{
MapperModule.state = MapperModule.MapperState.IDLE;
SaveAndGoToPlanSelectionv2();
}
else if (done)
{
return;
}
//If a last PC deleted arrived, enter here
if (MapperModule.lastPointCloudDeletedBool)
{
//If this is the app that deleted the PC, exit
if (lastPCDeleted)
{
lastPCDeleted = false;
}
else
{
if (savedPointCloud.PointCloud.Count > 0)
{
Destroy(this.transform.GetChild(this.transform.childCount - 1).gameObject);
PointCloudID pointCloudToDelete = savedPointCloud.PointCloud[savedPointCloud.PointCloud.Count - 1].pointCloudID;
savedPointCloud.PointCloud.RemoveAt(savedPointCloud.PointCloud.Count - 1);
numAdded--;
}
}
MapperModule.lastPointCloudDeletedBool = false;
}
//Same as above but for every PC
if (MapperModule.allPointCloudsDeletedBool)
{
if (allPointCloudDeleted)
{
allPointCloudDeleted = false;
}
else
{
for (int i = 0; i < savedPointCloud.PointCloud.Count; i++)
{
Destroy(this.transform.GetChild(i).gameObject);
}
savedPointCloud.PointCloud.Clear();
numAdded = 0;
}
MapperModule.allPointCloudsDeletedBool = false;
}
//We obtain the message of the pointcloud with the structure that contains the points, rotation and ID
PCLMsg pclmsg = MapperModule.DequeuePCLFrame();
if (pclmsg != null)
{
//PCLMsg pclmsg = queue.Dequeue();
//If there are too many pointclouds, delete the last from local. In our tests the app didn't seem to reach a limit.
if (numDrawn > (CLOUDS_TO_RENDER - 1))
{
Destroy(GameObject.Find("cloudgo" + (numDrawn - CLOUDS_TO_RENDER)));
}
//Gets the points and rotation
PCLMesh cm = pclmsg.getCloudMesh();
// Instantiate pointclouds in gameobjects
GameObject instance = GameObject.Instantiate(prefab_cloud_go, this.transform, true);
instance.name = "cloudgo" + numDrawn.ToString();
instance.GetComponent <MeshFilter>().sharedMesh = new Mesh();
instance.GetComponent <MeshFilter>().sharedMesh.name = "cmesh" + numDrawn.ToString();
/*
* instance.GetComponent<MeshFilter>().sharedMesh.vertices = cm.points;
* instance.GetComponent<MeshFilter>().sharedMesh.SetIndices(cm.indices, MeshTopology.Points, 0, true);
* instance.GetComponent<MeshFilter>().sharedMesh.normals = cm.normals;
* instance.GetComponent<MeshFilter>().sharedMesh.colors = cm.colors;
*/
//UnityEngine.Debug.Log("Puntets: " + cm.points[0]);
//Executes the compute shader that turns the points in triangles
runComputeShader(ref cm.points, ref cm.normals);
Triangle[] result = new Triangle[cm.points.Length];
triangles.GetData(result); //GPU-> CPU
releaseCSBuffers(); // It is necessary to release compute buffers after using them
// insert the compute shader results in vectors
// TODO: from compute shader write directly in a buffer[numpoints*3] instead of buffer.p1, buffer.p2, buffer.3
Vector3[] allVertices = new Vector3[cm.points.Length * 3];
Vector3[] allNormals = new Vector3[cm.points.Length * 3];
Color[] allColors = new Color[cm.points.Length * 3];
int[] allIndices = new int[cm.points.Length * 3];
//After obtaining the vertex from gpu, the mesh is created
for (int i = 0, j = 0; i < cm.points.Length * 3; i += 3, j++)
{
allVertices[i] = result[j].p1;
allVertices[i + 1] = result[j].p2;
allVertices[i + 2] = result[j].p3;
CalculateBounds(allVertices[i], allVertices[i + 1], allVertices[i + 2]);
allIndices[i] = i;
allIndices[i + 1] = i + 1;
allIndices[i + 2] = i + 2;
allNormals[i] = cm.normals[j];
allNormals[i + 1] = cm.normals[j];
allNormals[i + 2] = cm.normals[j];
allColors[i] = cm.colors[j];
allColors[i + 1] = cm.colors[j];
allColors[i + 2] = cm.colors[j];
}
//TODO: replace de loop above by using Graphics.DrawProceduralIndirect (on a Camera script) and an appendBuffer in other CompShader (consumer). Tried it but didn't work. Didn't have much time to seriously try
//https://www.digital-dust.com/single-post/2017/07/10/Marching-cubes-on-the-GPU-in-Unity
// attach vertices, colors, and normals to the mesh
instance.GetComponent <MeshFilter>().sharedMesh.vertices = allVertices;
instance.GetComponent <MeshFilter>().sharedMesh.SetIndices(allIndices, MeshTopology.Triangles, 0, true);
instance.GetComponent <MeshFilter>().sharedMesh.normals = allNormals;
instance.GetComponent <MeshFilter>().sharedMesh.colors = allColors;
instance.GetComponent <MeshFilter>().sharedMesh.RecalculateBounds();
#if POZYX_TRANSFORM_COORDS
// Converting coords from Pozyx to Unity, mm to meters, and angles from right-handed to left-handed
Matrix4x4 toUnityCoordsMat = Matrix4x4.TRS(new Vector3(pclmsg.hdr.x * 0.001f, pclmsg.hdr.z * 0.001f, pclmsg.hdr.y * 0.001f),
Quaternion.Euler(pclmsg.hdr.pitch * -180f / 3.141592f, pclmsg.hdr.yaw * -180f / 3.141592f, pclmsg.hdr.roll * -180f / 3.141592f),
new Vector3(1, 1, 1));
instance.transform.rotation = toUnityCoordsMat.rotation;
instance.transform.position = new Vector3(toUnityCoordsMat.m03, toUnityCoordsMat.m13, toUnityCoordsMat.m23);
//instance.transform.RotateAround(drone.transform.position, new Vector3(1, 0, 0), toUnityCoordsMat.rotation.eulerAngles.x);
//instance.transform.RotateAround(drone.transform.position, new Vector3(0, 1, 0), -toUnityCoordsMat.rotation.eulerAngles.y);
//instance.transform.RotateAround(drone.transform.position, new Vector3(0, 0, 1), toUnityCoordsMat.rotation.eulerAngles.z);
//UnityEngine.Debug.Log(toUnityCoordsMat);
//instance.transform.localScale = new Vector3(-1 * instance.transform.localScale.x, instance.transform.localScale.y, instance.transform.localScale.z);
#endif
//The loop that checks if a pointcloud needs to be substitutedw
numDrawn++;
int posOfOldPointCloud = savedPointCloud.isTheCloudAlreadyIn(pclmsg.hdr.pointCloudID);
if (posOfOldPointCloud != -1)
{
GameObject aux = GameObject.Find("cloudgo" + pclmsg.hdr.pointCloudID.i + "" + pclmsg.hdr.pointCloudID.j + "" + pclmsg.hdr.pointCloudID.k + "" + pclmsg.hdr.pointCloudID.heading);
if (aux != null)
{
//Destroying the gameobject
Destroy(aux);
//UnityEngine.Debug.Log("Destroyed");
}
else
{
//search on saved DISK
}
//UnityEngine.Debug.Log("Removed");
//And remove it from the array
savedPointCloud.PointCloud.RemoveAt(posOfOldPointCloud);
}
//Renaming the pointcloud to manage it better
instance.name = "cloudgo" + pclmsg.hdr.pointCloudID.i + "" + pclmsg.hdr.pointCloudID.j + "" + pclmsg.hdr.pointCloudID.k + "" + pclmsg.hdr.pointCloudID.heading;
//savedPointCloud.PointCloud.Add(new SavedMeshPointCloud(allVertices, allColors, Matrix4x4.TRS(new Vector3(pclmsg.hdr.x * 0.001f, pclmsg.hdr.z * 0.001f, pclmsg.hdr.y * 0.001f),
// Quaternion.Euler(pclmsg.hdr.pitch * -180f / 3.141592f, pclmsg.hdr.yaw * -180f / 3.141592f, pclmsg.hdr.roll * -180f / 3.141592f), new Vector3(1, 1, 1)), new Vector3(pclmsg.hdr.pointCloudID.i, pclmsg.hdr.pointCloudID.j, pclmsg.hdr.pointCloudID.k), pclmsg.hdr.pointCloudID.heading));
//Adding the pointcloud to the array
savedPointCloud.PointCloud.Add(new SavedMeshPointCloud(new Vector3(pclmsg.hdr.pointCloudID.i, pclmsg.hdr.pointCloudID.j, pclmsg.hdr.pointCloudID.k), pclmsg.hdr.pointCloudID.heading));
}
//LoadFile();
// Código a ejecutar cuando ya hemos pintado la nube de puntos entera
//if (numDrawn == NUMCLOUDFILES)
//{
// // Ajustamos todos los parámetros de las cámaras para centrarlas sobre la nube de puntos y tratar de abarcarla entera
// AdjustCameras();
//
// canvas.SetActive(true);
//
// numDrawn++;
//}
}
