ref: e59ffed426f628794d4669f152eff9a6239b99db
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";
}