Point conversion between surfaces

builtin-programs/points.folk

@@ -0,0 +1,83 @@
+set pointsLib [library create pointsLib {} {
+    proc surfacePoint {surface xy} {
+        upvar this this
+        return [list surface-point $surface $xy]
+    }
+
+    proc surfacePointCheckType {surfacePoint} {
+        set type [lindex $surfacePoint 0]
+        if {$type ne "surface-point"} { error "expected type "surface-point", but got \"$type\""}
+    }
+
+    proc surfacePointVector {surfacePoint} {
+        surfacePointCheckType $surfacePoint
+        return [lindex $surfacePoint 2]
+    }
+
+    proc surfacePointId {surfacePoint} {
+        surfacePointCheckType $surfacePoint
+        return [lindex $surfacePoint 1]
+    }
+
+    proc spacePoint {space xyz} {
+        return [list space-point $space $xyz]
+    }
+
+    proc spacePointCheckType {spacePoint} {
+        set type [lindex $spacePoint 0]
+        if {$type ne "space-point"} { error "expected type "space-point", but got \"$type\""}
+    }
+
+    proc spacePointVector {spacePoint} {
+        spacePointCheckType $spacePoint
+        return [lindex $spacePoint 2]
+    }
+
+    proc spacePointSpace {spacePoint} {
+        spacePointCheckType $spacePoint
+        return [lindex $spacePoint 1]
+    }
+
+    # User API
+    proc point {x y} {
+        upvar this this
+        return [surfacePoint $this [list $x $y]]
+    }
+}]
+Claim the points library is $pointsLib
+
+When when point /point/ has point /anything/ on /targetSurface/ /lambda/ with environment /anything/ {
+    Wish point $point is projected to surface $targetSurface
+}
+
+When the quad library is /quadLib/ &\
+     the quad changer is /quadChange/ &\
+     /someone/ wishes point /point/ is projected to surface /targetSurface/ {
+    set pointSurface [$pointsLib surfacePointId $point]
+    When $pointSurface has quad /pointQuad/ &\
+         $targetSurface has quad /targetQuad/ {
+        # "Now in 3D!"
+        set pointInSpace [$quadLib XYToPoint $pointQuad [$pointsLib surfacePointVector $point]]
+
+        # HACK currently the `quadChange` changes quads, not points. We
+        # construct an artifical quad that has zeros for the last three
+        # coordinates so we can still use quadLib for conversion.
+        set targetSpace [$quadLib space $targetQuad]
+        set pointInQuad [$quadLib create $targetSpace [list $pointInSpace {0.0 0.0 0.0} \
+                                                            {0.0 0.0 0.0} {0.0 0.0 0.0}]]
+
+        fn quadChange
+        # Note this will throw an error if the point can't be converted between these
+        # two spaces.
+        set pointQuadInTargetSpace [quadChange $pointInQuad $targetSpace]
+        set pointInTargetSpace [lindex [$quadLib vertices $pointQuadInTargetSpace] 0]
+
+        set projectedOntoTarget [$quadLib projectAlongSourceNormal $pointQuad $targetQuad $pointInTargetSpace]
+        set targetXY [$quadLib pointToXY $targetQuad $projectedOntoTarget]
+
+        # Flatland has entered the chat.
+        set pointOnTargetSurface [$pointsLib surfacePoint $targetSurface $targetXY]
+
+        Claim point $point has point $pointOnTargetSurface on $targetSurface
+    }
+}

builtin-programs/quad-lib.folk

@@ -6,6 +6,7 @@ set quadLib [library create quadLib {} {
         ::math::linearalgebra::sub \
         ::math::linearalgebra::norm \
         ::math::linearalgebra::dotproduct \
+        ::math::linearalgebra::crossproduct \
         ::math::linearalgebra::unitLengthVector \
         ::math::linearalgebra::scale
     rename scale scaleVector
@@ -230,10 +231,22 @@ set quadLib [library create quadLib {} {
         return [create [space $q] [list $topLeft $topRight $bottomRight $bottomLeft]]
     }
 
+    proc XYToPoint {quad xy} {
+        lassign $xy x y
+        lassign [vertices $quad] topLeft topRight bottomRight bottomLeft
+        set width [norm [sub $topRight $topLeft]]
+        set height [norm [sub $bottomLeft $topLeft]]
+        set xAxis [unitLengthVector [sub $topRight $topLeft]]
+        set yAxis [unitLengthVector [sub $bottomLeft $topLeft]]
+        set pointRelativeToTopLeft [add [scaleVector $x $xAxis] \
+                                        [scaleVector $y $yAxis]]
+        return [add $pointRelativeToTopLeft $topLeft]
+    }
+
     # Take a {x y z} point in the same space as the quad and project
     # it down onto a {top left} point on the quad (that can be used in
     # the drawing APIs, etc).
-    proc pointToXY {q xyz {checkBounds true}} {
+    proc pointToXY {q xyz} {
         lassign [vertices $q] topLeft topRight bottomRight bottomLeft
         set width [norm [sub $topRight $topLeft]]
         set height [norm [sub $bottomLeft $topLeft]]
@@ -243,15 +256,31 @@ set quadLib [library create quadLib {} {
         set x [dotproduct $delta $xAxis]
         set y [dotproduct $delta $yAxis]
 
-        set eps 0.000001
-        if {$x < -$eps || $x > $width + $eps ||
-            $y < -$eps || $y > $height + $eps} {
-            if {$checkBounds} {
-                error "quad pointToXY: point $xyz projects outside quad bounds as {$x $y}; bounds are {0 0} {$width $height}"
-            }
+        return [list $x $y]
+    }
+
+    proc projectAlongSourceNormal {sourceQuad targetQuad xyz} {
+        lassign [vertices $sourceQuad] sourceTopLeft sourceTopRight sourceBottomRight sourceBottomLeft
+        # We project a ray out from the source quad onto the target quad.
+        set rayDirection [unitLengthVector [crossproduct \
+            [sub $sourceTopRight $sourceTopLeft] \
+            [sub $sourceBottomLeft $sourceTopLeft]]]
+
+        lassign [vertices $targetQuad] targetTopLeft targetTopRight targetBottomRight targetBottomLeft
+        set targetNormal [unitLengthVector [crossproduct \
+            [sub $targetTopRight $targetTopLeft] \
+            [sub $targetBottomLeft $targetTopLeft]]]
+
+        set cosineToTarget [dotproduct $targetNormal $rayDirection]
+        if {abs($cosineToTarget) < 1e-6} {
+            error "projectAlongSourceNormal: source plane is perpendicular to target plane"
         }
 
-        return [list $x $y]
+        set distanceToTarget [expr {
+            [dotproduct $targetNormal [sub $targetTopLeft $xyz]] / $cosineToTarget
+        }]
+
+        return [add $xyz [scaleVector $distanceToTarget $rayDirection]]
     }
 }]
 Claim the quad library is $quadLib

builtin-programs/tags-to-quads.folk

@@ -42,6 +42,22 @@ $cc proc distort {double fx double fy double cx double cy
     out[0] = (x * (1.0 + radial) + dx)*fx + cx;
     out[1] = (y * (1.0 + radial) + dy)*fy + cy;
 }
+$cc proc undistort {double fx double fy double cx double cy
+                    double k1 double k2 double p1 double p2
+                    double xy[2] double out[2]} void {
+    double x0 = (xy[0] - cx)/fx;
+    double y0 = (xy[1] - cy)/fy;
+    double x = x0, y = y0;
+    for (int i = 0; i < 5; i++) {
+        double r2 = x*x + y*y;
+        double radial = 1.0 + k1 * r2 + k2 * r2*r2;
+        double dx = 2*p1*x*y + p2*(r2 + 2*x*x);
+        double dy = p1*(r2 + 2*y*y) + 2*p2*x*y;
+        x = (x0 - dx) / radial;
+        y = (y0 - dy) / radial;
+    }
+    out[0] = x*fx + cx; out[1] = y*fy + cy;
+}
 $cc proc project {Intrinsics intr double width double height
                   double[3] v} Jim_Obj* {
     double out[3] = {
@@ -61,6 +77,30 @@ $cc proc project {Intrinsics intr double width double height
     };
     return Jim_NewListObj(interp, retObjs, 2);
 }
+# Returns a homogenous point.
+$cc proc unproject {Intrinsics intr double width double height
+                    double u double v} Jim_Obj* {
+    // Normalize the provided `u` and `v` to our width and height.
+    double pixel[2] = {
+        u * intr.width / width,
+        v * intr.height / height
+    };
+
+    undistort(intr.fx, intr.fy, intr.cx, intr.cy,
+              intr.k1, intr.k2, intr.p1, intr.p2,
+              pixel, pixel);
+
+    // Apply inverse intrinsic matrix.
+    double y = (pixel[1] - intr.cy) / intr.fy;
+    double x = (pixel[0] - intr.cx - intr.s * y) / intr.fx;
+
+    Jim_Obj* retObjs[3] = {
+        Jim_NewDoubleObj(interp, x),
+        Jim_NewDoubleObj(interp, y),
+        Jim_NewDoubleObj(interp, 1)
+    };
+    return Jim_NewListObj(interp, retObjs, 3);
+}
 $cc proc rescaleAndUndistort {Intrinsics intr
                               double cameraWidth double cameraHeight
                               double* in