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);
}
}
}
}
}
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();
var pcv = new VelodynePointCloudVertex();
pcv.position = hit.point;
pcv.ringNumber = (System.UInt16)x;
pcv.distance = distance;
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;
pcv.color = tex.GetPixelBilinear(pixelUV.x, pixelUV.y);
}
else
{
pcv.color = Color.black;
}
pointCloud.Add(pcv);
}
}
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;
}
}
}
