/// <summary>
/// This one is to delete pointclouds
/// </summary>
/// <param name="module"></param>
/// <param name="action"></param>
/// <param name="pointCloudID"></param>
/// <returns></returns>
public bool sendCommand(byte module, byte action, PointCloudID pointCloudID)
{
byte[] header_bytes = new byte[17];
header_bytes[0] = (byte)'A';
header_bytes[1] = module;
header_bytes[2] = action;
header_bytes[3] = (byte)' ';
header_bytes[16] = pointCloudID.heading;
byte[] iArray = BitConverter.GetBytes(pointCloudID.i);
byte[] jArray = BitConverter.GetBytes(pointCloudID.j);
byte[] kArray = BitConverter.GetBytes(pointCloudID.k);
for (int i = 4; i < 8; i++)
{
header_bytes[i + 1 * 0] = iArray[i - 4];
header_bytes[i + 1 * 4] = jArray[i - 4];
header_bytes[i + 1 * 8] = kArray[i - 4];
}
NetMQMessage req_msg = new NetMQMessage();
req_msg.Append(header_bytes);
NetMQ.Msg resp_msg = new StdMessage(0x00, 0x00).to_Msg(); // it will be filled when receiving the response ## la respuesta de dos partes sigue siendo de este tipo?
reqSrv = new ReliableExternalClient(
GlobalSettings.Instance.getRequestPort(), TimeSpan.FromMilliseconds(1000), 3);
if (!reqSrv.sendAndReceive(ref req_msg, ref resp_msg))
{
UnityEngine.Debug.Log("Client: server not respoding");
return(false);
}
return(true);
}
public int isTheCloudAlreadyIn(PointCloudID id)
{
for (int i = 0; i < PointCloud.Count; i++)
{
PointCloudID aux = PointCloud[i].pointCloudID;
if (aux.i == id.i && aux.j == id.j && aux.k == id.k && aux.heading == id.heading)
{
return(i);
}
}
return(-1);
}
/// <summary>
/// Delete the last drawed pointcloud
/// </summary>
public void deleteLastPointCloud()
{
// A user can click faster than the quantity of pointclouds received
if (savedPointCloud.PointCloud.Count > 0)
{
Destroy(this.transform.GetChild(this.transform.childCount - 1).gameObject);
PointCloudID pointCloudToDelete = savedPointCloud.PointCloud[savedPointCloud.PointCloud.Count - 1].pointCloudID;
clientUnity.client.sendCommand((byte)Modules.MAPPER_MODULE, (byte)MapperCommandType.MAPPER_DELETE_POINT_CLOUD, pointCloudToDelete);
savedPointCloud.PointCloud.RemoveAt(savedPointCloud.PointCloud.Count - 1);
// UnityEngine.Debug.Log("PointCloud Deleted: " + pointCloudToDelete.i + "" + pointCloudToDelete.j + "" + pointCloudToDelete.k + "" + pointCloudToDelete.heading);
numAdded--;
lastPCDeleted = true;
}
}
public SavedMeshPointCloud(Vector3 posId, byte otherID)
{
//vertex = _Vertex;
//int i = 0;
//colors = new Vector3[vertex.Length];
//foreach (var color in _Colors)
//{
// colors[i] = new Vector3(color.r, color.g, color.b);
// i++;
//}
//matrix = _Matrix;
pointCloudID = new PointCloudID();
pointCloudID.i = (int)posId.x;
pointCloudID.j = (int)posId.y;
pointCloudID.k = (int)posId.z;
pointCloudID.heading = otherID;
}
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++;
//}
}