public void GeneratePointCloud(bool realtimeExport)
{
HashSet <MeshFilter> meshFilterDatabase = new HashSet <MeshFilter>();
var pointCloudAreas = GetComponentsInChildren <PointCloudArea>();
int totalCount = 0;
int batchExportIndex = 0;
int lastBatchExportIndex = batchExportIndex;
List <PointCloudVertex> pointCloudVertices = null;
if (gameobjects.Count > 0)
{
GameObject lastGo = gameobjects[0];
for (int ind = 0; ind < gameobjects.Count; ind++)
{
var go = gameobjects[ind];
if (go == null)
{
continue;
}
lastBatchExportIndex = batchExportIndex;
batchExportIndex = 0;
if (filenamePerObject && !realtimeExport)
{
pointCloudPool.Add(go, new List <PointCloudVertex>());
pointCloudVertices = pointCloudPool[go];
}
else
{
if (!pointCloudPool.ContainsKey(gameObject))
{
pointCloudPool.Add(gameObject, new List <PointCloudVertex>());
}
pointCloudVertices = pointCloudPool[gameObject];
}
var meshFilters = go.GetComponentsInChildren <MeshFilter>();
foreach (var mFilter in meshFilters)
{
var theMesh = mFilter.sharedMesh;
var renderer = mFilter.gameObject.GetComponent <Renderer>();
if (theMesh == null || renderer == null)
{
continue;
}
if (!meshFilterDatabase.Contains(mFilter))
{
mFilter.gameObject.isStatic = false;
for (int i = 0; i < theMesh.subMeshCount; i++)
{
var triangles = new List <int>();
theMesh.GetTriangles(triangles, i);
for (int j = 0; j < triangles.Count; j += 3)
{
var A = theMesh.vertices[triangles[j]];
var B = theMesh.vertices[triangles[j + 1]];
var C = theMesh.vertices[triangles[j + 2]];
var A_wld = mFilter.transform.TransformPoint(A);
var B_wld = mFilter.transform.TransformPoint(B);
var C_wld = mFilter.transform.TransformPoint(C);
var center_wld = (A_wld + B_wld + C_wld) / 3.0f;
float pointAmount = GetTriangleArea(A_wld, B_wld, C_wld) * density;
float extraAmount = .0f;
int totalAreas = 0;
float totalExtraScalers = .0f;
foreach (var pcArea in pointCloudAreas)
{
if (pcArea.Contains(center_wld))
{
totalExtraScalers += pcArea.densityScaler;
++totalAreas;
}
}
if (totalAreas > 0)
{
extraAmount = pointAmount * (totalExtraScalers / totalAreas) - pointAmount;
}
List <Vector3> points = new List <Vector3>();
if (Random.value < (pointAmount % 1.0f))
{
++pointAmount;
}
for (int k = 0; k < System.Math.Round(pointAmount); k++)
{
points.Add(GetRandomPointFromTriangle(A_wld, B_wld, C_wld));
}
if (Random.value < (extraAmount % 1.0f))
{
++extraAmount;
}
for (int k = 0; k < System.Math.Round(extraAmount); k++)
{
var p = GetRandomPointFromTriangle(A_wld, B_wld, C_wld);
bool keep = false;
foreach (var pcArea in pointCloudAreas)
{
if (pcArea.Contains(p))
{
keep = true;
}
}
if (keep)
{
points.Add(p);
}
}
foreach (var p in points)
{
if (boundShape == null || boundShape.Contains(p))
{
Material mat = mFilter.gameObject.GetComponent <Renderer>().sharedMaterials[i];
Vector2 uv = GetInterpolatedPointFromTriangle(p, A_wld, B_wld, C_wld, theMesh.uv[triangles[j]], theMesh.uv[triangles[j + 1]], theMesh.uv[triangles[j + 2]]);
Texture2D tex = mat.mainTexture as Texture2D;
Color c = Color.black;
if (tex != null)
{
try
{
c = tex.GetPixelBilinear(uv.x, uv.y);
}
catch (System.Exception)
{
Debug.Log("Texture GetPixel error: " + tex.name);
return;
}
}
pointCloudVertices.Add(new PointCloudVertex
{
position = p,
// normal = GetInterpolatedPointFromTriangle(p, A_wld, B_wld, C_wld, mFilter.transform.TransformVector(theMesh.normals[triangles[j]]), mFilter.transform.TransformVector(theMesh.normals[triangles[j + 1]]), mFilter.transform.TransformVector(theMesh.normals[triangles[j + 2]])),
uv = uv,
color = c,
material = mat,
});
++totalCount;
if (realtimeExport)
{
if (filenamePerObject)
{
if (go != lastGo)
{
RealtimeExport(pointCloudVertices, lastGo.name, lastBatchExportIndex);
lastGo = go;
pointCloudVertices.Clear();
}
else if (pointCloudVertices.Count >= batchSize)
{
RealtimeExport(pointCloudVertices, go.name, batchExportIndex++);
pointCloudVertices.Clear();
}
}
else
{
if (pointCloudVertices.Count >= batchSize)
{
RealtimeExport(pointCloudVertices, "", batchExportIndex++);
pointCloudVertices.Clear();
}
}
}
}
}
}
}
mFilter.gameObject.isStatic = true;
meshFilterDatabase.Add(mFilter);
}
}
}
if (realtimeExport && pointCloudVertices.Count > 0)
{
if (filenamePerObject)
{
RealtimeExport(pointCloudVertices, gameobjects[gameobjects.Count - 1].name, batchExportIndex++);
}
else
{
RealtimeExport(pointCloudVertices, "", batchExportIndex++);
}
}
}
Debug.Log("Total Point Cloud Vertices Count: " + totalCount);
}
private void FixedUpdate()
{
// Do nothing, if the simulator is paused.
if (!isPlaying)
{
return;
}
// Check if number of steps is greater than possible calculations by unity.
float numberOfStepsNeededInOneLap = 360 / Mathf.Abs(rotationAnglePerStep);
float numberOfStepsPossible = 1 / Time.fixedDeltaTime / rotationSpeedHz;
// Check if it is time to step. Example: 2hz = 2 rotations in a second.
var neededInterval = 1f / (numberOfStepsNeededInOneLap * rotationSpeedHz);
var diff = Time.fixedTime - lastUpdate;
if (diff > neededInterval)
{
int precalculateIterations = (int)(diff / neededInterval);
for (int i = 0; i < precalculateIterations; i++)
{
// Perform rotation.
transform.Rotate(0, rotationAnglePerStep, 0, Space.Self);
// Execute lasers.
for (int x = 0; x < lasers.Count; x++)
{
RaycastHit hit = lasers[x].ShootRay(LidarBitmask);
float distance = hit.distance;
if (distance != 0 && (filterShape == null || !filterShape.Contains(hit.point))) // Didn't hit anything or in filter shape, don't add to list.
{
//float verticalAngle = lasers[x].GetVerticalAngle();
Color c;
Renderer rend = hit.transform.GetComponent <Renderer>();
if (rend != null && (hit.collider as MeshCollider) != null && rend.sharedMaterial != null && rend.sharedMaterial.mainTexture != null)
{
Texture2D tex = rend.sharedMaterial.mainTexture as Texture2D; //Can be improved later dealing with multiple share materials
Vector2 pixelUV = hit.textureCoord;
c = tex.GetPixelBilinear(pixelUV.x, pixelUV.y);
}
else
{
c = Color.black;
}
pointCloud.Add(new VelodynePointCloudVertex()
{
position = hit.point,
ringNumber = (System.UInt16)x,
distance = distance,
color = c,
});
}
}
horizontalAngle += rotationAnglePerStep; // Keep track of our current rotation.
if (horizontalAngle >= 360)
{
horizontalAngle -= 360;
lastLapTime = Time.fixedTime;
publishTimeStamp = Time.fixedTime;
SendPointCloud(pointCloud);
pointCloud.Clear();
}
// Update current execution time.
lastUpdate += neededInterval;
}
}
}
private void FixedUpdate()
{
// Do nothing, if the simulator is paused.
if (!isPlaying)
{
return;
}
// Check if number of steps is greater than possible calculations by unity.
float numberOfStepsNeededInOneLap = 360 / Mathf.Abs(rotationAnglePerStep);
float numberOfStepsPossible = 1 / Time.fixedDeltaTime / 5;
float precalculateIterations = 1;
// Check if we need to precalculate steps.
if (numberOfStepsNeededInOneLap > numberOfStepsPossible)
{
precalculateIterations = (int)(numberOfStepsNeededInOneLap / numberOfStepsPossible);
if (360 % precalculateIterations != 0)
{
precalculateIterations += 360 % precalculateIterations;
}
}
// Check if it is time to step. Example: 2hz = 2 rotations in a second.
if (Time.fixedTime - lastUpdate > (1f / (numberOfStepsNeededInOneLap) / rotationSpeedHz) * precalculateIterations)
{
// Update current execution time.
lastUpdate = Time.fixedTime;
for (int i = 0; i < precalculateIterations; i++)
{
// Perform rotation.
transform.Rotate(0, rotationAnglePerStep, 0, Space.Self);
horizontalAngle += rotationAnglePerStep; // Keep track of our current rotation.
if (horizontalAngle >= 360)
{
horizontalAngle -= 360;
lastLapTime = Time.fixedTime;
publishTimeStamp = Time.fixedTime;
SendPointCloud(pointCloud);
pointCloud.Clear();
}
// Execute lasers.
for (int x = 0; x < lasers.Count; x++)
{
RaycastHit hit = lasers[x].ShootRay();
float distance = hit.distance;
if (distance != 0 && (filterShape == null || !filterShape.Contains(hit.point))) // Didn't hit anything or in filter shape, don't add to list.
{
float verticalAngle = lasers[x].GetVerticalAngle();
var pcv = new VelodynePointCloudVertex();
pcv.position = hit.point;
pcv.ringNumber = (System.UInt16)x;
pcv.distance = distance;
pointCloud.Add(pcv);
}
}
}
}
}
