From f5f94d3c1c212f8cb58cbe3cf9528c80a9f5786d Mon Sep 17 00:00:00 2001
From: numzero <numzer0@yandex.ru>
Date: Sat, 14 Sep 2024 18:27:57 +0300
Subject: [PATCH] Add put_object

---
 src/bin/flat/main.rs | 43 +++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 43 insertions(+)

diff --git a/src/bin/flat/main.rs b/src/bin/flat/main.rs
index 60a8e3e..966344c 100644
--- a/src/bin/flat/main.rs
+++ b/src/bin/flat/main.rs
@@ -146,6 +146,49 @@ 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.
+///
+/// Mathematically, returned `rot` is `M^-1 * orthonormalize(M * rot)` where `M^2` is the metric tensor.
+fn put_object(space: &impl Metric, pos: Vec2, rot: Mat2) -> Location {
+	let x_dir = rot.x_axis;
+	let metric_sqrt = Mat2::from(space.sqrt_at(pos));
+	let metric_inv_sqrt = Mat2::from(space.sqrt_at(pos).inverse());
+	let fx = (metric_sqrt * x_dir).normalize();
+	let fy = vec2(-fx.y, fx.x); // TODO: respect orientation of `rot`
+	let rot = metric_inv_sqrt * mat2(fx, fy);
+	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);