public void SubscriptionHandler(sensor_msgs.PointCloud2 message)
{
NumberOfClouds = message.data.Length / message.point_step; // Cut the pointcloud to points
if (NumberOfClouds > layer * MaxVerts)
{
NumberToGenerate = (NumberOfClouds - layer * MaxVerts) / MaxVerts + 1;
layer += NumberToGenerate;
createGO = true;
}
RgbPoint3[] Points = new RgbPoint3[NumberOfClouds];
byte[] byteSlice = new byte[message.point_step];
for (long i = 0; i < NumberOfClouds; i++)
{
Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
Points[i] = new RgbPoint3(byteSlice, message.fields);
}
lastMessage = Points;
vertices2d = new Vector3[layer][];
triangles2d = new int[layer][];
colors2d = new Color[layer][];
SetupMeshArrays(Points);
updateMesh = true;
}
public void SubscriptionHandler(sensor_msgs.PointCloud2 message)
{
long I = message.data.Length / message.point_step;
RgbPoint3[] Points = new RgbPoint3[I];
byte[] byteSlice = new byte[message.point_step];
for (long i = 0; i < I; i++)
{
Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
Points[i] = new RgbPoint3(byteSlice, message.fields);
}
newverts = new Vector3[I];
double[] y_array = new double[I];
//Assign all PointCloud points to the Vecto3[]
for (var i = 0; i < I; i++)
{
newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y);
y_array[i] = newverts[i].y;
}
if (I < Ik)
{
flag_object_init = true;
}
if (I > ((layer + 1) * Ik))
{
if (flag == true)
{
layer++;
flag = false;
}
if (I > (layer * Ik))
{
Ic = (int)Iold; //layer*Ik;
flag = true;
flag_object = true;
create_mesh = false;
objcount++;
}
}
CreateMesh(I, Ic, newverts);
Iold = I;
}
public void SubscriptionHandler(sensor_msgs.PointCloud2 message)
{
long I = message.data.Length / message.point_step;
Debug.Log("Long I " + I);
RgbPoint3[] Points = new RgbPoint3[I];
byte[] byteSlice = new byte[message.point_step];
for (long i = 0; i < I; i++)
{
Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
Points[i] = new RgbPoint3(byteSlice, message.fields);
}
newverts = new Vector3[I];
double[] y_array = new double[I];
//Assign all PointCloud points to the Vecto3[]
for (var i = 0; i < I; i++)
{
newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y);
y_array[i] = newverts[i].y;
VectorList.Add(newverts[i]);
}
float inc = 0.15f;
//Assign all the vertices
vertices = new Vector3[I * 8];
int vinc = 0;
for (int k = 0; k < I; k++)
{
for (int i = 0; i < I * 8; i++)
{
if (i == 0 + vinc)
{
vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z + inc);
}
else if (i == 1 + vinc)
{
vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z - inc);
}
else if (i == 2 + vinc)
{
vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z - inc);
}
else if (i == 3 + vinc)
{
vertices[i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z + inc);
}
else if (i == 4 + vinc)
{
vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z + inc);
}
else if (i == 5 + vinc)
{
vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z - inc);
}
else if (i == 6 + vinc)
{
vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z - inc);
}
else if (i == 7 + vinc)
{
vertices[i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z + inc);
}
}
vinc = vinc + 8;
}
int tris = 0; //36;
int vert = 0; //8;
triangles = new int[36 * I];
//Assign triangules of all the vertices
for (int k = 0; k < I; k++)
{
for (int i = 0; i < 36; i++)
{
if (i == 0)
{
triangles[i + tris] = 0 + vert;
}
else if (i == 1)
{
triangles[i + tris] = 2 + vert;
}
else if (i == 2)
{
triangles[i + tris] = 1 + vert;
}
else if (i == 3)
{
triangles[i + tris] = 0 + vert;
}
else if (i == 4)
{
triangles[i + tris] = 3 + vert;
}
else if (i == 5)
{
triangles[i + tris] = 2 + vert;
}
else if (i == 6)
{
triangles[i + tris] = 2 + vert;
}
else if (i == 7)
{
triangles[i + tris] = 3 + vert;
}
else if (i == 8)
{
triangles[i + tris] = 4 + vert;
}
else if (i == 9)
{
triangles[i + tris] = 2 + vert;
}
else if (i == 10)
{
triangles[i + tris] = 4 + vert;
}
else if (i == 11)
{
triangles[i + tris] = 5 + vert;
}
else if (i == 12)
{
triangles[i + tris] = 1 + vert;
}
else if (i == 13)
{
triangles[i + tris] = 2 + vert;
}
else if (i == 14)
{
triangles[i + tris] = 5 + vert;
}
else if (i == 15)
{
triangles[i + tris] = 1 + vert;
}
else if (i == 16)
{
triangles[i + tris] = 5 + vert;
}
else if (i == 17)
{
triangles[i + tris] = 6 + vert;
}
else if (i == 18)
{
triangles[i + tris] = 0 + vert;
}
else if (i == 19)
{
triangles[i + tris] = 7 + vert;
}
else if (i == 20)
{
triangles[i + tris] = 4 + vert;
}
else if (i == 21)
{
triangles[i + tris] = 0 + vert;
}
else if (i == 22)
{
triangles[i + tris] = 4 + vert;
}
else if (i == 23)
{
triangles[i + tris] = 3 + vert;
}
else if (i == 24)
{
triangles[i + tris] = 5 + vert;
}
else if (i == 25)
{
triangles[i + tris] = 4 + vert;
}
else if (i == 26)
{
triangles[i + tris] = 7 + vert;
}
else if (i == 27)
{
triangles[i + tris] = 5 + vert;
}
else if (i == 28)
{
triangles[i + tris] = 7 + vert;
}
else if (i == 29)
{
triangles[i + tris] = 6 + vert;
}
else if (i == 30)
{
triangles[i + tris] = 0 + vert;
}
else if (i == 31)
{
triangles[i + tris] = 6 + vert;
}
else if (i == 32)
{
triangles[i + tris] = 7 + vert;
}
else if (i == 33)
{
triangles[i + tris] = 0 + vert;
}
else if (i == 34)
{
triangles[i + tris] = 1 + vert;
}
else if (i == 35)
{
triangles[i + tris] = 6 + vert;
}
}
vert += 8;
tris += 36;
}
colors = new Color[vertices.Length];
index = 0;
ci = 0;
//Arrange array [y] with no repeat values
List <double> unique_y = new List <double>();
for (int i = 0; i < y_array.Length; i++)
{
bool found = false;
for (int prev = 0; prev < i; prev++)
{
if (y_array[prev] == y_array[i])
{
found = true;
break;
}
}
if (!found)
{
//unique_y.Add(Math.Round(y_array[i],2));
unique_y.Add(y_array[i]);
}
}
//Debug.Log("Y_ARRAY " + y_array.Length);
y_array = unique_y.ToArray();
//Color the octree cubes
for (int i = 0; i < y_array.Length; i++)
{
for (int k = 0; k < I; k++)
{
if (y_array[i] == newverts[k].y)
{
ColorLayer(k, index);
}
}
index++;
if (index == 6)
{
index = 0;
}
}
update_mesh = true;
}
public void SubscriptionHandler(sensor_msgs.PointCloud2 message)
{
long NumberOfClouds = message.data.Length / message.point_step; // Cut the pointcloud to points
Debug.Log(NumberOfClouds);
if (NumberOfClouds > layer * MaxVerts)
{
layer++;
createGO = true;
}
RgbPoint3[] Points = new RgbPoint3[NumberOfClouds];
byte[] byteSlice = new byte[message.point_step];
for (long i = 0; i < NumberOfClouds; i++)
{
Array.Copy(message.data, i * message.point_step, byteSlice, 0, message.point_step);
Points[i] = new RgbPoint3(byteSlice, message.fields);
}
newverts = new Vector3[NumberOfClouds];
//Assign all PointCloud points to the Vector3[]
for (var i = 0; i < NumberOfClouds; i++)
{
newverts[i] = new Vector3(Points[i].x, Points[i].z, Points[i].y);
}
vertices2d = new Vector3[layer][];
triangles2d = new int[layer][];
colors2d = new Color[layer][];
long arraySize;
for (int l = 0; l < layer; l++)
{
if (l + 1 == layer)
{
arraySize = -(layer - 1) * MaxVerts + NumberOfClouds;
}
else
{
arraySize = MaxVerts;
}
float inc = 0.3f;
int vinc = 0;
int tris = 0;
int vert = 0;
int first = l * MaxVerts;
int last = l * (int)MaxVerts + (int)arraySize;
vertices2d[l] = new Vector3[arraySize * 8];
triangles2d[l] = new int[36 * (last - first)];
colors2d[l] = new Color[arraySize * 8];
for (int k = first; k < last; k++)
{
for (int i = 0; i < (last - first) * 8; i++)
{
if (i == 0 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z + inc);
}
else if (i == 1 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y - inc, newverts[k].z - inc);
}
else if (i == 2 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z - inc);
}
else if (i == 3 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x - inc, newverts[k].y + inc, newverts[k].z + inc);
}
else if (i == 4 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z + inc);
}
else if (i == 5 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y + inc, newverts[k].z - inc);
}
else if (i == 6 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z - inc);
}
else if (i == 7 + vinc)
{
vertices2d[l][i] = new Vector3(newverts[k].x + inc, newverts[k].y - inc, newverts[k].z + inc);
}
}
vinc = vinc + 8;
for (int i = 0; i < 36; i++)
{
if (i == 0)
{
triangles2d[l][i + tris] = 0 + vert;
}
else if (i == 1)
{
triangles2d[l][i + tris] = 2 + vert;
}
else if (i == 2)
{
triangles2d[l][i + tris] = 1 + vert;
}
else if (i == 3)
{
triangles2d[l][i + tris] = 0 + vert;
}
else if (i == 4)
{
triangles2d[l][i + tris] = 3 + vert;
}
else if (i == 5)
{
triangles2d[l][i + tris] = 2 + vert;
}
else if (i == 6)
{
triangles2d[l][i + tris] = 2 + vert;
}
else if (i == 7)
{
triangles2d[l][i + tris] = 3 + vert;
}
else if (i == 8)
{
triangles2d[l][i + tris] = 4 + vert;
}
else if (i == 9)
{
triangles2d[l][i + tris] = 2 + vert;
}
else if (i == 10)
{
triangles2d[l][i + tris] = 4 + vert;
}
else if (i == 11)
{
triangles2d[l][i + tris] = 5 + vert;
}
else if (i == 12)
{
triangles2d[l][i + tris] = 1 + vert;
}
else if (i == 13)
{
triangles2d[l][i + tris] = 2 + vert;
}
else if (i == 14)
{
triangles2d[l][i + tris] = 5 + vert;
}
else if (i == 15)
{
triangles2d[l][i + tris] = 1 + vert;
}
else if (i == 16)
{
triangles2d[l][i + tris] = 5 + vert;
}
else if (i == 17)
{
triangles2d[l][i + tris] = 6 + vert;
}
else if (i == 18)
{
triangles2d[l][i + tris] = 0 + vert;
}
else if (i == 19)
{
triangles2d[l][i + tris] = 7 + vert;
}
else if (i == 20)
{
triangles2d[l][i + tris] = 4 + vert;
}
else if (i == 21)
{
triangles2d[l][i + tris] = 0 + vert;
}
else if (i == 22)
{
triangles2d[l][i + tris] = 4 + vert;
}
else if (i == 23)
{
triangles2d[l][i + tris] = 3 + vert;
}
else if (i == 24)
{
triangles2d[l][i + tris] = 5 + vert;
}
else if (i == 25)
{
triangles2d[l][i + tris] = 4 + vert;
}
else if (i == 26)
{
triangles2d[l][i + tris] = 7 + vert;
}
else if (i == 27)
{
triangles2d[l][i + tris] = 5 + vert;
}
else if (i == 28)
{
triangles2d[l][i + tris] = 7 + vert;
}
else if (i == 29)
{
triangles2d[l][i + tris] = 6 + vert;
}
else if (i == 30)
{
triangles2d[l][i + tris] = 0 + vert;
}
else if (i == 31)
{
triangles2d[l][i + tris] = 6 + vert;
}
else if (i == 32)
{
triangles2d[l][i + tris] = 7 + vert;
}
else if (i == 33)
{
triangles2d[l][i + tris] = 0 + vert;
}
else if (i == 34)
{
triangles2d[l][i + tris] = 1 + vert;
}
else if (i == 35)
{
triangles2d[l][i + tris] = 6 + vert;
}
}
vert += 8;
tris += 36;
for (int i = 0; i < 8; i++)
{
ci = i + (8 * (k - first));
colors2d[l][ci] = new Color32((byte)Points[k].rgb[0], (byte)Points[k].rgb[1], (byte)Points[k].rgb[2], 255);
}
}
}
updateMesh = true;
}
