From 3d53982e7292533faf700a389da50eb6a381440d Mon Sep 17 00:00:00 2001
From: Marc Planard <case@corpsmoderne.net>
Date: Sat, 1 Jul 2023 17:14:01 +0200
Subject: [PATCH] fix homography calculation + test

---
 src/point.rs                  |  4 +--
 src/transformer/homography.rs | 65 ++++++++++++++++++++++++++++-------
 src/worldstate.rs             |  8 +++++
 3 files changed, 62 insertions(+), 15 deletions(-)

diff --git a/src/point.rs b/src/point.rs
index 44fa9b4..5eb6d42 100644
--- a/src/point.rs
+++ b/src/point.rs
@@ -1,11 +1,11 @@
-#[derive(Debug,Clone,Copy,Default)]
+#[derive(Debug,Clone,Copy,Default,PartialEq)]
 pub struct Point {
     pub x: f32,
     pub y: f32,
     pub color: Color
 }
 
-#[derive(Debug,Clone,Copy,Default)]
+#[derive(Debug,Clone,Copy,Default,PartialEq)]
 pub struct Color {
     r: u8,
     g: u8,
diff --git a/src/transformer/homography.rs b/src/transformer/homography.rs
index bda21c3..0713583 100644
--- a/src/transformer/homography.rs
+++ b/src/transformer/homography.rs
@@ -1,9 +1,10 @@
 use crate::transformer::Transformers;
 use crate::point::Point;
-use crate::worldstate::WorldState;
+use crate::worldstate::{WorldState,EDH};
 use serde::{Serialize,Deserialize};
 use log::info;
 use nalgebra::Matrix3;
+use nalgebra::Matrix1x3;
 
 /// Homography
 
@@ -12,25 +13,63 @@ pub struct Homography {}
 
 impl Transformers for Homography {
     fn apply(&self, point_list: &[Point], ws: &WorldState) -> Vec<Point> {	
-	let m = ws.edh.matrix;
+	let edh : &EDH = &ws.edh;
 
 	point_list.iter()
 	    .map(| point | {
-
-		// THIS IS CERTAINLY ALL WRONG! NEEDS DEBUGING!!!
+		let p = Matrix1x3::new(point.x, point.y, 1.0);
+		let p = p * edh.matrix;
+		let new_p = Point { x: p[0] / p[2], y: p[1] / p[2], ..*point };
 		
-		let p = Matrix3::new(point.x, point.y, 1.0,
-				     1.0, 1.0, 1.0,
-				     1.0, 1.0, 1.0);
-		let dot = p.tr_dot(&m);
-		let new_p = Point { x: point.x / dot, y: point.y / dot,
-				    ..*point
-		};
-
-		info!("{:?} => {:?}", point, new_p);
+		dbg!("{:?} => {:?}", point, new_p);
 		
 		new_p
 	    })
 	    .collect()
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_homography_identity() {
+	let p0 = Point { x: -1500.0, y: 1500.0, ..Point::default() };
+
+	let edh = EDH::new(vec![
+	    vec![ 1.0, 0.0, 0.0 ],
+	    vec![ 0.0, 1.0, 0.0 ],
+	    vec![ 0.0, 0.0, 1.0 ]
+	]).unwrap();
+
+	let ws = WorldState { edh : edh, ..WorldState::default() };
+	
+	let homography = Homography{};
+	let result = homography.apply(&[p0], &ws);
+	
+	assert_eq!(result, vec![Point { x: -1500.0,
+					y: 1500.0,
+					..Point::default() }]);
+    }
+
+    #[test]
+    fn test_homography_rotation() {
+	let p0 = Point { x: -1500.0, y: 1500.0, ..Point::default() };
+
+	let edh = EDH::new(vec![
+	    vec![ 1.24107321e-03, 1.00500127e-03, 7.15439347e-01],
+	    vec![-9.93223912e-04, 1.22652939e-03,-6.98671238e-01],
+	    vec![ 1.06017142e-17,-4.69459541e-17, 3.32700590e-05]
+	]).unwrap();
+
+	let ws = WorldState { edh : edh, ..WorldState::default() };
+	
+	let homography = Homography{};
+	let result = homography.apply(&[p0], &ws);
+	
+	assert_eq!(result, vec![Point { x: 10860.557,
+					y: 79078.87,
+					..Point::default() }]);
+    }
+}
diff --git a/src/worldstate.rs b/src/worldstate.rs
index aaf26e7..8377212 100644
--- a/src/worldstate.rs
+++ b/src/worldstate.rs
@@ -18,9 +18,17 @@ impl EDH {
 	//
 	// [FIX] Not sure of the order, if is it's vec[x][y] or vec[y][x] ...
 	//
+	/*
 	let matrix = Matrix3::new(vec[0][0], vec[0][1], vec[0][2],
 				  vec[1][0], vec[1][1], vec[1][2],
 				  vec[2][0], vec[2][1], vec[2][2]);
+	 */
+	
+	// this is the matrix already transposed.
+	let matrix = Matrix3::new(vec[0][0], vec[1][0], vec[2][0],
+				  vec[0][1], vec[1][1], vec[2][1],
+				  vec[0][2], vec[1][2], vec[2][2]);
+
 	Ok(EDH { matrix })
     }
 }