diff --git a/src/lib.rs b/src/lib.rs
index f8a2b53..f69adc8 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,2 +1,3 @@
+pub mod mathx;
 pub mod mesh_loader;
 pub mod mesh_tracer;
diff --git a/src/mathx.rs b/src/mathx.rs
new file mode 100644
index 0000000..8176de7
--- /dev/null
+++ b/src/mathx.rs
@@ -0,0 +1,73 @@
+use glam::{Mat2, Mat3};
+
+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_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.)),
+		);
+	}
+}