Consider incident angle when calculating intensity (#317)

* Consider incident angle when calculating intensity

* Add API call for setting default intensity

* Review changes

include/rgl/api/core.h

@@ -775,6 +775,16 @@ RGL_API rgl_status_t rgl_node_raytrace_configure_mask(rgl_node_t node, const int
 RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node, float horizontal_beam_divergence,
                                                                  float vertical_beam_divergence);
 
+/**
+ * Modifies RaytraceNode to set default intensity.
+ * This value will be considered when hitting entities with no intensity texture set (`rgl_entity_set_intensity_texture`)
+ * Defaulted default intensity is set to zero.
+ * When accessing `RGL_FIELD_INTENSITY_U8` float is cast to uint8_t type with clamping at uint8_t max value (255).
+ * @param node RaytraceNode to modify.
+ * @param default_intensity Default intensity to set (cannot be a negative number).
+ */
+RGL_API rgl_status_t rgl_node_raytrace_configure_default_intensity(rgl_node_t node, float default_intensity);
+
 /**
  * Creates or modifies FormatPointsNode.
  * The Node converts internal representation into a binary format defined by the `fields` array.

src/api/apiCore.cpp

@@ -962,8 +962,8 @@ RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node
                                                                  float vertical_beam_divergence)
 {
 	auto status = rglSafeCall([&]() {
-		RGL_API_LOG("rgl_node_raytrace_configure_beam_divergence(node={}, horizontal_divergence={}, vertical_divergence={})", repr(node),
-		            horizontal_beam_divergence, vertical_beam_divergence);
+		RGL_API_LOG("rgl_node_raytrace_configure_beam_divergence(node={}, horizontal_divergence={}, vertical_divergence={})",
+		            repr(node), horizontal_beam_divergence, vertical_beam_divergence);
 		CHECK_ARG(node != nullptr);
 		CHECK_ARG((horizontal_beam_divergence > 0.0f && vertical_beam_divergence > 0.0f) ||
 		          (horizontal_beam_divergence == 0.0f && vertical_beam_divergence == 0.0f));
@@ -981,6 +981,27 @@ void TapeCore::tape_node_raytrace_configure_beam_divergence(const YAML::Node& ya
 	rgl_node_raytrace_configure_beam_divergence(node, yamlNode[1].as<float>(), yamlNode[2].as<float>());
 }
 
+RGL_API rgl_status_t rgl_node_raytrace_configure_default_intensity(rgl_node_t node, float default_intensity)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_default_intensity(node={}, default_intensity={})", repr(node),
+		            default_intensity);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(default_intensity >= 0.0f);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setDefaultIntensity(default_intensity);
+	});
+	TAPE_HOOK(node, default_intensity);
+	return status;
+}
+
+void TapeCore::tape_node_raytrace_configure_default_intensity(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_default_intensity(node, yamlNode[1].as<float>());
+}
+
 RGL_API rgl_status_t rgl_node_points_format(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count)
 {
 	auto status = rglSafeCall([&]() {

src/gpu/RaytraceRequestContext.hpp

@@ -28,6 +28,8 @@ struct RaytraceRequestContext
 	float nearNonHitDistance;
 	float farNonHitDistance;
 
+	float defaultIntensity;
+
 	const Mat3x4f* raysWorld;
 	size_t rayCount;
 

src/gpu/optixPrograms.cu

@@ -178,17 +178,20 @@ extern "C" __global__ void __closesthit__()
 		return;
 	}
 
-	// Normal vector and incident angle
+	// Normal vector
 	Vec3f rayDir = optixGetWorldRayDirection();
 	const Vec3f wA = optixTransformPointFromObjectToWorldSpace(A);
 	const Vec3f wB = optixTransformPointFromObjectToWorldSpace(B);
 	const Vec3f wC = optixTransformPointFromObjectToWorldSpace(C);
 	const Vec3f wAB = wB - wA;
 	const Vec3f wCA = wC - wA;
 	const Vec3f wNormal = wAB.cross(wCA).normalized();
-	const float incidentAngle = acosf(fabs(wNormal.dot(rayDir)));
 
-	float intensity = 0;
+	// Incident angle
+	const float cosIncidentAngle = fabs(wNormal.dot(rayDir));
+	const float incidentAngle = acosf(cosIncidentAngle);
+
+	float intensity = ctx.defaultIntensity;
 	bool isIntensityRequested = ctx.intensityF32 != nullptr || ctx.intensityU8 != nullptr;
 	if (isIntensityRequested && entityData.textureCoords != nullptr && entityData.texture != 0) {
 		assert(triangleIndices.x() < entityData.textureCoordsCount);
@@ -203,6 +206,7 @@ extern "C" __global__ void __closesthit__()
 
 		intensity = tex2D<TextureTexelFormat>(entityData.texture, uv[0], uv[1]);
 	}
+	intensity *= cosIncidentAngle;
 
 	// Save sub-sampling results
 	ctx.mrSamples.isHit[mrSamplesIdx] = true;

src/graph/NodesCore.hpp

@@ -134,9 +134,12 @@ struct RaytraceNode : IPointsNode
 	void enableRayDistortion(bool enabled) { doApplyDistortion = enabled; }
 	void setNonHitDistanceValues(float nearDistance, float farDistance);
 	void setNonHitsMask(const int8_t* maskRaw, size_t maskPointCount);
-	void setBeamDivergence(float hDivergenceRad, float vDivergenceRad) {
+	void setBeamDivergence(float hDivergenceRad, float vDivergenceRad)
+	{
 		hBeamHalfDivergenceRad = hDivergenceRad / 2.0f;
-		vBeamHalfDivergenceRad = vDivergenceRad / 2.0f;}
+		vBeamHalfDivergenceRad = vDivergenceRad / 2.0f;
+	}
+	void setDefaultIntensity(float intensity) { defaultIntensity = intensity; }
 
 private:
 	IRaysNode::Ptr raysNode;
@@ -150,6 +153,7 @@ struct RaytraceNode : IPointsNode
 	float farNonHitDistance{std::numeric_limits<float>::infinity()};
 	float hBeamHalfDivergenceRad = 0.0f;
 	float vBeamHalfDivergenceRad = 0.0f;
+	float defaultIntensity = 0.0f;
 
 	DeviceAsyncArray<int8_t>::Ptr rayMask;
 
@@ -747,8 +751,8 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 	                                   // In other words, how long object state can be predicted until it will be declared lost.
 	float movementSensitivity = 0.01f; // Max velocity for an object to be qualified as MovementStatus::Stationary.
 
-	Mat3x4f lookAtSensorFrameTransform { Mat3x4f::identity() };
-	decltype(Time::zero().asMilliseconds()) currentTime { Time::zero().asMilliseconds() };
+	Mat3x4f lookAtSensorFrameTransform{Mat3x4f::identity()};
+	decltype(Time::zero().asMilliseconds()) currentTime{Time::zero().asMilliseconds()};
 
 	HostPinnedArray<Field<XYZ_VEC3_F32>::type>::Ptr xyzHostPtr = HostPinnedArray<Field<XYZ_VEC3_F32>::type>::create();
 	HostPinnedArray<Field<DISTANCE_F32>::type>::Ptr distanceHostPtr = HostPinnedArray<Field<DISTANCE_F32>::type>::create();

src/graph/RaytraceNode.cpp

@@ -95,6 +95,7 @@ void RaytraceNode::enqueueExecImpl()
 	    .doApplyDistortion = doApplyDistortion,
 	    .nearNonHitDistance = nearNonHitDistance,
 	    .farNonHitDistance = farNonHitDistance,
+	    .defaultIntensity = defaultIntensity,
 	    .raysWorld = raysPtr,
 	    .rayCount = raysNode->getRayCount(),
 	    .rayOriginToWorld = raysNode->getCumulativeRayTransfrom(),

src/tape/TapeCore.hpp

@@ -56,6 +56,7 @@ class TapeCore
 	static void tape_node_raytrace_configure_non_hits(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace_configure_mask(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace_configure_beam_divergence(const YAML::Node& yamlNode, PlaybackState& state);
+	static void tape_node_raytrace_configure_default_intensity(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_points_format(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_points_yield(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_points_compact(const YAML::Node& yamlNode, PlaybackState& state);
@@ -114,6 +115,8 @@ class TapeCore
 		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_mask", TapeCore::tape_node_raytrace_configure_mask),
 		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_beam_divergence",
 		                      TapeCore::tape_node_raytrace_configure_beam_divergence),
+		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_default_intensity",
+		                      TapeCore::tape_node_raytrace_configure_default_intensity),
 		    TAPE_CALL_MAPPING("rgl_node_points_format", TapeCore::tape_node_points_format),
 		    TAPE_CALL_MAPPING("rgl_node_points_yield", TapeCore::tape_node_points_yield),
 		    TAPE_CALL_MAPPING("rgl_node_points_compact", TapeCore::tape_node_points_compact),

test/src/TapeTest.cpp

@@ -239,6 +239,9 @@ TEST_F(TapeTest, RecordPlayAllCalls)
 	float vBeamDivergence = 0.1f;
 	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_beam_divergence(raytrace, hBeamDivergence, vBeamDivergence));
 
+	float defaultIntensity = 1.1f;
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_default_intensity(raytrace, defaultIntensity));
+
 	rgl_node_t format = nullptr;
 	std::vector<rgl_field_t> fields = {RGL_FIELD_XYZ_VEC3_F32, RGL_FIELD_DISTANCE_F32};
 	EXPECT_RGL_SUCCESS(rgl_node_points_format(&format, fields.data(), fields.size()));

test/src/graph/nodes/RaytraceNodeTest.cpp

@@ -271,3 +271,56 @@ TEST_F(RaytraceNodeTest, config_non_hit_far_distance_should_correctly_change_out
 		EXPECT_EQ(outIsHits.at(i), 0);
 	}
 }
+
+TEST_F(RaytraceNodeTest, config_default_intensity_should_correctly_change_output)
+{
+	spawnCubeOnScene(Mat3x4f::TRS({0, 0, 0}));
+
+	std::vector<rgl_mat3x4f> rays = {
+	    Mat3x4f::TRS({0, 0, 0}, {0, 0, 0}).toRGL(),                  // hit point
+	    Mat3x4f::TRS({CUBE_HALF_EDGE * 3, 0, 0}, {0, 0, 0}).toRGL(), // non-hit point
+	};
+
+	float defaultIntensity = 100.0f;
+	const float expectedIntensityForNonHit = 0.0f;
+
+	std::vector<rgl_field_t> outFields{IS_HIT_I32, INTENSITY_F32};
+
+	rgl_node_t raysNode = nullptr;
+	rgl_node_t yieldNode = nullptr;
+
+	ASSERT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&raysNode, rays.data(), rays.size()));
+	ASSERT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr));
+	ASSERT_RGL_SUCCESS(rgl_node_raytrace_configure_default_intensity(raytraceNode, defaultIntensity));
+	ASSERT_RGL_SUCCESS(rgl_node_points_yield(&yieldNode, outFields.data(), outFields.size()));
+
+	ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(raysNode, raytraceNode));
+	ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(raytraceNode, yieldNode));
+
+	ASSERT_RGL_SUCCESS(rgl_graph_run(raysNode));
+
+	auto validateOutput = [&]() {
+		TestPointCloud outPointCloud = TestPointCloud::createFromNode(yieldNode, outFields);
+		std::vector<Field<INTENSITY_F32>::type> outIntensities = outPointCloud.getFieldValues<INTENSITY_F32>();
+		std::vector<Field<IS_HIT_I32>::type> outIsHits = outPointCloud.getFieldValues<IS_HIT_I32>();
+
+		ASSERT_EQ(rays.size(), outIntensities.size());
+		ASSERT_EQ(rays.size(), outIsHits.size());
+
+		// Hit point
+		EXPECT_EQ(outIsHits[0], 1);
+		EXPECT_EQ(outIntensities[0], defaultIntensity);
+
+		// Non-hit point
+		EXPECT_EQ(outIsHits[1], 0);
+		EXPECT_EQ(outIntensities[1], expectedIntensityForNonHit);
+	};
+
+	validateOutput();
+
+	// Second round
+	defaultIntensity = 25.0f;
+	ASSERT_RGL_SUCCESS(rgl_node_raytrace_configure_default_intensity(raytraceNode, defaultIntensity));
+	ASSERT_RGL_SUCCESS(rgl_graph_run(raysNode));
+	validateOutput();
+}

test/src/scene/textureTest.cpp

@@ -56,7 +56,8 @@ TEST_P(TextureTest, rgl_texture_reading)
 	// Create RGL graph pipeline.
 	rgl_node_t useRaysNode = nullptr, raytraceNode = nullptr, compactNode = nullptr, yieldNode = nullptr;
 
-	std::vector<rgl_mat3x4f> rays = makeLidar3dRays(360, 360, 0.36, 0.36);
+	std::vector<rgl_mat3x4f> rays = {// Ray must be incident perpendicular to the surface to receive all intensity
+	                                 Mat3x4f::TRS({0, 0, 0}, {0, 0, 0}).toRGL()};
 
 	std::vector<rgl_field_t> yieldFields = {INTENSITY_F32};