Add production bathymetric sonar slice
This commit is contained in:
chindit committed
1 parent
a18e82eb44
commit
7613daee84
28 files changed
+1825
-87
No files matched your search
@@ -0,0 +1,13 @@
|
||||
class_name SonarAcquisitionPort
|
||||
extends RefCounted
|
||||
|
||||
const SonarCaptureRequest = preload(
|
||||
"res://game/application/sonar/sonar_capture_request.gd"
|
||||
)
|
||||
const SonarCaptureResult = preload(
|
||||
"res://game/application/sonar/sonar_capture_result.gd"
|
||||
)
|
||||
|
||||
|
||||
func capture(_request: SonarCaptureRequest) -> SonarCaptureResult:
|
||||
return SonarCaptureResult.new(false, [], "sonar acquisition is not implemented")
|
||||
@@ -0,0 +1,48 @@
|
||||
class_name SonarCaptureRequest
|
||||
extends RefCounted
|
||||
|
||||
var scan_id: StringName
|
||||
var session_id: StringName
|
||||
var sensor_transform: Transform3D
|
||||
var minimum_range_m: float
|
||||
var maximum_range_m: float
|
||||
var horizontal_opening_radians: float
|
||||
var vertical_opening_radians: float
|
||||
var columns: int
|
||||
var rows: int
|
||||
|
||||
|
||||
func _init(
|
||||
p_scan_id: StringName,
|
||||
p_session_id: StringName,
|
||||
p_sensor_transform: Transform3D,
|
||||
p_minimum_range_m: float,
|
||||
p_maximum_range_m: float,
|
||||
p_horizontal_opening_radians: float,
|
||||
p_vertical_opening_radians: float,
|
||||
p_columns: int,
|
||||
p_rows: int
|
||||
) -> void:
|
||||
scan_id = p_scan_id
|
||||
session_id = p_session_id
|
||||
sensor_transform = p_sensor_transform
|
||||
minimum_range_m = p_minimum_range_m
|
||||
maximum_range_m = p_maximum_range_m
|
||||
horizontal_opening_radians = p_horizontal_opening_radians
|
||||
vertical_opening_radians = p_vertical_opening_radians
|
||||
columns = p_columns
|
||||
rows = p_rows
|
||||
|
||||
|
||||
func validation_error() -> String:
|
||||
if scan_id.is_empty() or session_id.is_empty():
|
||||
return "sonar capture identifiers must not be empty"
|
||||
if minimum_range_m < 0.0 or maximum_range_m <= minimum_range_m:
|
||||
return "sonar capture range is invalid"
|
||||
if horizontal_opening_radians <= 0.0 or horizontal_opening_radians >= PI:
|
||||
return "sonar capture horizontal opening is invalid"
|
||||
if vertical_opening_radians <= 0.0 or vertical_opening_radians >= PI:
|
||||
return "sonar capture vertical opening is invalid"
|
||||
if columns < 2 or rows < 2:
|
||||
return "sonar capture dimensions must be at least 2 by 2"
|
||||
return ""
|
||||
@@ -0,0 +1,20 @@
|
||||
class_name SonarCaptureResult
|
||||
extends RefCounted
|
||||
|
||||
const SonarCaptureSample = preload(
|
||||
"res://game/application/sonar/sonar_capture_sample.gd"
|
||||
)
|
||||
|
||||
var accepted: bool
|
||||
var samples: Array[SonarCaptureSample]
|
||||
var message: String
|
||||
|
||||
|
||||
func _init(
|
||||
p_accepted: bool,
|
||||
p_samples: Array[SonarCaptureSample] = [],
|
||||
p_message: String = ""
|
||||
) -> void:
|
||||
accepted = p_accepted
|
||||
samples = p_samples.duplicate()
|
||||
message = p_message
|
||||
@@ -0,0 +1,50 @@
|
||||
class_name SonarCaptureSample
|
||||
extends RefCounted
|
||||
|
||||
const SonarMaterial = preload("res://game/domain/sonar/sonar_material.gd")
|
||||
|
||||
var row: int
|
||||
var column: int
|
||||
var position_m: Vector3
|
||||
var normal: Vector3
|
||||
var intensity: float
|
||||
var elevation_m: float
|
||||
var material: SonarMaterial.Value
|
||||
|
||||
|
||||
func _init(
|
||||
p_row: int,
|
||||
p_column: int,
|
||||
p_position_m: Vector3,
|
||||
p_normal: Vector3,
|
||||
p_intensity: float,
|
||||
p_elevation_m: float,
|
||||
p_material: SonarMaterial.Value
|
||||
) -> void:
|
||||
row = p_row
|
||||
column = p_column
|
||||
position_m = p_position_m
|
||||
normal = p_normal
|
||||
intensity = p_intensity
|
||||
elevation_m = p_elevation_m
|
||||
material = p_material
|
||||
|
||||
|
||||
func validation_error(columns: int, rows: int) -> String:
|
||||
if row < 0 or row >= rows or column < 0 or column >= columns:
|
||||
return "sonar capture sample coordinates are invalid"
|
||||
if not _vector_is_finite(position_m) or not _vector_is_finite(normal):
|
||||
return "sonar capture sample vectors are not finite"
|
||||
if normal.length_squared() < 0.25:
|
||||
return "sonar capture sample normal is invalid"
|
||||
if not is_finite(intensity) or intensity < 0.0 or intensity > 1.0:
|
||||
return "sonar capture sample intensity is invalid"
|
||||
if not is_finite(elevation_m):
|
||||
return "sonar capture sample elevation is invalid"
|
||||
if material == SonarMaterial.Value.UNKNOWN or not SonarMaterial.is_valid(material):
|
||||
return "sonar capture sample material is invalid"
|
||||
return ""
|
||||
|
||||
|
||||
func _vector_is_finite(value: Vector3) -> bool:
|
||||
return is_finite(value.x) and is_finite(value.y) and is_finite(value.z)
|
||||
@@ -0,0 +1,18 @@
|
||||
class_name SonarCommandResult
|
||||
extends RefCounted
|
||||
|
||||
const SonarScan = preload("res://game/domain/sonar/sonar_scan.gd")
|
||||
|
||||
var accepted: bool
|
||||
var message: String
|
||||
var scan: SonarScan
|
||||
|
||||
|
||||
func _init(
|
||||
p_accepted: bool,
|
||||
p_message: String = "",
|
||||
p_scan: SonarScan = null
|
||||
) -> void:
|
||||
accepted = p_accepted
|
||||
message = p_message
|
||||
scan = p_scan
|
||||
@@ -0,0 +1,298 @@
|
||||
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()
|
||||
)
|
||||
Reference in new issue
Block a user