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();
}
public void fillMesh(ref byte[] data_bytes, PCLMsgHeader header, int num_points, int offset_data)
{
//UnityEngine.Debug.Log("offset data " + offset_data);
int N; //20000 * 3 < 65000 points allowed in Unity
if (num_points > 20000)
{
N = 20000;
}
else
{
N = num_points;
}
points = new Vector3[N];
normals = new Vector3[N];
colors = new Color[N];
indices = new int[N];
//Matrix to transform from pozyx to Unity. Not used right now
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 = offset_data; i < N; byte_index += PCLMsgOffsets.POINTNORMAL_SIZE, i++)
{
//Position of the point from the pointCloud
points[i].x = BitConverter.ToSingle(data_bytes, byte_index + PCLMsgOffsets.POINT_X);
points[i].y = BitConverter.ToSingle(data_bytes, byte_index + PCLMsgOffsets.POINT_Z); // Changing from Pozyx to Unity
points[i].z = BitConverter.ToSingle(data_bytes, byte_index + PCLMsgOffsets.POINT_Y);
#if TRANSFORM_COORDS
points[i] = toUnityCoordsMat.MultiplyPoint(points[i]);
#elif POZYX_TRANSFORM_COORDS
//points[i] = toUnityCoordsMat.MultiplyVector(points[i]);
#endif
//Color
colors[i].r = data_bytes[byte_index + PCLMsgOffsets.POINT_R] / 255f;
colors[i].g = data_bytes[byte_index + PCLMsgOffsets.POINT_G] / 255f;
colors[i].b = data_bytes[byte_index + PCLMsgOffsets.POINT_B] / 255f;
//colors[i].a = msg_bytes[byte_index + PCLMsgOffsets.POINT_A] / 255f;
colors[i].a = 1f;
//Normal
//normals[i] = new Vector3(0.1f, 0f, 0.9f).normalized;
normals[i].x = BitConverter.ToSingle(data_bytes, byte_index + PCLMsgOffsets.NORMAL_X);
normals[i].y = BitConverter.ToSingle(data_bytes, byte_index + PCLMsgOffsets.NORMAL_Z);
normals[i].z = BitConverter.ToSingle(data_bytes, byte_index + PCLMsgOffsets.NORMAL_Y);
indices[i] = i;
}
}
/// <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;
}
}
