ref: 1018d7a7091fd5e0433f501a3474cdc3fec322cc
dir: /appl/cmd/touchcal.b/
implement Touchcal; # # calibrate a touch screen # # Copyright © 2001 Vita Nuova Holdings Limited. All rights reserved. # include "sys.m"; sys: Sys; include "draw.m"; draw: Draw; Display, Font, Image, Point, Pointer, Rect: import draw; include "tk.m"; include "wmclient.m"; wmclient: Wmclient; Window: import wmclient; include "translate.m"; translate: Translate; Dict: import translate; Touchcal: module { init: fn(ctxt: ref Draw->Context, argv: list of string); }; Margin: con 20; prompt:= "Please tap the centre\nof the cross\nwith the stylus"; mousepid := 0; init(ctxt: ref Draw->Context, args: list of string) { r: Rect; disp: ref Image; if(args != nil) args = tl args; debug := args != nil && hd args == "-d"; sys = load Sys Sys->PATH; draw = load Draw Draw->PATH; if(draw == nil) err(sys->sprint("no Draw module: %r")); sys->pctl(Sys->NEWPGRP|Sys->FORKFD, nil); translate = load Translate Translate->PATH; if(translate != nil){ translate->init(); (dict, nil) := translate->opendict(translate->mkdictname("", "touchcal")); if(dict != nil) prompt = dict.xlate(prompt); dict = nil; translate = nil; } display: ref Display; win: ref Window; ptr: chan of ref Pointer; if(ctxt != nil){ display = ctxt.display; wmclient = load Wmclient Wmclient->PATH; if(wmclient == nil) err(sys->sprint("cannot load %s: %r", Wmclient->PATH)); wmclient->init(); win = wmclient->window(ctxt, "Touchcal", Wmclient->Plain); win.reshape(ctxt.display.image.r); ptr = chan of ref Pointer; win.onscreen("exact"); win.startinput("ptr"::nil); pidc := chan of int; ptr = win.ctxt.ptr; display = ctxt.display; disp = win.image; r = disp.r; }else{ # standalone, catch them ourselves display = draw->Display.allocate(nil); disp = display.image; r = disp.r; mfd := sys->open("/dev/pointer", Sys->OREAD); if(mfd == nil) err(sys->sprint("can't open /dev/pointer: %r")); pidc := chan of int; ptr = chan of ref Pointer; spawn rawmouse(mfd, ptr, pidc); mousepid = <-pidc; } white := display.white; black := display.black; red := display.color(Draw->Red); disp.draw(r, white, nil, r.min); samples := array[4] of Point; points := array[4] of Point; points[0] = (r.min.x+Margin, r.min.y+Margin); points[1] = (r.max.x-Margin, r.min.y+Margin); points[2] = (r.max.x-Margin, r.max.y-Margin); points[3] = (r.min.x+Margin, r.max.y-Margin); midpoint := Point((r.min.x+r.max.x)/2, (r.min.y+r.max.y)/2); refx := FX((points[1].x - points[0].x) + (points[2].x - points[3].x), 1); refy := FX((points[3].y - points[0].y) + (points[2].y - points[1].y), 1); ctl := sys->open("/dev/touchctl", Sys->ORDWR); if(ctl == nil) ctl = sys->open("/dev/null", Sys->ORDWR); if(ctl == nil) err(sys->sprint("can't open /dev/touchctl: %r")); #oldvalues := array[128] of byte; #nr := sys->read(ctl, oldvalues, len oldvalues); #if(nr < 0) # err(sys->sprint("can't read old values from /dev/touchctl: %r")); #oldvalues = oldvalues[0:nr]; sys->fprint(ctl, "X %d %d %d\nY %d %d %d\n", FX(1,1), 0, 0, 0, FX(1,1), 0); # identity font := Font.open(display, sys->sprint("/fonts/lucida/unicode.%d.font", 6+(r.dx()/512))); if(font == nil) font = Font.open(display, "*default*"); if(font != nil){ drawtext(disp, midpoint, black, font, prompt); font = nil; } for(;;) { tm := array[] of {0 to 2 =>array[] of {0, 0, 0}}; for(i := 0; i < 4; i++){ cross(disp, points[i], red); samples[i] = getpoint(ptr); cross(disp, points[i], white); } # first, rotate if necessary rotate := 0; if(abs(samples[1].x-samples[2].x) > 80 && abs(samples[2].y-samples[3].y) > 80){ rotate = 1; for(i = 0; i < len samples; i++) samples[i] = (samples[i].y, samples[i].x); } # calculate scaling and offset transformations actx := (samples[1].x-samples[0].x)+(samples[2].x-samples[3].x); acty := (samples[3].y-samples[0].y)+(samples[2].y-samples[1].y); if(actx == 0 || acty == 0) continue; # either the user or device is not trying tm[0][rotate] = refx/actx; tm[0][2] = FX(points[0].x - XF(tm[0][rotate]*samples[0].x), 1); tm[1][1-rotate] = refy/acty; tm[1][2] = FX(points[0].y - XF(tm[1][1-rotate]*samples[0].y), 1); cross(disp, midpoint, red); m := getpoint(ptr); cross(disp, midpoint, white); p := Point(ptmap(tm[0], m.x, m.y), ptmap(tm[1], m.x, m.y)); if(debug){ for(k:=0; k<4; k++) sys->print("%d %d,%d %d,%d\n", k, points[k].x,points[k].y, samples[k].x, samples[k].y); if(rotate) sys->print("rotated\n"); sys->print("rx=%d ax=%d ry=%d ay=%d tm[0][0]=%d\n", refx, actx, refy, acty, tm[0][0]); sys->print("%g %g %g\n%g %g %g\n", G(tm[0][0]), G(tm[0][1]), G(tm[0][2]), G(tm[1][0]), G(tm[1][1]), G(tm[1][2])); sys->print("%d %d -> %d %d (%d %d)\n", m.x, m.y, p.x, p.y, midpoint.x, midpoint.y); } if(abs(p.x-midpoint.x) > 5 || abs(p.y-midpoint.y) > 5) continue; printmat(sys->fildes(1), tm); if(debug || printmat(ctl, tm) >= 0){ disp.draw(r, white, nil, r.min); break; } sys->fprint(sys->fildes(2), "touchcal: can't set calibration: %r\n"); } if(mousepid > 0) kill(mousepid); } printmat(fd: ref Sys->FD, tm: array of array of int): int { return sys->fprint(fd, "X %d %d %d\nY %d %d %d\n", tm[0][0], tm[0][1], tm[0][2], tm[1][0], tm[1][1], tm[1][2]); } FX(a, b: int): int { return (a << 16)/b; } XF(v: int): int { return v>>16; } G(v: int): real { return real v / 65536.0; } ptmap(m: array of int, x, y: int): int { return XF(m[0]*x + m[1]*y + m[2]); } rawmouse(fd: ref Sys->FD, mc: chan of ref Pointer, pidc: chan of int) { pidc <-= sys->pctl(0, nil); buf := array[64] of byte; for(;;){ n := sys->read(fd, buf, len buf); if(n <= 0) err(sys->sprint("can't read /dev/pointer: %r")); if(int buf[0] != 'm' || n < 1+3*12) continue; x := int string buf[ 1:13]; y := int string buf[12:25]; b := int string buf[24:37]; mc <-= ref Pointer(b, (x,y), 0); } } getpoint(mousec: chan of ref Pointer): Point { p := Point(0,0); while((m := <-mousec).buttons == 0) p = m.xy; n := 0; do{ if(abs(p.x-m.xy.x) > 10 || abs(p.y-m.xy.y) > 10){ n = 0; p = m.xy; }else{ p = p.mul(n).add(m.xy).div(n+1); n++; } }while((m = <-mousec).buttons & 7); return p; } cross(im: ref Image, p: Point, col: ref Image) { im.line(p.sub((0,10)), p.add((0,10)), Draw->Endsquare, Draw->Endsquare, 0, col, col.r.min); im.line(p.sub((10,0)), p.add((10,0)), Draw->Endsquare, Draw->Endsquare, 0, col, col.r.min); im.flush(Draw->Flushnow); } drawtext(im: ref Image, p: Point, col: ref Image, font: ref Font, text: string) { (n, lines) := sys->tokenize(text, "\n"); p = p.sub((0, (n+1)*font.height)); for(; lines != nil; lines = tl lines){ s := hd lines; w := font.width(s); im.text(p.sub((w/2, 0)), col, col.r.min, font, s); p = p.add((0, font.height)); } } abs(x: int): int { if(x < 0) return -x; return x; } kill(pid: int) { fd := sys->open("#p/"+string pid+"/ctl", Sys->OWRITE); if(fd != nil) sys->fprint(fd, "kill"); } err(s: string) { sys->fprint(sys->fildes(2), "touchcal: %s\n", s); if(mousepid > 0) kill(mousepid); raise "fail:touch"; }