fix homography calculation + test

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,

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 | {
+		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 };
 		
-		// THIS IS CERTAINLY ALL WRONG! NEEDS DEBUGING!!!
-		
-		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() }]);
+    }
+}

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 })
     }
 }