Add production bathymetric sonar slice

game/world/sonar/reference_bathymetry_acquisition.gd

@@ -0,0 +1,89 @@
+class_name ReferenceBathymetryAcquisition
+extends "res://game/application/interfaces/sonar_acquisition_port.gd"
+
+const SonarMaterial = preload("res://game/domain/sonar/sonar_material.gd")
+const SonarCaptureSample = preload(
+	"res://game/application/sonar/sonar_capture_sample.gd"
+)
+
+const PROBE_HEIGHT_M := 48.0
+const PROBE_DEPTH_M := 92.0
+const COLLISION_MASK := 1
+
+var _world: World3D
+var _excluded_rids: Array[RID] = []
+
+
+func _init(p_world: World3D, excluded_body: CollisionObject3D = null) -> void:
+	_world = p_world
+	if excluded_body != null:
+		_excluded_rids.append(excluded_body.get_rid())
+
+
+func capture(request: SonarCaptureRequest) -> SonarCaptureResult:
+	if _world == null:
+		return SonarCaptureResult.new(false, [], "sonar world is missing")
+	var request_error := request.validation_error()
+	if not request_error.is_empty():
+		return SonarCaptureResult.new(false, [], request_error)
+
+	var samples: Array[SonarCaptureSample] = []
+	var inverse_sensor := request.sensor_transform.affine_inverse()
+	for row in request.rows:
+		var distance_ratio := float(row) / float(request.rows - 1)
+		var forward_distance := lerpf(
+			request.minimum_range_m,
+			request.maximum_range_m,
+			distance_ratio
+		)
+		var half_width := tan(request.horizontal_opening_radians * 0.5) * forward_distance
+		for column in request.columns:
+			var lateral_ratio := float(column) / float(request.columns - 1)
+			var lateral_offset := lerpf(-half_width, half_width, lateral_ratio)
+			var ray_start := request.sensor_transform * Vector3(
+				lateral_offset,
+				PROBE_HEIGHT_M,
+				forward_distance
+			)
+			var ray_end := request.sensor_transform * Vector3(
+				lateral_offset,
+				-PROBE_DEPTH_M,
+				forward_distance
+			)
+			var query := PhysicsRayQueryParameters3D.create(
+				ray_start,
+				ray_end,
+				COLLISION_MASK,
+				_excluded_rids
+			)
+			query.hit_back_faces = true
+			var hit := _world.direct_space_state.intersect_ray(query)
+			if hit.is_empty():
+				continue
+			var world_position: Vector3 = hit["position"]
+			var world_normal: Vector3 = hit["normal"]
+			var local_position := inverse_sensor * world_position
+			var local_normal := (inverse_sensor.basis * world_normal).normalized()
+			var collider := hit.get("collider") as Object
+			var material := SonarMaterial.Value.ROCK
+			var reflectivity := 0.72
+			if collider != null:
+				material = int(collider.get_meta("sonar_material", material))
+				reflectivity = float(
+					collider.get_meta("sonar_reflectivity", reflectivity)
+				)
+			var incidence := clampf(absf(world_normal.dot(Vector3.UP)), 0.18, 1.0)
+			var attenuation := lerpf(1.0, 0.55, distance_ratio)
+			var intensity := clampf(incidence * reflectivity * attenuation, 0.0, 1.0)
+			samples.append(
+				SonarCaptureSample.new(
+					row,
+					column,
+					local_position,
+					local_normal,
+					intensity,
+					local_position.y,
+					material
+				)
+			)
+	return SonarCaptureResult.new(true, samples)