diff --git a/builtin-programs/points.folk b/builtin-programs/points.folk new file mode 100644 index 00000000..fda09a83 --- /dev/null +++ b/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 + } +} \ No newline at end of file diff --git a/builtin-programs/quad-lib.folk b/builtin-programs/quad-lib.folk index 60c3e829..79c21449 100644 --- a/builtin-programs/quad-lib.folk +++ b/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 diff --git a/builtin-programs/tags-to-quads.folk b/builtin-programs/tags-to-quads.folk index b9475f10..86fdb83b 100644 --- a/builtin-programs/tags-to-quads.folk +++ b/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