First draft at c homography

1 files changed, 45 insertions, 7 deletions

diff --git a/virtual-programs/tags-and-calibration.folk b/virtual-programs/tags-and-calibration.folk
index 2ad6cb59..f6b3414d 100644
--- a/virtual-programs/tags-and-calibration.folk
+++ b/virtual-programs/tags-and-calibration.folk
@@ -58,15 +58,53 @@ set ::H [subst {
 }]
 set ::Hinv [solvePGauss $::H [math::linearalgebra::mkIdentity 3]]
 
-# FIXME: these shouldn't be global
-proc ::cameraToProjector {cameraPoint} {
-    # FIXME: this is slow but not the cause of leak
-    lassign [::math::linearalgebra::matmul $::H [list [lindex $cameraPoint 0] [lindex $cameraPoint 1] 1]] Hx Hy Hz
-    set Hx [expr {$Hx / $Hz}]
-    set Hy [expr {$Hy / $Hz}]
-    return [list $Hx $Hy]
+set cc [c create]
+
+$cc include <stdlib.h>
+
+$cc code [csubst {
+  static float H[3][3] = {
+    {$a0, $a1, $a2},
+    {$b0, $b1, $b2},
+    {$c0, $c1, 1},
+  };
+}]
+
+$cc proc ::cameraToProjector {Tcl_Interp* interp Tcl_Obj* cameraPoint} Tcl_Obj* {
+    int cameraPointLength;
+    Tcl_Obj** cameraPointElements;
+    if (Tcl_ListObjGetElements(interp, cameraPoint, &cameraPointLength, &cameraPointElements) != TCL_OK) {
+      exit(1);
+    }
+
+    int cx, cy;
+    if (Tcl_GetIntFromObj(interp, cameraPointElements[0], &cx) != TCL_OK) {
+      exit(1);
+    }
+    if (Tcl_GetIntFromObj(interp, cameraPointElements[1], &cy) != TCL_OK) {
+      exit(1);
+    }
+
+    double Hx, Hy, Hz;
+
+    Hx = H[0][0] * cx + H[0][1] * cy + H[0][2] * 1.0;
+    Hy = H[1][0] * cx + H[1][1] * cy + H[1][2] * 1.0;
+    Hz = H[2][0] * cx + H[2][1] * cy + H[2][2] * 1.0;
+
+    Hx = Hx / Hz;
+    Hy = Hy / Hz;
+
+		Tcl_Obj* projectorPoint_v[] = {
+      Tcl_NewDoubleObj(Hx),
+      Tcl_NewDoubleObj(Hy),
+    };
+
+    Tcl_Obj* projectorPoint = Tcl_NewListObj(2, projectorPoint_v);
+    return projectorPoint;
 }
 
+$cc compile
+
 proc ::projectorToCamera {projectorPoint} {
     lassign [::math::linearalgebra::matmul $::Hinv [list [lindex $projectorPoint 0] [lindex $projectorPoint 1] 1]] Hinvx Hinvy Hinvz
     set Hinvx [expr {$Hinvx / $Hinvz}]