Extract iteration limiting into a function

src/bin/flat/float_fun.rs

@@ -1,40 +0,0 @@
-use glam::FloatExt;
-
-mod bounds {
-	pub trait Pair<T> {}
-
-	impl<T> Pair<T> for (T, T) {}
-}
-
-pub trait FloatExt2<T>: bounds::Pair<T> {
-	fn lerp(self, t: T) -> T;
-	fn inverse_lerp(self, y: T) -> T;
-}
-
-impl<F: FloatExt> FloatExt2<F> for (F, F) {
-	fn lerp(self, t: F) -> F {
-		F::lerp(self.0, self.1, t)
-	}
-	fn inverse_lerp(self, y: F) -> F {
-		F::inverse_lerp(self.0, self.1, y)
-	}
-}
-
-#[cfg(test)]
-mod test {
-	use super::FloatExt2;
-
-	#[test]
-	fn test_lerp() {
-		assert_eq!((3., 7.).lerp(-0.5), 1.);
-		assert_eq!((3., 7.).lerp(0.0), 3.);
-		assert_eq!((3., 7.).lerp(0.5), 5.);
-		assert_eq!((3., 7.).lerp(1.0), 7.);
-		assert_eq!((3., 7.).lerp(1.5), 9.);
-		assert_eq!((3., 7.).inverse_lerp(1.), -0.5);
-		assert_eq!((3., 7.).inverse_lerp(3.), 0.0);
-		assert_eq!((3., 7.).inverse_lerp(5.), 0.5);
-		assert_eq!((3., 7.).inverse_lerp(7.), 1.0);
-		assert_eq!((3., 7.).inverse_lerp(9.), 1.5);
-	}
-}

src/bin/flat/fns.rs

@@ -1,4 +1,4 @@
-use crate::float_fun::FloatExt2;
+use refraction::mathx::FloatExt2;
 
 pub trait Limiter {
 	fn value(&self, x: f32) -> f32;

src/bin/flat/ifaces.rs

@@ -0,0 +1,29 @@
+use crate::types::{Hit, Location, Ray};
+use glam::Vec2;
+
+pub trait Traceable {
+	/// Traces a ray from a given starting point. `ray` is relative to the camera.
+	///
+	/// Returns all objects the ray touched. `Hit::distance` is along the ray; it may be a rough estimate except, when two objects overlap the difference of corresponding `distance`s shall be correct.
+	fn trace(&self, camera: Location, ray: Ray) -> Vec<Hit>;
+}
+
+pub trait OptimizedTraceable: Traceable {
+	type State;
+
+	/// Prepares tracing from a given starting point. `ray` is relative to the camera.
+	fn init(&self, camera: Location, ray: Ray) -> Self::State;
+
+	/// Similar to [`Traceable::trace`] but allows stopping early.
+	fn trace(&self, state: Self::State) -> (Option<Self::State>, Vec<Hit>);
+}
+
+pub struct RayPath {
+	pub points: Vec<Vec2>,
+	pub end_dir: Vec2,
+}
+
+pub trait DebugTraceable: Traceable {
+	/// Identical to [`Traceable::trace`], except also returns the ray path.
+	fn trace_dbg(&self, camera: Location, ray: Ray) -> (Vec<Hit>, RayPath);
+}

src/bin/flat/main.rs

@@ -4,15 +4,17 @@ use flo_canvas::*;
 use flo_draw::*;
 use glam::*;
 
+use crate::ifaces::{DebugTraceable, Traceable};
 use crate::types::FlatTraceResult;
+use refraction::mathx::MatExt;
 use riemann::{trace_iter, Metric};
 use tube::metric::Tube;
 use tube::Space;
 use tube::Subspace::{Boundary, Inner, Outer};
 use types::{Location, Object, Ray};
 
-mod float_fun;
 mod fns;
+mod ifaces;
 mod riemann;
 mod tube;
 mod types;
@@ -48,19 +50,26 @@ pub fn main() {
 				.enumerate()
 				.map(|(k, &y)| Object {
 					id: k as i32,
-					loc: {
-						let pos = vec2(0.0, y * tube.external_halflength);
-						let adj: Mat2 = tube.sqrt_at(pos).inverse().into();
-						let rot = Mat2::from_angle(y);
-						Location {
-							pos,
-							rot: adj * rot,
-						}
-					},
+					loc: put_object(
+						&tube,
+						vec2(0.0, y * tube.external_halflength),
+						Mat2::from_angle(y),
+					),
 					r: 20.0,
 				})
 				.collect();
 			let space = Space { tube, objs };
+			let cam1 = put_object(&space.tube, vec2(-500., 0.), Mat2::IDENTITY);
+			let cam2 = put_object(
+				&space.tube,
+				vec2(-2.5 * tube.outer_radius, 1.25 * tube.external_halflength),
+				mat2(vec2(1., -1.), vec2(1., 1.)),
+			);
+			let cam3 = put_object(
+				&space.tube,
+				vec2(0.25 * tube.inner_radius, 0.25 * tube.external_halflength),
+				mat2(vec2(0., -1.), vec2(1., 0.)),
+			);
 
 			gc.canvas_height(500.0);
 			gc.transform(Transform2D::rotate(FRAC_PI_2));
@@ -68,16 +77,13 @@ pub fn main() {
 			gc.line_width(0.5);
 			// gc.stroke_color(Color::Rgba(1.0, 0.5, 0.0, 0.5));
 			// draw_fan(gc, &tube, vec2(-500.0, 0.0), vec2(1.0, 0.0), 1.0);
-			gc.stroke_color(Color::Rgba(1.0, 0.5, 0.0, 1.0));
-			draw_fan_2(gc, &space, vec2(-500.0, 0.0), vec2(1.0, 0.0), 1.0);
+			gc.stroke_color(Color::Rgba(0.0, 0.8, 1.0, 1.0));
+			draw_fan_2(gc, &space, cam3, 1.0);
 			gc.stroke_color(Color::Rgba(0.5, 1.0, 0.0, 1.0));
-			draw_fan_2(
-				gc,
-				&space,
-				vec2(-2.5 * tube.outer_radius, 1.25 * tube.external_halflength),
-				vec2(1.0, -1.0),
-				1.0,
-			);
+			draw_fan_2(gc, &space, cam2, 1.0);
+			gc.stroke_color(Color::Rgba(1.0, 0.5, 0.0, 1.0));
+			draw_fan_2(gc, &space, cam1, 1.0);
+
 			draw_track(gc, &space, vec2(-500.0, 0.0), vec2(1.0, 0.2));
 			draw_track(gc, &space, vec2(-500.0, 0.0), vec2(1.0, 0.5));
 			draw_track(
@@ -146,6 +152,44 @@ fn rel_to_abs(space: &impl Metric, base: &Location, rel: Vec2, steps: usize) ->
 		.unwrap()
 }
 
+/// Converts a position and a rotation to a [Location]. Only the X direction is preserved from `rot` to ensure the resulting Location describes an orthonormal coordinate system.
+fn put_object(space: &impl Metric, pos: Vec2, rot: Mat2) -> Location {
+	let metric_sqrt = space.sqrt_at(pos);
+	let metric_inv_sqrt = space.sqrt_at(pos).inverse();
+	let rot = metric_inv_sqrt * (metric_sqrt * rot).orthonormalize();
+	Location { pos, rot }
+}
+
+#[test]
+fn test_put_object() {
+	use approx::assert_abs_diff_eq;
+
+	let ε = 1e-5;
+	let m = riemann::samples::ScaledMetric {
+		scale: vec2(3., 4.),
+	};
+
+	let loc = put_object(&m, vec2(1., 2.), mat2(vec2(1., 0.), vec2(0., 1.)));
+	assert_eq!(loc.pos, vec2(1., 2.));
+	assert_abs_diff_eq!(loc.rot * vec2(1., 0.), vec2(1. / 3., 0.), epsilon = ε);
+	assert_abs_diff_eq!(loc.rot * vec2(0., 1.), vec2(0., 1. / 4.), epsilon = ε);
+
+	let loc = put_object(&m, vec2(1., 2.), mat2(vec2(0., 1.), vec2(-1., 0.)));
+	assert_eq!(loc.pos, vec2(1., 2.));
+	assert_abs_diff_eq!(loc.rot * vec2(1., 0.), vec2(0., 1. / 4.), epsilon = ε);
+	assert_abs_diff_eq!(loc.rot * vec2(0., 1.), vec2(-1. / 3., 0.), epsilon = ε);
+
+	let c = 0.5 * std::f32::consts::SQRT_2;
+	let loc = put_object(&m, vec2(1., 2.), mat2(vec2(c, c), vec2(-c, c)));
+	assert_eq!(loc.pos, vec2(1., 2.));
+	assert_abs_diff_eq!(loc.rot * vec2(1., 0.), vec2(1. / 5., 1. / 5.), epsilon = ε);
+	assert_abs_diff_eq!(
+		loc.rot * vec2(0., 1.),
+		vec2(-4. / 15., 3. / 20.),
+		epsilon = ε
+	);
+}
+
 fn draw_cross(gc: &mut Vec<Draw>, pos: Vec2, r: f32) {
 	gc.move_to(pos.x - r, pos.y - r);
 	gc.line_to(pos.x + r, pos.y + r);
@@ -153,92 +197,36 @@ fn draw_cross(gc: &mut Vec<Draw>, pos: Vec2, r: f32) {
 	gc.line_to(pos.x + r, pos.y - r);
 }
 
-fn draw_ray_2(gc: &mut Vec<Draw>, space: &Space, base: Vec2, dir: Vec2) {
-	fn trace_to_flat(gc: &mut Vec<Draw>, space: &Space, ray: Ray) -> (Ray, FlatTraceResult) {
-		for ray in space.trace_iter(ray).skip(1) {
-			gc.line_to(ray.pos.x, ray.pos.y);
-			match space.which_subspace(ray.pos) {
-				Inner => return (ray, space.trace_inner(ray)),
-				Outer => return (ray, space.trace_outer(ray)),
-				Boundary => continue,
-			};
-		}
-		unreachable!("Space::trace_iter terminated!")
+fn draw_ray_2(gc: &mut Vec<Draw>, space: &Space, camera: Location, dir: Vec2) {
+	let pos = vec2(0., 0.);
+	let (hits, path) = space.trace_dbg(camera, Ray { pos, dir });
+	let hits2 = space.trace(camera, Ray { pos, dir });
+	for (a, b) in hits.into_iter().zip(hits2.into_iter()) {
+		assert_eq!(a.id, b.id);
+		assert_eq!(a.pos, b.pos);
+		assert_eq!(a.rel, b.rel);
 	}
 
-	let mut hits = Vec::<Draw>::new();
-	let dir = space.tube.globalize(base, dir);
 	gc.new_path();
-	gc.move_to(base.x, base.y);
-	let mut ray = Ray {
-		pos: base,
-		dir: space.tube.normalize_vec_at(base, dir) * DT,
-	};
-	for _ in 0..100 {
-		let ret;
-		(ray, ret) = trace_to_flat(gc, space, ray);
-		gc.stroke();
-		gc.new_dash_pattern();
-		// gc.dash_length(6.0);
-		gc.new_path();
-		gc.move_to(ray.pos.x, ray.pos.y);
-		for hit in ret.objects {
-			let obj = space.objs[hit.id as usize];
-			let apx_hit_pos = rel_to_abs(&space.tube, &obj.loc, hit.rel.pos, 128);
-			// assert_abs_diff_eq!(apx_hit_pos, hit.pos, epsilon=1.0);
-			let Ray { pos: rel, dir } = hit.rel;
-			let diff = rel.dot(dir).powi(2)
-				- dir.length_squared() * (rel.length_squared() - obj.r.powi(2));
-			assert!(diff >= 0.0);
-			let t = (-rel.dot(dir) + diff.sqrt()) / dir.length_squared();
-			let rel2 = hit.rel.forward(t).pos;
-			let pos2 = rel_to_abs(&space.tube, &obj.loc, rel2, 128);
-			draw_cross(&mut hits, pos2, 1.0);
-		}
-		let a = ray.pos;
-		if let Some(r) = ret.end {
-			ray = r
-		} else {
-			ray = ray.forward(1000.0 / DT);
-			gc.line_to(ray.pos.x, ray.pos.y);
-			break;
-		}
-		for p in space.line(a, ray.pos, 10.0) {
-			gc.line_to(p.x, p.y);
-		}
-		gc.stroke();
-		gc.new_dash_pattern();
-		gc.new_path();
-		gc.move_to(ray.pos.x, ray.pos.y);
-	}
-	gc.stroke();
-	gc.new_path();
-	gc.new_dash_pattern();
-	gc.append(&mut hits);
-	gc.stroke();
-}
-
-fn draw_fan_2(gc: &mut Vec<Draw>, space: &Space, base: Vec2, dir: Vec2, spread: f32) {
-	let dir = dir.normalize();
-	let v = vec2(-dir.y, dir.x);
-	for y in itertools_num::linspace(-spread, spread, 101) {
-		draw_ray_2(gc, space, base, dir + y * v);
-	}
-}
-
-fn draw_ray(gc: &mut Vec<Draw>, space: &impl Metric, base: Vec2, dir: Vec2) {
-	let dir = space.globalize(base, dir);
-	gc.new_path();
-	gc.move_to(base.x, base.y);
-	for pt in trace_iter(space, base, dir, DT).take(10000) {
+	gc.move_to(pos.x, pos.y);
+	for pt in &path.points[1..] {
 		gc.line_to(pt.x, pt.y);
-		if pt.abs().cmpgt(Vec2::splat(1000.0)).any() {
-			break;
-		}
 	}
+	let end_pos = *path
+		.points
+		.last()
+		.expect("the starting point is always in the path");
+	let dir_pos = end_pos + 1000.0 * path.end_dir;
+	gc.line_to(dir_pos.x, dir_pos.y);
 	gc.stroke();
 }
 
+fn draw_fan_2(gc: &mut Vec<Draw>, space: &Space, camera: Location, spread: f32) {
+	for y in itertools_num::linspace(-spread, spread, 101) {
+		draw_ray_2(gc, space, camera, vec2(1., y));
+	}
+}
+
 fn draw_track(gc: &mut Vec<Draw>, space: &Space, start: Vec2, dir: Vec2) {
 	const SCALE: f32 = 5.0;
 	const STEP: f32 = 2.0 * SCALE;
@@ -275,14 +263,6 @@ fn draw_track(gc: &mut Vec<Draw>, space: &Space, start: Vec2, dir: Vec2) {
 	}
 }
 
-fn draw_fan(gc: &mut Vec<Draw>, space: &impl Metric, base: Vec2, dir: Vec2, spread: f32) {
-	let dir = dir.normalize();
-	let v = vec2(-dir.y, dir.x);
-	for y in itertools_num::linspace(-spread, spread, 101) {
-		draw_ray(gc, space, base, dir + y * v);
-	}
-}
-
 trait Renderable {
 	fn render(&self, gc: &mut Vec<Draw>);
 }

src/bin/flat/riemann.rs

@@ -22,6 +22,17 @@ impl Decomp2 {
 	}
 }
 
+impl<T> std::ops::Mul<T> for Decomp2
+where
+	Mat2: std::ops::Mul<T>,
+{
+	type Output = <Mat2 as std::ops::Mul<T>>::Output;
+
+	fn mul(self, rhs: T) -> Self::Output {
+		Mat2::from(self) * rhs
+	}
+}
+
 impl From<Decomp2> for Mat2 {
 	fn from(value: Decomp2) -> Self {
 		value.ortho.transpose() * Mat2::from_diagonal(value.diag) * value.ortho
@@ -52,10 +63,6 @@ pub trait Metric {
 	fn normalize_vec_at(&self, at: Vec2, v: Vec2) -> Vec2 {
 		v / self.vec_length_at(at, v)
 	}
-
-	fn globalize(&self, at: Vec2, v: Vec2) -> Vec2 {
-		Mat2::from(self.sqrt_at(at).inverse()) * v
-	}
 }
 
 pub struct TraceIter<'a, M: Metric> {
@@ -156,6 +163,7 @@ pub mod samples {
 	use super::{Decomp2, Metric};
 
 	pub struct ScaledMetric {
+		/// Specifies unit size in each cardinal direction. E.g. with scale=(2, 3), vector (1, 0) has length 2 while a unit vector with the same direction is (1/2, 0).
 		pub scale: Vec2,
 	}
 
@@ -214,12 +222,6 @@ mod tests {
 			metric.normalize_vec_at(rng.gen(), vec2(1., 1.)),
 			vec2(1. / 5., 1. / 5.)
 		);
-		assert_eq!(metric.globalize(rng.gen(), vec2(1., 0.)), vec2(1. / 3., 0.));
-		assert_eq!(metric.globalize(rng.gen(), vec2(0., 1.)), vec2(0., 1. / 4.));
-		assert_eq!(
-			metric.globalize(rng.gen(), vec2(1., 1.)),
-			vec2(1. / 3., 1. / 4.)
-		);
 	}
 
 	#[test]

src/bin/flat/tube/mod.rs

@@ -1,12 +1,14 @@
 use glam::{bool, f32, vec2, Mat2, Vec2};
 
+use crate::ifaces::{DebugTraceable, RayPath, Traceable};
 use coords::{FlatCoordinateSystem, InnerCS, OuterCS};
 use metric::Tube;
 use Subspace::{Boundary, Inner, Outer};
 
-use crate::riemann;
+use crate::riemann::Metric;
 use crate::tube::coords::FlatRegion;
 use crate::types::{FlatTraceResult, Hit, Location, Object, Ray};
+use crate::{riemann, DT};
 
 mod coords;
 pub mod metric;
@@ -24,7 +26,7 @@ pub enum Subspace {
 }
 
 impl Space {
-	pub fn which_subspace(&self, pt: Vec2) -> Subspace {
+	fn which_subspace(&self, pt: Vec2) -> Subspace {
 		if pt.y.abs() > self.tube.external_halflength {
 			Outer
 		} else if pt.x.abs() > self.tube.outer_radius {
@@ -61,16 +63,24 @@ impl Space {
 		std::iter::successors(Some(ray), |&ray| Some(self.trace_step(ray)))
 	}
 
-	pub fn trace_inner(&self, ray: Ray) -> FlatTraceResult {
+	fn trace_inner(&self, ray: Ray) -> FlatTraceResult {
 		assert_eq!(self.which_subspace(ray.pos), Inner);
 		self.trace_flat(InnerCS(self.tube), ray)
 	}
 
-	pub fn trace_outer(&self, ray: Ray) -> FlatTraceResult {
+	fn trace_outer(&self, ray: Ray) -> FlatTraceResult {
 		assert_eq!(self.which_subspace(ray.pos), Outer);
 		self.trace_flat(OuterCS(self.tube), ray)
 	}
 
+	fn obj_hitter(&self, pos: Vec2) -> Option<fn(&Self, ray: Ray) -> FlatTraceResult> {
+		match self.which_subspace(pos) {
+			Inner => Some(Self::trace_inner),
+			Outer => Some(Self::trace_outer),
+			Boundary => None,
+		}
+	}
+
 	fn trace_flat(&self, cs: impl FlatRegion, ray: Ray) -> FlatTraceResult {
 		let ray = cs.global_to_flat(ray);
 		let dist = cs.distance_to_boundary(ray);
@@ -151,6 +161,83 @@ impl Space {
 			Boundary => panic!("Can't draw a line here!"),
 		}
 	}
+
+	fn camera_ray_to_abs(&self, camera: Location, ray: Ray) -> Ray {
+		let pos = camera.pos;
+		let dir = camera.rot * ray.dir;
+		// TODO account for ray.pos
+		let dir = DT * self.tube.normalize_vec_at(pos, dir);
+		Ray { pos, dir }
+	}
+}
+
+/// Like [`std::iter::successors`] but with an upper limit on iteration count.
+///
+/// # Panics
+///
+/// Panics if the sequence doesn’t terminate in `max_iters` calls of `succ`.
+fn iterate_with_limit<T>(max_iters: usize, init: T, mut succ: impl FnMut(T) -> Option<T>) {
+	let mut state = init;
+	for _ in 0..max_iters {
+		match succ(state) {
+			Some(next) => state = next,
+			None => return,
+		}
+	}
+	panic!("iteration limit exceeded");
+}
+
+impl Traceable for Space {
+	fn trace(&self, camera: Location, ray: Ray) -> Vec<Hit> {
+		let ray = self.camera_ray_to_abs(camera, ray);
+		let mut hits = vec![];
+		iterate_with_limit(100, ray, |ray| {
+			let ret = self
+				.trace_iter(ray)
+				.skip(1)
+				.find_map(|ray| self.obj_hitter(ray.pos).map(|hitter| hitter(self, ray)))
+				.expect("Space::trace_iter does not terminate");
+			hits.extend(ret.objects); // TODO fix distance
+			ret.end
+		});
+		hits
+	}
+}
+
+impl DebugTraceable for Space {
+	fn trace_dbg(&self, camera: Location, ray: Ray) -> (Vec<Hit>, RayPath) {
+		let mut points = vec![];
+		let mut hits = vec![];
+		let mut ray = self.camera_ray_to_abs(camera, ray);
+
+		let trace_to_flat = |points: &mut Vec<Vec2>, ray| {
+			for ray in self.trace_iter(ray).skip(1) {
+				points.push(ray.pos);
+				if let Some(hitter) = self.obj_hitter(ray.pos) {
+					return (ray, hitter(self, ray));
+				}
+			}
+			unreachable!("Space::trace_iter terminated!")
+		};
+
+		points.push(ray.pos);
+		for _ in 0..100 {
+			let (ray_into_flat, ret) = trace_to_flat(&mut points, ray);
+			hits.extend(ret.objects); // TODO fix distance
+			let Some(ray_outta_flat) = ret.end else {
+				return (
+					hits,
+					RayPath {
+						points,
+						end_dir: ray_into_flat.dir.normalize(),
+					},
+				);
+			};
+			points.extend(self.line(ray_into_flat.pos, ray_outta_flat.pos, 10.0));
+			ray = ray_outta_flat;
+		}
+		panic!("tracing didn't terminate");
+	}
 }
 
 struct Rect {

src/bin/mesh/main.rs

@@ -1,7 +1,7 @@
 use glam::*;
 use refraction::mesh_loader::load_mesh;
 use refraction::mesh_tracer::{trace_to_mesh, Mesh};
-use show_image::{ImageInfo, ImageView, WindowOptions};
+use show_image::{exit, ImageInfo, ImageView, WindowOptions};
 use std::env;
 use std::error::Error;
 use std::f32::consts::PI;
@@ -88,14 +88,40 @@ fn render(mesh: &Mesh, camera: impl Fn(Vec2) -> (Vec3, Vec3)) -> Image {
 	img
 }
 
+fn persp(dist: f32, off: Vec2) -> (Vec3, Vec3) {
+	(vec3(0., 0., -dist), vec3(off.x, off.y, dist))
+}
+
+fn ortho(dist: f32, off: Vec2) -> (Vec3, Vec3) {
+	(vec3(off.x, off.y, -dist), vec3(0., 0., 1.))
+}
+
+#[test]
+fn test_projs() {
+	fn check(f: fn(dist: f32, off: Vec2) -> (Vec3, Vec3), x: f32, y: f32, z: f32) {
+		let (base, ray) = f(z, vec2(x, y));
+		let at_dist = base + ray * (z / ray.z);
+		assert_eq!(at_dist, vec3(x, y, 0.));
+	}
+	check(persp, 1., 2., 3.);
+	check(ortho, 1., 2., 3.);
+	check(persp, 5., 3., 7.);
+	check(ortho, 9., 1., 8.);
+}
+
 #[show_image::main]
 fn main() -> Result<(), Box<dyn Error>> {
 	let args: Vec<String> = env::args().collect();
+	if args.len() != 2 {
+		println!("Usage: {} path/to/model.obj", args[0]);
+		exit(1);
+	}
 	let mesh = {
 		let f = File::open(&args[1])?;
 		let mut f = BufReader::new(f);
 		load_mesh(&mut f)?
 	};
+	let proj = persp;
 	let window = show_image::create_window("Raytracing", WindowOptions::default())?;
 	loop {
 		for phi in 0..360 {
@@ -106,14 +132,7 @@ fn main() -> Result<(), Box<dyn Error>> {
 			));
 			let m_camera = m_view.transpose();
 			let img = render(mesh.as_slice(), |off| {
-				// perspective projection
-				let base = vec3(0.0, 0.0, -40.0);
-				let ray = vec3(off.x, off.y, 2.0);
-
-				// orthographic projection
-				// let base = vec3(off.x, off.y, -10.0);
-				// let ray = vec3(0.0, 0.0, 1.0);
-
+				let (base, ray) = proj(40., 20. * off);
 				(m_camera * base, m_camera * ray)
 			});
 

src/lib.rs

@@ -1,2 +1,3 @@
+pub mod mathx;
 pub mod mesh_loader;
 pub mod mesh_tracer;

src/mathx.rs

@@ -0,0 +1,107 @@
+use glam::{FloatExt, Mat2, Mat3};
+
+mod bounds {
+	pub trait Pair<T> {}
+
+	impl<T> Pair<T> for (T, T) {}
+}
+
+pub trait FloatExt2<T>: bounds::Pair<T> {
+	fn lerp(self, t: T) -> T;
+	fn inverse_lerp(self, y: T) -> T;
+}
+
+impl<F: FloatExt> FloatExt2<F> for (F, F) {
+	fn lerp(self, t: F) -> F {
+		F::lerp(self.0, self.1, t)
+	}
+	fn inverse_lerp(self, y: F) -> F {
+		F::inverse_lerp(self.0, self.1, y)
+	}
+}
+
+pub trait MatExt {
+	fn orthonormalize(&self) -> Self;
+}
+
+impl MatExt for Mat2 {
+	fn orthonormalize(&self) -> Self {
+		let fx = self.x_axis.normalize();
+		let fy = (self.y_axis - self.y_axis.project_onto_normalized(fx)).normalize();
+		Self::from_cols(fx, fy)
+	}
+}
+
+impl MatExt for Mat3 {
+	fn orthonormalize(&self) -> Self {
+		let fx = self.x_axis.normalize();
+		let fy = (self.y_axis - self.y_axis.project_onto_normalized(fx)).normalize();
+		let fz = (self.z_axis
+			- self.z_axis.project_onto_normalized(fx)
+			- self.z_axis.project_onto_normalized(fy))
+		.normalize();
+		Self::from_cols(fx, fy, fz)
+	}
+}
+
+#[cfg(test)]
+mod tests {
+	use super::*;
+
+	use approx::assert_abs_diff_eq;
+	use glam::{mat2, mat3, vec2, vec3};
+
+	#[test]
+	fn test_lerp() {
+		assert_eq!((3., 7.).lerp(-0.5), 1.);
+		assert_eq!((3., 7.).lerp(0.0), 3.);
+		assert_eq!((3., 7.).lerp(0.5), 5.);
+		assert_eq!((3., 7.).lerp(1.0), 7.);
+		assert_eq!((3., 7.).lerp(1.5), 9.);
+		assert_eq!((3., 7.).inverse_lerp(1.), -0.5);
+		assert_eq!((3., 7.).inverse_lerp(3.), 0.0);
+		assert_eq!((3., 7.).inverse_lerp(5.), 0.5);
+		assert_eq!((3., 7.).inverse_lerp(7.), 1.0);
+		assert_eq!((3., 7.).inverse_lerp(9.), 1.5);
+	}
+
+	#[test]
+	fn test_orthonormalize_2d() {
+		assert_abs_diff_eq!(
+			mat2(vec2(1., 0.), vec2(0., 1.)).orthonormalize(),
+			mat2(vec2(1., 0.), vec2(0., 1.)),
+		);
+		assert_abs_diff_eq!(
+			mat2(vec2(2., 0.), vec2(3., 5.)).orthonormalize(),
+			mat2(vec2(1., 0.), vec2(0., 1.)),
+		);
+		assert_abs_diff_eq!(
+			mat2(vec2(0., -3.), vec2(5., 1.)).orthonormalize(),
+			mat2(vec2(0., -1.), vec2(1., 0.)),
+		);
+		assert_abs_diff_eq!(
+			mat2(vec2(3., 4.), vec2(5., 1.)).orthonormalize(),
+			mat2(vec2(0.6, 0.8), vec2(0.8, -0.6)),
+		);
+		assert_abs_diff_eq!(
+			mat2(vec2(3., 4.), vec2(1., 5.)).orthonormalize(),
+			mat2(vec2(0.6, 0.8), vec2(-0.8, 0.6)),
+		);
+	}
+
+	#[test]
+	fn test_orthonormalize_3d() {
+		assert_abs_diff_eq!(
+			mat3(vec3(1., 0., 0.), vec3(0., 1., 0.), vec3(0., 0., 1.)).orthonormalize(),
+			mat3(vec3(1., 0., 0.), vec3(0., 1., 0.), vec3(0., 0., 1.)),
+		);
+		assert_abs_diff_eq!(
+			mat3(vec3(2., 0., 0.), vec3(3., 4., 0.), vec3(5., 6., 7.)).orthonormalize(),
+			mat3(vec3(1., 0., 0.), vec3(0., 1., 0.), vec3(0., 0., 1.)),
+		);
+		assert_abs_diff_eq!(
+			mat3(vec3(0., 5., 0.), vec3(0., 7., 6.), vec3(9., 2., 3.)).orthonormalize(),
+			mat3(vec3(0., 1., 0.), vec3(0., 0., 1.), vec3(1., 0., 0.)),
+		);
+	}
+}