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; } } }