use std::ffi::{c_char, c_void, CString};
use std::path::{Path, PathBuf};
use std::sync::{Mutex, OnceLock};
use libloading::{Library, Symbol};
use crate::error::{Error, Result};
use crate::pointcloud::Attr;
use crate::shots::reconstruct::{reconstruct_misses, MissReport};
use crate::{Point, PointCloud, Shots};
#[repr(C)]
#[derive(Clone, Copy, Default)]
struct Xyz32 {
x: f32,
y: f32,
z: f32,
}
#[repr(C)]
#[derive(Clone, Copy, Default)]
struct Attributes {
amplitude: f32,
reflectance: f32,
deviation: u16,
flags: u16,
background_radiation: f32,
}
type Handle = *mut c_void;
type OpenFn = unsafe extern "C" fn(*const c_char, i32, *mut Handle) -> i32;
type ReadFn = unsafe extern "C" fn(Handle, u32, *mut Xyz32, *mut Attributes, *mut u64, *mut u32, *mut i32) -> i32;
type CloseFn = unsafe extern "C" fn(Handle) -> i32;
type LastErrorFn = unsafe extern "C" fn(*mut c_char, u32, *mut u32) -> i32;
pub const ECHO_SINGLE: u16 = 0;
pub const ECHO_FIRST: u16 = 1;
pub const ECHO_INTERIOR: u16 = 2;
pub const ECHO_LAST: u16 = 3;
pub(crate) const FLAG_PSEUDO_ECHO: u16 = 1 << 4;
static LIBRARY: OnceLock<Mutex<Option<Library>>> = OnceLock::new();
pub fn search_roots() -> Vec<PathBuf> {
let mut roots = Vec::new();
for var in ["RIVLIB_PATH", "RIVLIB_HOME"] {
if let Ok(v) = std::env::var(var) {
roots.push(PathBuf::from(v));
}
}
if let Some(home) = std::env::var_os("HOME").map(PathBuf::from) {
roots.push(home.join(".local/lib"));
roots.push(home.join(".local"));
roots.push(home.join("lib"));
roots.push(home.join("opt"));
roots.push(home);
}
roots.push(PathBuf::from("/opt"));
roots.push(PathBuf::from("/usr/local/lib"));
roots.push(PathBuf::from("/usr/local"));
roots.push(PathBuf::from("/usr/lib"));
roots
}
const LIB_NAMES: &[&str] = &["libscanifc-mt.so", "libscanifc.so", "scanifc-mt.dll", "libscanifc-mt.dylib"];
pub(crate) fn search_dir(dir: &Path, depth: usize, names: &[&str]) -> Option<PathBuf> {
if !dir.is_dir() {
return None;
}
for name in names {
let p = dir.join(name);
if p.is_file() {
return Some(p);
}
}
if depth == 0 {
return None;
}
let mut entries: Vec<PathBuf> = std::fs::read_dir(dir).ok()?.flatten().map(|e| e.path()).filter(|p| p.is_dir()).collect();
entries.sort();
for e in entries {
let n = e.file_name().and_then(|n| n.to_str()).unwrap_or("");
if n.to_ascii_lowercase().contains("rivlib") || n == "lib" || depth >= 2 {
if let Some(p) = search_dir(&e, depth - 1, names) {
return Some(p);
}
}
}
None
}
pub fn find_rivlib(hint: Option<&Path>) -> Result<PathBuf> {
if let Some(h) = hint {
if h.is_file() {
return Ok(h.to_path_buf());
}
if let Some(p) = search_dir(h, 3, LIB_NAMES) {
return Ok(p);
}
}
for root in search_roots() {
if root.is_file() {
return Ok(root);
}
if let Some(p) = search_dir(&root, 3, LIB_NAMES) {
return Ok(p);
}
}
Err(Error::Riegl(
"libscanifc not found. Download RiVLib from RIEGL and set RIVLIB_PATH to the extracted \
directory (containing lib/libscanifc-mt.so)"
.into(),
))
}
fn with_library<T>(hint: Option<&Path>, f: impl FnOnce(&Library) -> Result<T>) -> Result<T> {
let cell = LIBRARY.get_or_init(|| Mutex::new(None));
let mut guard = cell.lock().unwrap();
if guard.is_none() {
let path = find_rivlib(hint)?;
let lib = unsafe { Library::new(&path) }.map_err(|e| Error::Riegl(format!("{}: {e}", path.display())))?;
*guard = Some(lib);
}
f(guard.as_ref().unwrap())
}
#[derive(Debug, Clone)]
pub struct RxpOptions {
pub library: Option<PathBuf>,
pub drop_pseudo_echoes: bool,
pub min_range: f64,
pub max_range: f64,
pub stride: usize,
pub shot_stride: usize,
pub max_points: Option<usize>,
pub echoes: String,
pub mta: super::rxp_mta::MtaOptions,
pub fill_missing: bool,
pub zenith_range: Option<(f64, f64)>,
}
impl Default for RxpOptions {
fn default() -> Self {
RxpOptions {
library: None,
drop_pseudo_echoes: true,
min_range: 0.5,
max_range: f64::INFINITY,
stride: 1,
shot_stride: 1,
max_points: None,
echoes: "all".into(),
mta: Default::default(),
fill_missing: false,
zenith_range: None,
}
}
}
struct RawRxp {
xyz: Vec<Point>,
amplitude: Vec<f32>,
reflectance: Vec<f32>,
deviation: Vec<u16>,
flags: Vec<u16>,
time_ns: Vec<u64>,
misses: Vec<Point>,
report: Option<MissReport>,
}
fn read_raw(path: &Path, opts: &RxpOptions) -> Result<RawRxp> {
with_library(opts.library.as_deref(), |lib| {
let (open, read, close, last_error) = unsafe {
let open: Symbol<OpenFn> = lib.get(b"scanifc_point3dstream_open\0").map_err(|e| Error::Riegl(e.to_string()))?;
let read: Symbol<ReadFn> = lib.get(b"scanifc_point3dstream_read\0").map_err(|e| Error::Riegl(e.to_string()))?;
let close: Symbol<CloseFn> = lib.get(b"scanifc_point3dstream_close\0").map_err(|e| Error::Riegl(e.to_string()))?;
let last_error: Symbol<LastErrorFn> = lib.get(b"scanifc_get_last_error\0").map_err(|e| Error::Riegl(e.to_string()))?;
(open, read, close, last_error)
};
let describe = |code: i32| -> Error {
let mut buf = vec![0u8; 1024];
let mut size = 0u32;
unsafe { last_error(buf.as_mut_ptr() as *mut c_char, buf.len() as u32, &mut size) };
let msg = String::from_utf8_lossy(&buf[..(size as usize).min(buf.len())]).trim_end_matches('\0').to_string();
Error::Riegl(format!("{path:?}: scanifc error {code}: {msg}", path = path.display()))
};
let uri = CString::new(format!("file:{}", path.canonicalize().unwrap_or(path.to_path_buf()).display()))
.map_err(|e| Error::Riegl(e.to_string()))?;
let mut handle: Handle = std::ptr::null_mut();
let rc = unsafe { open(uri.as_ptr(), 0, &mut handle) };
if rc != 0 {
return Err(describe(rc));
}
const CHUNK: usize = 1 << 18;
let mut xyz_buf = vec![Xyz32::default(); CHUNK];
let mut attr_buf = vec![Attributes::default(); CHUNK];
let mut time_buf = vec![0u64; CHUNK];
let mut raw = RawRxp { xyz: Vec::new(), amplitude: Vec::new(), reflectance: Vec::new(), deviation: Vec::new(), flags: Vec::new(), time_ns: Vec::new(), misses: Vec::new(), report: None };
let mut counter = 0usize;
let mut pulse = 0usize;
let mut last_time = u64::MAX;
let mut registered = false;
let fill = opts.fill_missing;
let (mut pulse_time, mut pulse_dir, mut echo_pulse): (Vec<u64>, Vec<Point>, Vec<u32>) = (Vec::new(), Vec::new(), Vec::new());
loop {
let mut got = 0u32;
let mut eof = 0i32;
let rc = unsafe { read(handle, CHUNK as u32, xyz_buf.as_mut_ptr(), attr_buf.as_mut_ptr(), time_buf.as_mut_ptr(), &mut got, &mut eof) };
if rc != 0 {
unsafe { close(handle) };
return Err(describe(rc));
}
let n = got as usize;
for i in 0..n {
counter += 1;
if time_buf[i] != last_time {
last_time = time_buf[i];
pulse += 1;
registered = false;
}
let a = attr_buf[i];
let p = xyz_buf[i];
let r = ((p.x as f64).powi(2) + (p.y as f64).powi(2) + (p.z as f64).powi(2)).sqrt();
if opts.drop_pseudo_echoes && a.flags & FLAG_PSEUDO_ECHO != 0 {
continue;
}
if r < opts.min_range {
continue;
}
if fill && !registered {
pulse_time.push(time_buf[i]);
pulse_dir.push([p.x as f64, p.y as f64, p.z as f64]);
registered = true;
}
if !fill && opts.shot_stride > 1 && pulse % opts.shot_stride != 0 {
continue;
}
if opts.stride > 1 && counter % opts.stride != 0 {
continue;
}
if r > opts.max_range {
continue;
}
let echo = a.flags & 3;
let keep = match opts.echoes.as_str() {
"first" => echo == ECHO_FIRST || echo == ECHO_SINGLE,
"last" => echo == ECHO_LAST || echo == ECHO_SINGLE,
"single" => echo == ECHO_SINGLE,
_ => true,
};
if !keep {
continue;
}
raw.xyz.push([p.x as f64, p.y as f64, p.z as f64]);
raw.amplitude.push(a.amplitude);
raw.reflectance.push(a.reflectance);
raw.deviation.push(a.deviation);
raw.flags.push(a.flags);
raw.time_ns.push(time_buf[i]);
if fill {
echo_pulse.push((pulse_time.len() - 1) as u32);
}
if opts.max_points.map(|m| raw.xyz.len() >= m).unwrap_or(false) {
break;
}
}
if n == 0 && eof == 0 {
break;
}
if opts.max_points.map(|m| raw.xyz.len() >= m).unwrap_or(false) {
break;
}
}
unsafe { close(handle) };
if fill {
let rec = reconstruct_misses(&pulse_time, &pulse_dir, opts.zenith_range, opts.shot_stride);
drop(pulse_time);
drop(pulse_dir);
if opts.shot_stride > 1 {
let keep: Vec<bool> = echo_pulse.iter().map(|&p| rec.keep[p as usize]).collect();
let mut k = keep.iter();
raw.xyz.retain(|_| *k.next().unwrap());
let mut k = keep.iter();
raw.amplitude.retain(|_| *k.next().unwrap());
let mut k = keep.iter();
raw.reflectance.retain(|_| *k.next().unwrap());
let mut k = keep.iter();
raw.deviation.retain(|_| *k.next().unwrap());
let mut k = keep.iter();
raw.flags.retain(|_| *k.next().unwrap());
let mut k = keep.iter();
raw.time_ns.retain(|_| *k.next().unwrap());
}
raw.misses = rec.misses;
raw.report = Some(rec.report);
}
Ok(raw)
})
}
pub fn read_rxp(path: impl AsRef<Path>, opts: &RxpOptions) -> Result<PointCloud> {
if mta_active(opts) {
return super::rxp_mta::read_rxp_mta(path.as_ref(), opts, &opts.mta);
}
Ok(cloud_from_raw(read_raw(path.as_ref(), opts)?))
}
fn mta_active(opts: &RxpOptions) -> bool {
use super::rxp_mta::{find_shim, MtaMode};
match opts.mta.mode {
MtaMode::Off => false,
MtaMode::On => true,
MtaMode::Auto => find_shim(opts.mta.shim.as_deref()).is_ok(),
}
}
fn cloud_from_raw(raw: RawRxp) -> PointCloud {
let mut cloud = PointCloud::new(raw.xyz);
cloud.attrs.insert("amplitude".into(), Attr::F32(raw.amplitude));
cloud.attrs.insert("reflectance".into(), Attr::F32(raw.reflectance));
cloud.attrs.insert("deviation".into(), Attr::U16(raw.deviation));
cloud.attrs.insert("echo_type".into(), Attr::U8(raw.flags.iter().map(|f| (f & 3) as u8).collect()));
cloud.attrs.insert("gps_time".into(), Attr::F64(raw.time_ns.iter().map(|&t| t as f64 * 1e-9).collect()));
cloud
}
pub(crate) fn pulse_contiguous(cloud: PointCloud) -> PointCloud {
let (Some(line), Some(shot)) = (cloud.attr_f64("scan_line_index"), cloud.attr_f64("shot_index_line")) else {
return cloud;
};
let key = |i: usize| (-(line[i] as i64), shot[i] as i64);
if (1..cloud.len()).all(|i| key(i - 1) <= key(i)) {
return cloud;
}
let mut idx: Vec<usize> = (0..cloud.len()).collect();
idx.sort_by_key(|&i| key(i));
cloud.take(&idx)
}
pub fn read_rxp_shots_with_report(path: impl AsRef<Path>, opts: &RxpOptions) -> Result<(Shots, Option<MissReport>)> {
if mta_active(opts) {
if !opts.fill_missing {
let cloud = pulse_contiguous(read_rxp(path, opts)?);
return Ok((Shots::from_pointcloud(&cloud, [0.0, 0.0, 0.0]), None));
}
let mut all = opts.clone();
all.shot_stride = 1;
all.stride = 1;
all.max_points = None;
all.echoes = "all".into();
let cloud = pulse_contiguous(super::rxp_mta::read_rxp_mta(path.as_ref(), &all, &all.mta)?);
let shots = Shots::from_pointcloud(&cloud, [0.0, 0.0, 0.0]);
let times = shots.echo_attrs.get("gps_time").map(|a| a.to_f64()).ok_or_else(|| Error::invalid("the MTA stream carries no gps_time"))?;
let time_ns: Vec<u64> = shots.echo_start.iter().map(|&s| (times[s] * 1e9).round().max(0.0) as u64).collect();
let rec = reconstruct_misses(&time_ns, &shots.direction, opts.zenith_range, opts.shot_stride);
let kept = if opts.shot_stride > 1 { shots.subset(&rec.keep) } else { shots };
return with_misses(kept, rec.misses).map(|s| (s, Some(rec.report)));
}
let mut raw = read_raw(path.as_ref(), opts)?;
let misses = std::mem::take(&mut raw.misses);
let report = raw.report.take();
let cloud = cloud_from_raw(raw);
let shots = Shots::from_pointcloud(&cloud, [0.0, 0.0, 0.0]);
with_misses(shots, misses).map(|s| (s, report))
}
fn with_misses(shots: Shots, misses: Vec<Point>) -> Result<Shots> {
if misses.is_empty() {
return Ok(shots);
}
let n = misses.len();
let empty = Shots {
origin: vec![[0.0, 0.0, 0.0]; n],
direction: misses,
echo_start: vec![0; n],
echo_count: vec![0; n],
echo_range: Vec::new(),
echo_attrs: shots.echo_attrs.iter().map(|(k, v)| (k.clone(), v.take(&[]))).collect(),
};
Shots::concatenate(&[&shots, &empty])
}
pub fn read_rxp_shots(path: impl AsRef<Path>, opts: &RxpOptions) -> Result<Shots> {
read_rxp_shots_with_report(path, opts).map(|(s, _)| s)
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn mta_echoes_are_gathered_into_their_pulses() {
let rows: [(i32, i32, f64, f64); 7] = [
(-1, 7, 1.0, 10.0),
(-1, 8, 2.0, 5.0),
(-1, 7, 1.0, 90.0), (-1, 8, 2.0, 30.0),
(-2, 0, 3.0, 4.0),
(-1, 8, 2.0, 95.0), (-2, 1, 4.0, 6.0),
];
let mut cloud = PointCloud::new(rows.iter().map(|r| [0.0, 0.0, r.3]).collect());
cloud.attrs.insert("scan_line_index".into(), Attr::I32(rows.iter().map(|r| r.0).collect()));
cloud.attrs.insert("shot_index_line".into(), Attr::I32(rows.iter().map(|r| r.1).collect()));
cloud.attrs.insert("gps_time".into(), Attr::F64(rows.iter().map(|r| r.2).collect()));
assert_eq!(Shots::from_pointcloud(&cloud, [0.0; 3]).n_shots(), 7);
let sorted = pulse_contiguous(cloud.clone());
assert_eq!(sorted.attr_f64("gps_time").unwrap(), vec![1.0, 1.0, 2.0, 2.0, 2.0, 3.0, 4.0]);
let s = Shots::from_pointcloud(&sorted, [0.0; 3]);
assert_eq!(s.echo_count, vec![2, 3, 1, 1]);
assert_eq!(s.echo_range, vec![10.0, 90.0, 5.0, 30.0, 95.0, 4.0, 6.0]);
assert_eq!(pulse_contiguous(sorted.clone()), sorted);
let mut plain = cloud.clone();
plain.attrs.remove("scan_line_index");
assert_eq!(pulse_contiguous(plain.clone()), plain);
}
}