96 lines
3.1 KiB
GDScript
96 lines
3.1 KiB
GDScript
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:
|
|
if collider.has_method("sonar_material_at"):
|
|
material = int(collider.call("sonar_material_at", world_position))
|
|
reflectivity = float(
|
|
collider.call("sonar_reflectivity_at", world_position)
|
|
)
|
|
else:
|
|
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)
|