public void fillMesh(ref PCLMesh mesh, byte[] data_bytes, PCLMsgHeader header, UInt64 num_points, int offset_data, ref int tamañoPointCloudActual) { //UnityEngine.Debug.Log("offset data " + offset_data); UInt64 N; //20000 * 3 < 65000 points allowed in Unity if (num_points > 20000) { N = 20000; } else { N = num_points; } UnityEngine.Debug.Log(N); mesh.points = new Vector3[N]; mesh.normals = new Vector3[N]; mesh.colors = new Color[N]; mesh.indices = new int[N]; Matrix4x4 toUnityCoordsMat = Matrix4x4.TRS(new Vector3(header.x * 0.001f, header.z * 0.001f, header.y * 0.001f), Quaternion.Euler(-header.pitch, -header.yaw, -header.roll), new Vector3(1, 1, 1)); for (int i = 0, byte_index = 8 + offset_data; i < (int)N; byte_index += PCLMsgOffsets.POINTNORMAL_SIZE, i++) { mesh.points[i].x = BitConverter.ToSingle(data_bytes, 36 + byte_index + PCLMsgOffsets.POINT_X); tamañoPointCloudActual += 4; mesh.points[i].y = BitConverter.ToSingle(data_bytes, 36 + byte_index + PCLMsgOffsets.POINT_Z); // Changing from Pozyx to Unity tamañoPointCloudActual += 4; mesh.points[i].z = BitConverter.ToSingle(data_bytes, 36 + byte_index + PCLMsgOffsets.POINT_Y); tamañoPointCloudActual += 4; #if TRANSFORM_COORDS points[i] = toUnityCoordsMat.MultiplyPoint(points[i]); #elif POZYX_TRANSFORM_COORDS //points[i] = toUnityCoordsMat.MultiplyVector(points[i]); #endif mesh.colors[i].r = data_bytes[36 + byte_index + PCLMsgOffsets.POINT_R] / 255f; mesh.colors[i].g = data_bytes[36 + byte_index + PCLMsgOffsets.POINT_G] / 255f; mesh.colors[i].b = data_bytes[36 + byte_index + PCLMsgOffsets.POINT_B] / 255f; //colors[i].a = msg_bytes[byte_index + PCLMsgOffsets.POINT_A] / 255f; mesh.colors[i].a = 1f; tamañoPointCloudActual += 4; //normals[i] = new Vector3(0.1f, 0f, 0.9f).normalized; mesh.normals[i].x = BitConverter.ToSingle(data_bytes, 36 + byte_index + PCLMsgOffsets.NORMAL_X); tamañoPointCloudActual += 4; mesh.normals[i].y = BitConverter.ToSingle(data_bytes, 36 + byte_index + PCLMsgOffsets.NORMAL_Z); tamañoPointCloudActual += 4; mesh.normals[i].z = BitConverter.ToSingle(data_bytes, 36 + byte_index + PCLMsgOffsets.NORMAL_Y); tamañoPointCloudActual += 4; mesh.indices[i] = i; } }
/* old code * public PCLMsg(ref byte[] all_data_bytes) * { * hdr = new PCLMsgHeader(); * cloud_mesh = new PCLMesh(); * * this.data_bytes = all_data_bytes; * setHeader(ref all_data_bytes); * } * * public void setHeader(ref byte[] all_data_bytes) * { * hdr.airt = all_data_bytes[PCLMsgOffsets.AIRT]; * hdr.module = all_data_bytes[PCLMsgOffsets.MODULE]; * hdr.action = all_data_bytes[PCLMsgOffsets.ACTION]; * * hdr.numPoints = BitConverter.ToUInt32(all_data_bytes, PCLMsgOffsets.NUMPOINTS); * //Debug.Log(hdr.numPoints); * * hdr.i = BitConverter.ToInt32(all_data_bytes, PCLMsgOffsets.I); * hdr.j = BitConverter.ToInt32(all_data_bytes, PCLMsgOffsets.J); * hdr.k = BitConverter.ToInt32(all_data_bytes, PCLMsgOffsets.K); * } * */ public PCLMsg(byte[] header_bytes) { hdr = new PCLMsgHeader(); cloud_mesh = new PCLMesh(); this.header_bytes = header_bytes; setHeaderValues(); }
/// <summary> /// The function that loads a pointcloud. Same as mentioned in PlaceModel.cs /// </summary> /// <param name="pcl"></param> /// <param name="daeModel"></param> void LoadPointCloud(byte[] pcl, ref GameObject daeModel) { int numDrawn = 0; //PCLMsg pclmsg = queue.Dequeue(); PCLMsgHeader cm = new PCLMsgHeader(); string cabezeraBasura = BitConverter.ToString(pcl, 0, 4); UInt64 numberOfPCL = BitConverter.ToUInt64(pcl, 28); PCLMesh[] mesh; mesh = new PCLMesh[numberOfPCL]; int tamañoPointCloudActual = 0; int tamañoPointCloudPasado = 0; for (UInt64 i = 0; i < numberOfPCL; i++) { string firma = BitConverter.ToString(pcl, 36 + tamañoPointCloudPasado, 4); tamañoPointCloudActual += 4; cm.x = BitConverter.ToSingle(pcl, 36 + tamañoPointCloudPasado + 4); tamañoPointCloudActual += 4; cm.y = BitConverter.ToSingle(pcl, 36 + tamañoPointCloudPasado + 8); tamañoPointCloudActual += 4; cm.z = BitConverter.ToSingle(pcl, 36 + tamañoPointCloudPasado + 12); tamañoPointCloudActual += 4; cm.pitch = BitConverter.ToSingle(pcl, 36 + tamañoPointCloudPasado + 16); tamañoPointCloudActual += 4; cm.roll = BitConverter.ToSingle(pcl, 36 + tamañoPointCloudPasado + 20); tamañoPointCloudActual += 4; cm.yaw = BitConverter.ToSingle(pcl, 36 + tamañoPointCloudPasado + 24); tamañoPointCloudActual += 4; UInt64 numberOfPoints = BitConverter.ToUInt32(pcl, 36 + tamañoPointCloudPasado + 28); tamañoPointCloudActual += 8; mesh[i] = new PCLMesh(); fillMesh(ref mesh[i], pcl, cm, numberOfPoints, tamañoPointCloudPasado, ref tamañoPointCloudActual); // Instanciate 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]); runComputeShader(ref mesh[i].points, ref mesh[i].normals); Triangle[] result = new Triangle[mesh[i].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[mesh[i].points.Length * 3]; Vector3[] allNormals = new Vector3[mesh[i].points.Length * 3]; Color[] allColors = new Color[mesh[i].points.Length * 3]; int[] allIndices = new int[mesh[i].points.Length * 3]; for (int k = 0, l = 0; k < mesh[i].points.Length * 3; k += 3, l++) { allVertices[k] = result[l].p1; allVertices[k + 1] = result[l].p2; allVertices[k + 2] = result[l].p3; //CalculateBounds(allVertices[k], allVertices[k + 1], allVertices[k + 2]); allIndices[k] = k; allIndices[k + 1] = k + 1; allIndices[k + 2] = k + 2; allNormals[k] = mesh[i].normals[l]; allNormals[k + 1] = mesh[i].normals[l]; allNormals[k + 2] = mesh[i].normals[l]; allColors[k] = mesh[i].colors[l]; allColors[k + 1] = mesh[i].colors[l]; allColors[k + 2] = mesh[i].colors[l]; } //TODO: replace de loop above by using Graphics.DrawProceduralIndirect (on a Camera script) and an appendBuffer in other CompShader (consumer) // 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(); // Converting coords from Pozyx to Unity, mm to meters, and angles from right-handed to left-handed Matrix4x4 toUnityCoordsMat = Matrix4x4.TRS(new Vector3(cm.x * 0.001f, cm.z * 0.001f, cm.y * 0.001f), Quaternion.Euler(cm.pitch * -180f / 3.141592f, cm.yaw * -180f / 3.141592f, cm.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); //instance.name = "cloudgo" + pclmsg.cm.pointCloudID.i + "" + pclmsg.cm.pointCloudID.j + "" + pclmsg.cm.pointCloudID.k + "" + pclmsg.cm.pointCloudID.heading; instance.transform.parent = daeModel.transform; tamañoPointCloudPasado = tamañoPointCloudActual; } }
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++; //} }