libredr/
camera.rs

1use nalgebra as na;
2use pyo3::prelude::*;
3use ndarray::{Axis, Array3};
4use anyhow::{Result, anyhow, ensure};
5use numpy::{PyReadonlyArray1, PyArray2, ToPyArray, PyUntypedArrayMethods};
6
7mod orthogonal;
8mod perspective;
9
10const EPS: f32 = 1e-6;
11
12/// Orthogonal camera model (Rust and Python)
13pub use self::orthogonal::{orthogonal_ray, py_orthogonal_ray};
14/// Perspective camera model (Rust and Python)
15pub use self::perspective::{perspective_ray, py_perspective_ray};
16
17/// Convert `ray_corner` (6, height + 1, width + 1) to `ray` (18, height, width)
18/// Camera model use x-right, y-down, z-forward axis scheme
19fn from_ray_corner(ray_corner: Array3<f32>) -> Result<Array3<f32>> {
20  ensure!(ray_corner.shape()[1] > 1);
21  ensure!(ray_corner.shape()[2] > 1);
22  let mut ray = Array3::zeros((18, ray_corner.shape()[1] - 1, ray_corner.shape()[2] - 1));
23  ray.axis_iter_mut(Axis(2)).enumerate().for_each(|(x, mut ray)| {
24    ray.axis_iter_mut(Axis(1)).enumerate().for_each(|(y, mut ray)| {
25      for i in 0..3 {
26        ray[i] = ray_corner[(i, y, x)];
27        ray[i + 3] = ray_corner[(i, y, x + 1)] - ray_corner[(i, y, x)];
28        ray[i + 6] = ray_corner[(i, y + 1, x)] - ray_corner[(i, y, x)];
29        ray[i + 9] = ray_corner[(i + 3, y, x)];
30        ray[i + 12] = ray_corner[(i + 3, y, x + 1)] - ray_corner[(i + 3, y, x)];
31        ray[i + 15] = ray_corner[(i + 3, y + 1, x)] - ray_corner[(i + 3, y, x)];
32      }
33    });
34  });
35  Ok(ray)
36}
37
38/// Construct 4 * 4 extrinsic matrix from `position`, `look_at`, `up` vectors
39pub fn look_at_extrinsic(
40    position: na::VectorView3<f32>,
41    look_at: na::VectorView3<f32>,
42    up: na::VectorView3<f32>) -> Result<na::Matrix4<f32>> {
43  ensure!(position.fold(true, |acc, v| { acc && v.is_finite() }),
44          "camera::look_at_extrinsic: `position` is not finite");
45  ensure!(look_at.fold(true, |acc, v| { acc && v.is_finite() }),
46          "camera::look_at_extrinsic: `look_at` is not finite");
47  ensure!(up.fold(true, |acc, v| { acc && v.is_finite() }),
48          "camera::look_at_extrinsic: `up` is not finite");
49  let d = (look_at - position).try_normalize(EPS).ok_or(anyhow!("camera::look_at_extrinsic: `d` is 0"))?;
50  let right = d.cross(&up).try_normalize(EPS).ok_or(anyhow!("camera::look_at_extrinsic: `right` is 0"))?;
51  let down = d.cross(&right).normalize();
52  let rotation = na::Matrix3::from_rows(&[right.transpose(), down.transpose(), d.transpose()]).to_homogeneous();
53  let translation = na::Matrix4::new_translation(&-position);
54  Ok(rotation * translation)
55}
56
57/// Python version [`look_at_extrinsic`]
58/// Return matrix is F-contiguous (column first order)
59#[pyfunction]
60#[pyo3(name = "look_at_extrinsic")]
61pub fn py_look_at_extrinsic<'py>(
62    py: Python<'py>,
63    position: PyReadonlyArray1<f32>,
64    look_at: PyReadonlyArray1<f32>,
65    up: PyReadonlyArray1<f32>) -> PyResult<Bound<'py, PyArray2<f32>>> {
66  let position = position.try_as_matrix().ok_or(
67    anyhow!("camera::look_at_extrinsic: `position` expected shape {:?}, found {:?}", [3], position.shape()))?;
68  let look_at = look_at.try_as_matrix().ok_or(
69    anyhow!("camera::look_at_extrinsic: `look_at` expected shape {:?}, found {:?}", [3], look_at.shape()))?;
70  let up = up.try_as_matrix().ok_or(
71    anyhow!("camera::look_at_extrinsic: `up` expected shape {:?}, found {:?}", [3], up.shape()))?;
72  let extrinsic = look_at_extrinsic(position, look_at, up)?;
73  Ok(extrinsic.to_pyarray(py))
74}
75
76/// All camera models (Python)
77pub fn py_camera<'py>(py: Python<'py>, parent_module: &Bound<'py, PyModule>) -> PyResult<()> {
78  let module = PyModule::new(py, "camera")?;
79  module.add_function(wrap_pyfunction!(py_look_at_extrinsic, &module)?)?;
80  module.add_function(wrap_pyfunction!(py_orthogonal_ray, &module)?)?;
81  module.add_function(wrap_pyfunction!(py_perspective_ray, &module)?)?;
82  parent_module.add_submodule(&module)?;
83  Ok(())
84}
libredr/camera.rs

libredr/
camera.rs
