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

72 lines
2.1 KiB
Rust
Raw Normal View History

2023-02-06 03:52:42 +00:00
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 here: 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)
}