299 lines
9.0 KiB
GDScript
299 lines
9.0 KiB
GDScript
class_name SonarService
|
|
extends RefCounted
|
|
|
|
const SonarMaterial = preload("res://game/domain/sonar/sonar_material.gd")
|
|
const SonarScanState = preload("res://game/domain/sonar/sonar_scan_state.gd")
|
|
const SonarSample = preload("res://game/domain/sonar/sonar_sample.gd")
|
|
const SonarSampleBuffer = preload(
|
|
"res://game/domain/sonar/sonar_sample_buffer.gd"
|
|
)
|
|
const SonarContact = preload("res://game/domain/sonar/sonar_contact.gd")
|
|
const SonarScan = preload("res://game/domain/sonar/sonar_scan.gd")
|
|
const SonarCaptureRequest = preload(
|
|
"res://game/application/sonar/sonar_capture_request.gd"
|
|
)
|
|
const SonarCaptureResult = preload(
|
|
"res://game/application/sonar/sonar_capture_result.gd"
|
|
)
|
|
const SonarAcquisitionPort = preload(
|
|
"res://game/application/interfaces/sonar_acquisition_port.gd"
|
|
)
|
|
const SonarCommandResult = preload(
|
|
"res://game/application/sonar/sonar_command_result.gd"
|
|
)
|
|
|
|
const MINIMUM_RANGE_M := 5.0
|
|
const MAXIMUM_RANGE_M := 95.0
|
|
const HORIZONTAL_OPENING_RADIANS := deg_to_rad(76.0)
|
|
const VERTICAL_OPENING_RADIANS := deg_to_rad(64.0)
|
|
const COLUMNS := 49
|
|
const ROWS := 31
|
|
const REVEAL_DURATION_SECONDS := 2.5
|
|
const RECOVERY_DURATION_SECONDS := 0.35
|
|
const FIRST_RETURN_PROGRESS := 0.04
|
|
const PREVIOUS_SCAN_STARTING_OPACITY := 0.52
|
|
const MINIMUM_COLOR_SPAN_M := 6.0
|
|
|
|
var _acquisition: SonarAcquisitionPort
|
|
var _session_id: StringName
|
|
var _scan_sequence := 0
|
|
var _phase_elapsed_seconds := 0.0
|
|
var _current_scan: SonarScan
|
|
var _previous_scan: SonarScan
|
|
|
|
|
|
func _init(p_acquisition: SonarAcquisitionPort, p_session_id: StringName) -> void:
|
|
_acquisition = p_acquisition
|
|
_session_id = p_session_id
|
|
|
|
|
|
func request_scan(sensor_transform: Transform3D) -> SonarCommandResult:
|
|
if _acquisition == null:
|
|
return SonarCommandResult.new(false, "sonar acquisition port is missing")
|
|
if _session_id.is_empty():
|
|
return SonarCommandResult.new(false, "sonar session id is empty")
|
|
if _current_scan != null and _current_scan.state != SonarScanState.Value.READY:
|
|
return SonarCommandResult.new(false, "sonar is already scanning")
|
|
|
|
var scan_id := StringName("%s-scan-%d" % [_session_id, _scan_sequence])
|
|
var request := SonarCaptureRequest.new(
|
|
scan_id,
|
|
_session_id,
|
|
sensor_transform,
|
|
MINIMUM_RANGE_M,
|
|
MAXIMUM_RANGE_M,
|
|
HORIZONTAL_OPENING_RADIANS,
|
|
VERTICAL_OPENING_RADIANS,
|
|
COLUMNS,
|
|
ROWS
|
|
)
|
|
var request_error := request.validation_error()
|
|
if not request_error.is_empty():
|
|
return SonarCommandResult.new(false, request_error)
|
|
var capture: SonarCaptureResult = _acquisition.capture(request)
|
|
if capture == null or not capture.accepted:
|
|
return SonarCommandResult.new(
|
|
false,
|
|
"sonar acquisition failed" if capture == null else capture.message
|
|
)
|
|
var built = _build_scan(request, capture)
|
|
if built is String:
|
|
return SonarCommandResult.new(false, built)
|
|
|
|
_previous_scan = _current_scan
|
|
_current_scan = built as SonarScan
|
|
_phase_elapsed_seconds = 0.0
|
|
_scan_sequence += 1
|
|
return SonarCommandResult.new(true, "", _current_scan)
|
|
|
|
|
|
func advance(elapsed_seconds: float) -> SonarScan:
|
|
if _current_scan == null or not is_finite(elapsed_seconds) or elapsed_seconds < 0.0:
|
|
return _current_scan
|
|
if _current_scan.state == SonarScanState.Value.REVEALING:
|
|
_phase_elapsed_seconds += elapsed_seconds
|
|
var progress := clampf(
|
|
_phase_elapsed_seconds / REVEAL_DURATION_SECONDS,
|
|
0.0,
|
|
1.0
|
|
)
|
|
if progress >= 1.0:
|
|
_current_scan = _current_scan.with_phase(
|
|
SonarScanState.Value.RECOVERY,
|
|
1.0
|
|
)
|
|
_phase_elapsed_seconds = 0.0
|
|
_previous_scan = null
|
|
else:
|
|
_current_scan = _current_scan.with_phase(
|
|
SonarScanState.Value.REVEALING,
|
|
progress
|
|
)
|
|
elif _current_scan.state == SonarScanState.Value.RECOVERY:
|
|
_phase_elapsed_seconds += elapsed_seconds
|
|
if _phase_elapsed_seconds >= RECOVERY_DURATION_SECONDS:
|
|
_current_scan = _current_scan.with_phase(
|
|
SonarScanState.Value.READY,
|
|
1.0
|
|
)
|
|
_phase_elapsed_seconds = 0.0
|
|
return _current_scan
|
|
|
|
|
|
func current_scan() -> SonarScan:
|
|
return _current_scan
|
|
|
|
|
|
func previous_scan() -> SonarScan:
|
|
return _previous_scan
|
|
|
|
|
|
func visible_row_count() -> int:
|
|
if _current_scan == null:
|
|
return 0
|
|
if _current_scan.state != SonarScanState.Value.REVEALING:
|
|
return _current_scan.samples.rows
|
|
if _current_scan.reveal_progress < FIRST_RETURN_PROGRESS:
|
|
return 0
|
|
var normalized := inverse_lerp(
|
|
FIRST_RETURN_PROGRESS,
|
|
1.0,
|
|
_current_scan.reveal_progress
|
|
)
|
|
return clampi(
|
|
floori(normalized * float(_current_scan.samples.rows - 1)) + 1,
|
|
1,
|
|
_current_scan.samples.rows
|
|
)
|
|
|
|
|
|
func previous_scan_opacity() -> float:
|
|
if _previous_scan == null or _current_scan == null:
|
|
return 0.0
|
|
return PREVIOUS_SCAN_STARTING_OPACITY * (1.0 - _current_scan.reveal_progress)
|
|
|
|
|
|
func _build_scan(request: SonarCaptureRequest, capture: SonarCaptureResult) -> Variant:
|
|
var buffer := SonarSampleBuffer.new(request.columns, request.rows)
|
|
var minimum_elevation := INF
|
|
var maximum_elevation := -INF
|
|
for captured_sample in capture.samples:
|
|
if captured_sample == null:
|
|
return "sonar acquisition returned a missing sample"
|
|
var capture_error: String = captured_sample.validation_error(
|
|
request.columns,
|
|
request.rows
|
|
)
|
|
if not capture_error.is_empty():
|
|
return capture_error
|
|
var sample := SonarSample.new(
|
|
captured_sample.position_m,
|
|
captured_sample.normal,
|
|
captured_sample.intensity,
|
|
captured_sample.elevation_m,
|
|
captured_sample.material,
|
|
true
|
|
)
|
|
var write_error := buffer.set_sample(
|
|
captured_sample.row,
|
|
captured_sample.column,
|
|
sample
|
|
)
|
|
if not write_error.is_empty():
|
|
return write_error
|
|
minimum_elevation = minf(minimum_elevation, sample.elevation_m)
|
|
maximum_elevation = maxf(maximum_elevation, sample.elevation_m)
|
|
var seal_error := buffer.seal()
|
|
if not seal_error.is_empty():
|
|
return seal_error
|
|
if buffer.observed_count() == 0:
|
|
minimum_elevation = -MINIMUM_COLOR_SPAN_M * 0.5
|
|
maximum_elevation = MINIMUM_COLOR_SPAN_M * 0.5
|
|
elif maximum_elevation - minimum_elevation < MINIMUM_COLOR_SPAN_M:
|
|
var center := (minimum_elevation + maximum_elevation) * 0.5
|
|
minimum_elevation = center - MINIMUM_COLOR_SPAN_M * 0.5
|
|
maximum_elevation = center + MINIMUM_COLOR_SPAN_M * 0.5
|
|
|
|
var scan := SonarScan.new(
|
|
request.scan_id,
|
|
request.session_id,
|
|
request.sensor_transform,
|
|
request.minimum_range_m,
|
|
request.maximum_range_m,
|
|
request.horizontal_opening_radians,
|
|
request.vertical_opening_radians,
|
|
REVEAL_DURATION_SECONDS,
|
|
RECOVERY_DURATION_SECONDS,
|
|
buffer,
|
|
_cluster_contacts(request.scan_id, buffer),
|
|
minimum_elevation,
|
|
maximum_elevation,
|
|
SonarScanState.Value.REVEALING,
|
|
0.0
|
|
)
|
|
var scan_error := scan.validation_error()
|
|
return scan if scan_error.is_empty() else scan_error
|
|
|
|
|
|
func _cluster_contacts(
|
|
scan_id: StringName,
|
|
buffer: SonarSampleBuffer
|
|
) -> Array[SonarContact]:
|
|
var metal_indices: Dictionary = {}
|
|
for row in buffer.rows:
|
|
for column in buffer.columns:
|
|
var sample: SonarSample = buffer.sample_at(row, column)
|
|
if sample.observed and sample.material == SonarMaterial.Value.METAL:
|
|
metal_indices[row * buffer.columns + column] = true
|
|
|
|
var contacts: Array[SonarContact] = []
|
|
var visited: Dictionary = {}
|
|
for seed_index: int in metal_indices:
|
|
if visited.has(seed_index):
|
|
continue
|
|
var queue: Array[int] = [seed_index]
|
|
var cluster_indices: Array[int] = []
|
|
visited[seed_index] = true
|
|
while not queue.is_empty():
|
|
var index: int = queue.pop_front()
|
|
cluster_indices.append(index)
|
|
var row: int = index / buffer.columns
|
|
var column: int = index % buffer.columns
|
|
for row_offset: int in range(-1, 2):
|
|
for column_offset: int in range(-1, 2):
|
|
var neighbor_row: int = row + row_offset
|
|
var neighbor_column: int = column + column_offset
|
|
if (
|
|
neighbor_row < 0
|
|
or neighbor_row >= buffer.rows
|
|
or neighbor_column < 0
|
|
or neighbor_column >= buffer.columns
|
|
):
|
|
continue
|
|
var neighbor_index: int = neighbor_row * buffer.columns + neighbor_column
|
|
if metal_indices.has(neighbor_index) and not visited.has(neighbor_index):
|
|
visited[neighbor_index] = true
|
|
queue.append(neighbor_index)
|
|
contacts.append(
|
|
_contact_from_indices(scan_id, contacts.size(), buffer, cluster_indices)
|
|
)
|
|
return contacts
|
|
|
|
|
|
func _contact_from_indices(
|
|
scan_id: StringName,
|
|
contact_index: int,
|
|
buffer: SonarSampleBuffer,
|
|
indices: Array[int]
|
|
) -> SonarContact:
|
|
var center := Vector3.ZERO
|
|
var minimum := Vector3(INF, INF, INF)
|
|
var maximum := Vector3(-INF, -INF, -INF)
|
|
var maximum_intensity := 0.0
|
|
for index: int in indices:
|
|
var sample: SonarSample = buffer.sample_at(
|
|
index / buffer.columns,
|
|
index % buffer.columns
|
|
)
|
|
center += sample.position_m
|
|
minimum = Vector3(
|
|
minf(minimum.x, sample.position_m.x),
|
|
minf(minimum.y, sample.position_m.y),
|
|
minf(minimum.z, sample.position_m.z)
|
|
)
|
|
maximum = Vector3(
|
|
maxf(maximum.x, sample.position_m.x),
|
|
maxf(maximum.y, sample.position_m.y),
|
|
maxf(maximum.z, sample.position_m.z)
|
|
)
|
|
maximum_intensity = maxf(maximum_intensity, sample.intensity)
|
|
center /= float(indices.size())
|
|
return SonarContact.new(
|
|
StringName("%s-contact-%d" % [scan_id, contact_index]),
|
|
SonarMaterial.Value.METAL,
|
|
center,
|
|
AABB(minimum, maximum - minimum),
|
|
maximum_intensity,
|
|
indices.size()
|
|
)
|