72 lines
2.1 KiB
Rust
72 lines
2.1 KiB
Rust
|
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)
|
||
|
|
}
|