diff options
| author | Omar Rizwan <omar@omar.website> | 2023-10-05 23:25:22 +0000 |
|---|---|---|
| committer | Omar Rizwan <omar@omar.website> | 2023-10-05 23:25:22 +0000 |
| commit | 5f870b4b4bf184ddd2ed7082fee8203781827336 (patch) | |
| tree | bcbe90b44876e9aea51c4933a0b99b1d04aef187 /virtual-programs | |
| parent | Merge pull request #95 from FolkComputer/nm/apriltags (diff) | |
| parent | calibrate: Return to autoexposure at end? (diff) | |
| download | folk-5f870b4b4bf184ddd2ed7082fee8203781827336.tar.gz folk-5f870b4b4bf184ddd2ed7082fee8203781827336.zip | |
Merge branch 'osnr/vulkan-display'
Diffstat (limited to 'virtual-programs')
| -rw-r--r-- | virtual-programs/apriltags.folk | 22 | ||||
| -rw-r--r-- | virtual-programs/camera.folk | 13 | ||||
| -rw-r--r-- | virtual-programs/display.folk | 431 | ||||
| -rw-r--r-- | virtual-programs/images.folk | 39 | ||||
| -rw-r--r-- | virtual-programs/tags-and-calibration.folk | 4 |
5 files changed, 450 insertions, 59 deletions
diff --git a/virtual-programs/apriltags.folk b/virtual-programs/apriltags.folk index 69fcf51c..7e190a5b 100644 --- a/virtual-programs/apriltags.folk +++ b/virtual-programs/apriltags.folk @@ -53,17 +53,17 @@ On process { [list /someone/ wishes /something/ displays camera slice /slice/] proc subimage {im x y subwidth subheight} { - dict with im { - set x [expr {int($x)}] - set y [expr {int($y)}] - set subdata [expr {$data + ($y*$width + $x) * $components}] - dict create \ - width [int $subwidth] \ - height [int $subheight] \ - components $components \ - bytesPerRow $bytesPerRow \ - data [format 0x%x $subdata] - } + dict with im { + set x [expr {int($x)}] + set y [expr {int($y)}] + set subdata [expr {[lindex $data 1] + ($y*$width + $x) * $components}] + dict create \ + width [int $subwidth] \ + height [int $subheight] \ + components $components \ + bytesPerRow $bytesPerRow \ + data [format "(uint8_t*) 0x%x" $subdata] + } } set detector [AprilTags new $tagfamily] diff --git a/virtual-programs/camera.folk b/virtual-programs/camera.folk index 428c4195..0e1710e7 100644 --- a/virtual-programs/camera.folk +++ b/virtual-programs/camera.folk @@ -16,15 +16,22 @@ set height $::Camera::HEIGHT On process { source pi/Camera.tcl + Camera::init $width $height - puts "Camera tid: [getTid] booting at [clock milliseconds]" + puts "Camera tid: [getTid] booted at [clock milliseconds]" + set ::oldFrames [list] When $::thisProcess has step count /c/ { - set grayFrame [Camera::grayFrame] + set frame [Camera::grayFrame] Commit { Claim the camera time is $::stepTime - Claim the camera frame is $grayFrame at [clock milliseconds] + Claim the camera frame is $frame at [clock milliseconds] + } + lappend ::oldFrames $frame + if {[llength $::oldFrames] >= 10} { + set ::oldFrames [lassign $::oldFrames oldestFrame] + Camera::freeImage $oldestFrame } } } diff --git a/virtual-programs/display.folk b/virtual-programs/display.folk index b07ec644..f7b70959 100644 --- a/virtual-programs/display.folk +++ b/virtual-programs/display.folk @@ -1,10 +1,12 @@ -if {$::isLaptop} return - namespace eval ::Display { variable WIDTH variable HEIGHT variable LAYER 0 - regexp {mode "(\d+)x(\d+)"} [exec fbset] -> WIDTH HEIGHT + if {$::isLaptop} { + set WIDTH 640; set HEIGHT 480 + } else { + regexp {mode "(\d+)x(\d+)"} [exec fbset] -> WIDTH HEIGHT + } proc drawOnTop {func args} { set ::Display::LAYER 1 @@ -12,35 +14,396 @@ namespace eval ::Display { set ::Display::LAYER 0 } - proc stroke {points width color} { - uplevel [list Wish display runs [list Display::stroke $points $width $color] on layer $::Display::LAYER] + # Create proxy versions of drawing primitives for the main Folk + # process (that will forward those draw commands to the display + # subprocess). + foreach func {stroke circle text fillTriangle fillQuad fillPolygon} { + proc $func args { + set func [lindex [info level 0] 0] + uplevel [list Wish display runs [list $func {*}$args] on layer $::Display::LAYER] + } } +} - proc circle {x y radius thickness color {filled false}} { - uplevel [list Wish display runs [list Display::circle $x $y $radius $thickness $color $filled] on layer $::Display::LAYER] - } +On process { + puts "Display pid: [pid]" - proc text args { - uplevel [list Wish display runs [list Display::text {*}$args] on layer $::Display::LAYER] - } + source pi/Gpu.tcl + Gpu::init + Gpu::ImageManager::imageManagerInit - proc fillTriangle args { - uplevel [list Wish display runs [list Display::fillTriangle {*}$args] on layer $::Display::LAYER] - } + namespace eval Display { + namespace eval Colors { source "pi/Colors.tcl" } - proc fillQuad args { - uplevel [list Wish display runs [list Display::fillQuad {*}$args] on layer $::Display::LAYER] - } + variable rotate [Gpu::fn {vec2 v float a} vec2 { + float s = sin(a); + float c = cos(a); + mat2 m = mat2(c, s, -s, c); + return m * v; + }] + variable cross2d [Gpu::fn {vec2 a vec2 b} float { + return a.x*b.y - a.y*b.x; + }] + # See https://www.shadertoy.com/view/lsBSDm + variable invBilinear [Gpu::fn {vec2 p vec2 a vec2 b vec2 c vec2 d fn cross2d} vec2 { + vec2 res = vec2(-1.0); - proc fillPolygon args { - uplevel [list Wish display runs [list Display::fillPolygon {*}$args] on layer $::Display::LAYER] - } -} + vec2 e = b-a; + vec2 f = d-a; + vec2 g = a-b+c-d; + vec2 h = p-a; + + float k2 = cross2d( g, f ); + float k1 = cross2d( e, f ) + cross2d( h, g ); + float k0 = cross2d( h, e ); -On process { - source pi/Display.tcl - Display::init - puts "Display tid: [getTid]" + // if edges are parallel, this is a linear equation + k2 /= k0; k1 /= k0; k0 = 1.0; + if( abs(k2)<0.001*abs(k0) ) + { + res = vec2( (h.x*k1+f.x*k0)/(e.x*k1-g.x*k0), -k0/k1 ); + } + // otherwise, it's a quadratic + else + { + float w = k1*k1 - 4.0*k0*k2; + if( w<0.0 ) return vec2(-1.0); + w = sqrt( w ); + + float ik2 = 0.5/k2; + float v = (-k1 - w)*ik2; + float u = (h.x - f.x*v)/(e.x + g.x*v); + + if( u<0.0 || u>1.0 || v<0.0 || v>1.0 ) + { + v = (-k1 + w)*ik2; + u = (h.x - f.x*v)/(e.x + g.x*v); + } + res = vec2( u, v ); + } + return res; + }] + + namespace eval font { + proc load {name} { + set csvFd [open "vendor/fonts/$name.csv" r]; set csv [read $csvFd]; close $csvFd + set glyphInfos [dict create] + foreach line [split $csv "\n"] { + set info [lassign [split $line ,] glyph] + lassign $info advance \ + planeLeft planeBottom planeRight planeTop \ + atlasLeft atlasBottom atlasRight atlasTop + dict set glyphInfos $glyph \ + [list $advance \ + [list $planeLeft $planeBottom $planeRight $planeTop] \ + [list $atlasLeft $atlasBottom $atlasRight $atlasTop]] + } + + set im [image load "[pwd]/vendor/fonts/$name.png"] + set gim [Gpu::ImageManager::copyImageToGpu $im] + + return [list $glyphInfos $im $gim] + } + proc hasGlyphInfo {font charCode} { dict exists [lindex $font 0] $charCode } + proc glyphInfo {font charCode} { dict get [lindex $font 0] $charCode } + proc atlasImage {font} { lindex $font 1 } + proc gpuAtlasImage {font} { lindex $font 2 } + + namespace export * + namespace ensemble create + } + variable font [font load "PTSans-Regular"] + variable glyphMsd [Gpu::fn {sampler2D atlas vec4 atlasGlyphBounds vec2 glyphUv} vec4 { + vec2 atlasUv = mix(atlasGlyphBounds.xw, atlasGlyphBounds.zy, glyphUv); + return texture(atlas, vec2(atlasUv.x, 1.0-atlasUv.y)); + }] + variable median [Gpu::fn {float r float g float b} float { + return max(min(r, g), min(max(r, g), b)); + }] + variable glyph [Gpu::pipeline {sampler2D atlas vec2 atlasSize + vec4 atlasGlyphBounds + vec4 planeGlyphBounds + vec2 pos float radians float em + fn rotate} { + float left = planeGlyphBounds[0] * em; + float bottom = planeGlyphBounds[1] * em; + float right = planeGlyphBounds[2] * em; + float top = planeGlyphBounds[3] * em; + vec2 a = pos + rotate(vec2(left, -top), -radians); + vec2 b = pos + rotate(vec2(right, -top), -radians); + vec2 c = pos + rotate(vec2(right, -bottom), -radians); + vec2 d = pos + rotate(vec2(left, -bottom), -radians); + + vec2 vertices[4] = vec2[4](a, b, d, c); + return vertices[gl_VertexIndex]; + } {fn rotate fn invBilinear fn glyphMsd fn median} { + float left = planeGlyphBounds[0] * em; + float bottom = planeGlyphBounds[1] * em; + float right = planeGlyphBounds[2] * em; + float top = planeGlyphBounds[3] * em; + vec2 a = pos + rotate(vec2(left, -top), -radians); + vec2 b = pos + rotate(vec2(right, -top), -radians); + vec2 c = pos + rotate(vec2(right, -bottom), -radians); + vec2 d = pos + rotate(vec2(left, -bottom), -radians); + + vec2 glyphUv = invBilinear(gl_FragCoord.xy, a, b, c, d); + if( max( abs(glyphUv.x-0.5), abs(glyphUv.y-0.5))>=0.5 ) { + return vec4(0, 0, 0, 0); + } + vec3 msd = glyphMsd(atlas, atlasGlyphBounds/atlasSize.xyxy, glyphUv).rgb; + float sd = median(msd.r, msd.g, msd.b); + float screenPxDistance = 4.5*(sd - 0.5); + float opacity = clamp(screenPxDistance + 0.5, 0.0, 1.0); + return mix(vec4(0, 0, 0, 0), vec4(1, 1, 1, 1), opacity); + }] + + proc start {} { Gpu::drawStart } + + proc getColor {color} { + if {[info exists Colors::$color]} { return [set Colors::$color] } \ + else { return $Colors::white } + } + + variable line [Gpu::pipeline {vec2 from vec2 to float thickness vec4 color} { + vec2 vertices[4] = vec2[4]( + min(from, to) - thickness, + vec2(max(from.x, to.x) + thickness, min(from.y, to.y) - thickness), + vec2(min(from.x, to.x) - thickness, max(from.y, to.y) + thickness), + max(from, to) + thickness + ); + return vertices[gl_VertexIndex]; + } { + float l = length(to - from); + vec2 d = (to - from) / l; + vec2 q = (gl_FragCoord.xy - (from + to)*0.5); + q = mat2(d.x, -d.y, d.y, d.x) * q; + q = abs(q) - vec2(l, thickness)*0.5; + float dist = length(max(q, 0.0)) + min(max(q.x, q.y), 0.0); + + return dist < 0.0 ? color : vec4(0, 0, 0, 0); + }] + proc stroke {points width color} { + variable line + for {set i 0} {$i < [expr {[llength $points] - 1}]} {incr i} { + set from [lindex $points $i] + set to [lindex $points [expr $i+1]] + Gpu::draw $line $from $to $width [getColor $color] + } + } + + # TODO: Fix bool support. + variable circle [Gpu::pipeline {vec2 center float radius float thickness vec4 color int filled} { + float r = radius + thickness; + vec2 vertices[4] = vec2[4]( + center - r, + vec2(center.x + r, center.y - r), + vec2(center.x - r, center.y + r), + center + r + ); + return vertices[gl_VertexIndex]; + } { + float dist = length(gl_FragCoord.xy - center) - radius; + if (filled == 1) { + return (dist < thickness) ? color : vec4(0, 0, 0, 0); + } else { + return (dist < thickness && dist > 0.0) ? color : vec4(0, 0, 0, 0); + } + }] + proc circle {x y radius thickness color {filled false}} { + variable circle + Gpu::draw $circle [list $x $y] $radius $thickness [getColor $color] [ne $filled "false"] + } + + proc textExtent {text scale} { + variable font + set em [* $scale 25.0] + set x 0; set y 0 + set width 0 + for {set i 0} {$i < [string length $text]} {incr i} { + set char [string index $text $i] + if {$char eq "\n"} { + set y [+ $y $em]; set x 0; continue + } + set charCode [scan $char %c] + if {[font hasGlyphInfo $font $charCode]} { + set glyphInfo [font glyphInfo $font $charCode] + } else { + set glyphInfo [font glyphInfo $font [scan ? %c]] + } + lassign $glyphInfo advance planeBounds atlasBounds + set x [+ $x [* $advance $em]] + if {$x > $width} { set width $x } + } + return [list $width [+ $y $em]] + } + proc text {x0 y0 scale text radians} { + variable font + variable glyph + set fontAtlas [font gpuAtlasImage $font] + set fontAtlasSize [list [::image width [font atlasImage $font]] \ + [::image height [font atlasImage $font]]] + + set extent [vec2 rotate [textExtent $text $scale] $radians] + + set em [* $scale 25.0] + + # TODO: Add text alignment/anchor options (right now, this + # setup centers the text). + set x0 [expr {$x0 - [lindex $extent 0]/2}] + set y0 [expr {$y0 - [lindex $extent 1]/2}] + set x $x0; set y $y0 + + set lineNum 0 + for {set i 0} {$i < [string length $text]} {incr i} { + set char [string index $text $i] + if {$char eq "\n"} { + incr lineNum + lassign [vec2 add [list $x0 $y0] \ + [vec2 rotate [list 0 [* $lineNum $em]] $radians]] x y + continue + } + set charCode [scan $char %c] + if {[font hasGlyphInfo $font $charCode]} { + set glyphInfo [font glyphInfo $font $charCode] + } else { + set glyphInfo [font glyphInfo $font [scan ? %c]] + } + lassign $glyphInfo advance planeBounds atlasBounds + if {$char ne " "} { + Gpu::draw $glyph $fontAtlas $fontAtlasSize \ + $atlasBounds $planeBounds [list $x $y] $radians $em + } + lassign [vec2 add [list $x $y] \ + [vec2 rotate [list [* $advance $em] 0] $radians]] x y + } + } + + variable fillTriangle [Gpu::pipeline {vec2 p0 vec2 p1 vec2 p2 vec4 color} { + vec2 vertices[4] = vec2[4](p0, p1, p2, p0); + return vertices[gl_VertexIndex]; + } { + return color; + }] + proc fillTriangle {p0 p1 p2 color} { + variable fillTriangle + Gpu::draw $fillTriangle $p0 $p1 $p2 [getColor $color] + } + + proc fillQuad {p0 p1 p2 p3 color} { + fillTriangle $p1 $p2 $p3 $color + fillTriangle $p0 $p1 $p3 $color + } + + proc fillPolygon {points color} { + set num_points [llength $points] + if {$num_points < 3} { + error "At least 3 points are required to form a polygon." + } elseif {$num_points == 3} { + eval fillTriangle $points $color + } elseif {$num_points == 4} { + eval fillQuad $points $color + } else { + # Get the first point in the list as the "base" point of the triangles + set p0 [lindex $points 0] + + for {set i 1} {$i < $num_points - 1} {incr i} { + set p1 [lindex $points $i] + set p2 [lindex $points [expr {$i+1}]] + fillTriangle $p0 $p1 $p2 $color + } + } + } + + variable image [Gpu::pipeline {sampler2D image vec2 imageSize + vec2 pos float radians float scale + fn rotate} { + vec2 a = pos + rotate(-imageSize/2, -radians); + vec2 b = pos + rotate(vec2(imageSize.x, -imageSize.y)/2, -radians); + vec2 c = pos + rotate(imageSize/2, -radians); + vec2 d = pos + rotate(vec2(-imageSize.x, imageSize.y)/2, -radians); + vec2 vertices[4] = vec2[4](a, b, d, c); + return vertices[gl_VertexIndex]; + } {fn invBilinear fn rotate} { + vec2 a = pos + rotate(-imageSize/2, -radians); + vec2 b = pos + rotate(vec2(imageSize.x, -imageSize.y)/2, -radians); + vec2 c = pos + rotate(imageSize/2, -radians); + vec2 d = pos + rotate(vec2(-imageSize.x, imageSize.y)/2, -radians); + vec2 p = gl_FragCoord.xy; + vec2 uv = invBilinear(p, a, b, c, d); + if( max( abs(uv.x-0.5), abs(uv.y-0.5))<0.5 ) { + return texture(image, uv); + } + return vec4(0.0, 0.0, 0.0, 0.0); + }] + variable imCache [dict create] + variable IMCACHE_MAX_IMAGES [- $Gpu::ImageManager::GPU_MAX_IMAGES 1] + proc checkImCacheAndCopyIfNeeded {imDrawSet} { + variable imCache + variable IMCACHE_MAX_IMAGES + + dict for {im v} $imDrawSet { + if {![dict exists $imCache $im]} { continue } + # Check for staleness and remove from cache if so. + lassign [dict get $imCache $im] gim expectedVersion + set version [Heap::folkHeapGetVersion [string map {uint8_t void} [::image_t data_ptr $im]]] + if {$expectedVersion != $version} { + Gpu::ImageManager::freeGpuImage $gim + dict unset imCache $im + } + } + + set notInCache [dictset difference $imDrawSet $imCache] + set notInDrawSet [dictset difference $imCache $imDrawSet] + + set numImagesToCopy [dictset size $notInCache] + if {$numImagesToCopy > 0} { + if {[dictset size $imCache] + $numImagesToCopy > $IMCACHE_MAX_IMAGES} { + set numImagesToEvict \ + [expr {[dictset size $imCache] + $numImagesToCopy - $IMCACHE_MAX_IMAGES}] + + # What can we safely evict? + # - Anything that's stale + # - Anything that's not in use + set numImagesEvicted 0 + dict for {im v} $imCache { + if {$numImagesEvicted == $numImagesToEvict} { break } + + lassign $v gim expectedVersion + set version [Heap::folkHeapGetVersion [string map {uint8_t void} [::image_t data_ptr $im]]] + if {$expectedVersion != $version} { + Gpu::ImageManager::freeGpuImage $gim + dict unset imCache $im + incr numImagesEvicted + continue + } + if {![dict exists $imDrawSet $im]} { + Gpu::ImageManager::freeGpuImage $gim + dict unset imCache $im + incr numImagesEvicted + } + } + } + + dict for {im _} $notInCache { + # TODO: This is unsafe (has a race condition) -- we're + # not locking the image, so version and gim may diverge. + set version [Heap::folkHeapGetVersion [string map {uint8_t void} [::image_t data_ptr $im]]] + set gim [Gpu::ImageManager::copyImageToGpu $im] + dict set imCache $im [list $gim $version] + } + } + } + proc image {x y im radians {scale 1.0}} { + # TODO: Implement scale. + variable image + variable imCache + lassign [dict get $imCache $im] gim + Gpu::draw $image $gim [list [::image width $im] [::image height $im]] \ + [list $x $y] $radians $scale + } + + proc end {} { Gpu::drawEnd; Gpu::poll } + } # TODO: Clean this up. We retract these so that we don't bounce # statements back to the main Folk process that it sends us. @@ -78,10 +441,22 @@ On process { set displayCommands [lmap sublist [lsort -command lcomp $displayList] {lindex $sublist 1}] - set renderTime [baretime [list foreach command $displayCommands { {*}$command }]] - set commitTime [baretime commitThenClearStaging] + set imDrawSet [dictset create] + foreach command $displayCommands { + if {[lindex $command 0] eq "Display::image"} { + set im [lindex $command 3] + dictset add imDrawSet $im + } + } + Display::checkImCacheAndCopyIfNeeded $imDrawSet + + set renderTime [baretime { + Display::start + foreach command $displayCommands { {*}$command } + Display::end + }] - Commit { Claim the display time is "render $renderTime us + commit $commitTime us ($::stepTime)" } + Commit { Claim the display time is "render $renderTime us ($::stepTime)" } Step } } diff --git a/virtual-programs/images.folk b/virtual-programs/images.folk index 927b2da7..bfa5daef 100644 --- a/virtual-programs/images.folk +++ b/virtual-programs/images.folk @@ -1,40 +1,44 @@ - # Example program, i.e the public API # # When $this has camera slice /slice/ { # Wish $this displays camera slice $slice # } -if {$::isLaptop} { return } - # Generic image class for sub-image manipulations namespace eval ::image { proc width {im} { dict get $im width } proc height {im} { dict get $im height } proc subimage {im x y subwidth subheight} { - dict with im { - set x [expr {int($x)}] - set y [expr {int($y)}] - set subdata [expr {$data + ($y*$width + $x) * $components}] - dict create \ - width $subwidth \ - height $subheight \ - components $components \ - bytesPerRow $bytesPerRow \ - data [format 0x%x $subdata] - } + dict with im { + set x [expr {int($x)}] + set y [expr {int($y)}] + set subdata [expr {[lindex $data 1] + ($y*$width + $x) * $components}] + dict create \ + width $subwidth \ + height $subheight \ + components $components \ + bytesPerRow $bytesPerRow \ + data [format "(uint8_t*) 0x%x" $subdata] + } } set cc [c create] c loadlib [expr {$tcl_platform(os) eq "Darwin" ? "/opt/homebrew/lib/libjpeg.dylib" : [lindex [exec /usr/sbin/ldconfig -p | grep libjpeg] end]}] c loadlib [expr {$tcl_platform(os) eq "Darwin" ? "/opt/homebrew/lib/libpng.dylib" : [lindex [exec /usr/sbin/ldconfig -p | grep libpng] end]}] + if {$tcl_platform(os) eq "Darwin"} { + $cc cflags -I/opt/homebrew/include -L/opt/homebrew/lib + } - $cc cflags -ljpeg + $cc cflags -ljpeg -lpng source "pi/cUtils.tcl" defineImageType $cc $cc include <stdlib.h> $cc include <string.h> - $cc import ::Heap::cc folkHeapAlloc as folkHeapAlloc + if {[namespace exists ::Heap]} { + $cc import ::Heap::cc folkHeapAlloc as folkHeapAlloc + } else { + $cc code { #define folkHeapAlloc malloc } + } $cc code { #undef EXTERN @@ -67,7 +71,7 @@ namespace eval ::image { image[i][j * 3 + 2] = data[(i * width + j) * 3 + 2]; } } - } + } else { exit(1); } struct jpeg_compress_struct compress; struct jpeg_error_mgr error; @@ -260,6 +264,7 @@ namespace eval ::image { $cc proc freePng {image_t im} void { // TODO: Free the PNG. } + $cc compile variable imagesCache [dict create] diff --git a/virtual-programs/tags-and-calibration.folk b/virtual-programs/tags-and-calibration.folk index b219b541..8bfa067c 100644 --- a/virtual-programs/tags-and-calibration.folk +++ b/virtual-programs/tags-and-calibration.folk @@ -33,6 +33,10 @@ When camera /camera/ has width /cameraWidth/ height /cameraHeight/ { [expr {double($dy)/$generatedCalibration::displayHeight*$Display::HEIGHT}]] } } + if {[llength $points] < 4} { + puts stderr "tags-and-calibration: Calibration isn't valid (not enough points). Stopping Folk." + exit 1 + } for {set i 0} {$i < [llength $points]} {incr i} { lassign [lindex $points $i] x$i y$i u$i v$i } |
