csci5607/assignment-1b/src/utils.rs

use anyhow::Result;
use nalgebra::{Matrix3, Vector3};
use ordered_float::NotNan;

/// Finds the minimum of an iterator of f64s, ignoring any NaN values
pub fn min_f64<I>(i: I) -> Option<f64>
where
  I: Iterator<Item = f64>,
{
  i.filter_map(|i| NotNan::new(i).ok())
    .min()
    .map(|i| i.into_inner())
}

/// Finds the minimum of an iterator of f64s using the given predicate, ignoring
/// any NaN values
pub fn min_f64_by_key<I, F>(i: I, f: F) -> Option<f64>
where
  I: Iterator<Item = f64>,
  F: FnMut(&NotNan<f64>),
{
  i.filter_map(|i| NotNan::new(i).ok())
    .min_by_key(f)
    .map(|i| i.into_inner())
}

/// Calculate the rotation matrix between the 2 given vectors
///
/// Based on the method given [here][1].
///
/// [1]: https://math.stackexchange.com/a/897677
pub fn compute_rotation_matrix(
  a: Vector3<f64>,
  b: Vector3<f64>,
) -> Result<Matrix3<f64>> {
  // Special case: if a and b are in the same direction, just return the identity matrix
  if a.normalize() == b.normalize() {
    return Ok(Matrix3::identity())
  }

  let cos_t = a.dot(&b);
  let sin_t = a.cross(&b).norm();

  let g = Matrix3::new(cos_t, -sin_t, 0.0, sin_t, cos_t, 0.0, 0.0, 0.0, 1.0);

  // New basis vectors
  let u = a;
  let v = (b - a.dot(&b) * a).normalize();
  let w = b.cross(&a);

  // Not sure if this is required to be invertible?
  let f_inverse = Matrix3::from_columns(&[u, v, w]);
  let f = match f_inverse.try_inverse() {
    Some(v) => v,
    None => {
      // So I ran into this case trying to compute the rotation matrix where one
      // of the vector endpoints was (0, 0, 0). I'm pretty sure this case makes
      // no sense in reality, so going to just error out here and screw
      // recovering.
      //
      // println!("Failed to compute inverse matrix.");
      // println!("- Initial: a = {a}, b = {b}");
      // println!("- cos(t) = {cos_t}, sin(t) = {sin_t}");
      // println!("- Basis: u = {u}, v = {v}, w = {w}");
      bail!("Failed to compute inverse matrix of {f_inverse}\na = {a}\nb = {b}")
    }
  };

  // if (f_inverse * g * f).norm() != 1.0 {
  //   bail!("WTF {}", (f_inverse * g * f).norm());
  // }

  Ok(f_inverse * g * f)
}