sys/src/cmd/bitsy/pencal.c

   1 #include <u.h>
   2 #include <libc.h>
   3 #include <draw.h>
   4
   5 int debug = 0;
   6
   7 struct Cal {
   8         double scalex;
   9         double transx;
  10         double scaley;
  11         double transy;
  12 } cal;
  13
  14 /* Calibrate the pen */
  15
  16 Point pts[5];
  17
  18 Point pens[5];
  19
  20 Point up        = {  0, -10};
  21 Point down      = {  0,  10};
  22 Point left      = {-10,   0};
  23 Point right     = { 10,   0};
  24
  25 Point
  26 pen2scr(Point p)
  27 {
  28         return Pt(p.x * cal.scalex / 65536.0 + cal.transx,
  29                           p.y * cal.scaley / 65536.0 + cal.transy);
  30 }
  31
  32 Point
  33 scr2pen(Point p)
  34 {
  35         return Pt((p.x - cal.transx) * 65536.0 / cal.scalex,
  36                           (p.y + cal.transy) * 65536.0 / cal.scaley);
  37 }
  38
  39 void
  40 cross(Point p) {
  41         draw(screen, screen->r, display->white, nil, ZP);
  42         line(screen, addpt(p, up), addpt(p, down), Endsquare, Endsquare, 0, display->black, ZP);
  43         line(screen, addpt(p, left), addpt(p, right), Endsquare, Endsquare, 0, display->black, ZP);
  44         flushimage(display, 1);
  45 }
  46
  47 Point
  48 getmouse(int mouse) {
  49         static char buf[50];
  50         int x, y, b, n;
  51         Point p;
  52
  53         n = 0;
  54         p.x = p.y = 0;
  55         do{
  56                 if (read(mouse, buf, 48) != 48)
  57                         sysfatal("read: %r");
  58                 if (debug > 1) fprint(2, "%s\n", buf);
  59                 if (buf[0] == 'r') {
  60                         b = 1;
  61                         continue;
  62                 }
  63                 if (buf[0] != 'm')
  64                         sysfatal("mouse message: %s", buf);
  65                 x = strtol(buf+1, nil, 10);
  66                 y = strtol(buf+13, nil, 10);
  67                 b = strtol(buf+25, nil, 10);
  68                 p.x = (n * p.x + x)/(n+1);
  69                 p.y = (n * p.y + y)/(n+1);
  70                 n++;
  71         }while (b & 0x7);
  72         return p;
  73 }
  74
  75 void
  76 main(int argc, char **argv) {
  77         int i, mouse, mousectl, wctl, ntries;
  78         Rectangle r, oldsize;
  79
  80         ARGBEGIN{
  81         case 'd':
  82                 debug = 1;
  83                 break;
  84         }ARGEND;
  85
  86         if((mouse = open("/dev/mouse", OREAD)) < 0
  87          && (mouse = open("#m/mouse", OREAD)) < 0)
  88                 sysfatal("#m/mouse: %r");
  89
  90         mousectl = open("#m/mousectl", ORDWR);
  91         if(mousectl < 0)
  92                 sysfatal("#m/mousectl: %r");
  93
  94         if(initdraw(nil, nil, "calibrate") < 0)
  95                 sysfatal("initdraw: %r");
  96
  97         wctl = -1;
  98         for(ntries = 0; ntries < 3; ntries++){
  99                 r = insetrect(display->image->r, 2);
 100
 101                 if(wctl < 0)
 102                         wctl = open("/dev/wctl", ORDWR);
 103                 if(wctl >= 0){
 104                         char buf[4*12+1];
 105
 106                         buf[48] = 0;
 107                         oldsize = screen->r;
 108                         if(fprint(wctl, "resize -r %R", r) <= 0)
 109                                 sysfatal("write /dev/wctl, %r");
 110                         if(getwindow(display, Refbackup) < 0)
 111                                 sysfatal("getwindow");
 112                         if(debug) fprint(2, "resize: %R\n", screen->r);
 113
 114                         if(read(mousectl, buf, 48) != 48)
 115                                 sysfatal("read mousectl: %r");
 116                         if(debug > 1) fprint(2, "mousectl %s\n", buf);
 117                         if(buf[0] != 'c')
 118                                 sysfatal("mousectl message: %s", buf);
 119                         cal.scalex = strtol(buf+ 1, nil, 10);
 120                         cal.scaley = strtol(buf+13, nil, 10);
 121                         cal.transx = strtol(buf+25, nil, 10);
 122                         cal.transy = strtol(buf+37, nil, 10);
 123                 }else{
 124                         fprint(mousectl, "calibrate");
 125                         cal.scalex = 1<<16;
 126                         cal.scaley = 1<<16;
 127                         cal.transx = 0;
 128                         cal.transy = 0;
 129                 }
 130
 131                 fprint(2, "calibrate %ld %ld %ld %ld (old)\n",
 132                         (long)cal.scalex, (long)cal.scaley, (long)cal.transx, (long)cal.transy);
 133
 134                 if(debug)
 135                         fprint(2, "screen coords %R\n", screen->r);
 136                 pts[0] = addpt(Pt( 16,  16), screen->r.min);
 137                 pts[1] = addpt(Pt( 16, -16), Pt(screen->r.min.x, screen->r.max.y));
 138                 pts[2] = addpt(Pt(-16,  16), Pt(screen->r.max.x, screen->r.min.y));
 139                 pts[3] = addpt(Pt(-16, -16), screen->r.max);
 140                 pts[4] = Pt((screen->r.max.x + screen->r.min.x)/2,
 141                                         (screen->r.max.y + screen->r.min.y)/2);;
 142
 143                 for(i = 0; i < 4; i++){
 144                         cross(pts[i]);
 145                         pens[i] = scr2pen(getmouse(mouse));
 146                         if (debug) fprint(2, "%P\n", pens[i]);
 147                 }
 148
 149                 cal.scalex = (pts[2].x + pts[3].x - pts[0].x - pts[1].x) * 65536.0 /
 150                                   (pens[2].x + pens[3].x - pens[0].x - pens[1].x);
 151                 cal.scaley = (pts[1].y + pts[3].y - pts[0].y - pts[2].y) * 65536.0 /
 152                                   (pens[1].y + pens[3].y - pens[0].y - pens[2].y);
 153                 cal.transx = pts[0].x - (pens[0].x*cal.scalex) / 65536.0;
 154                 cal.transy = pts[0].y - (pens[0].y*cal.scaley) / 65536.0;
 155
 156                 if(debug)
 157                         fprint(2, "scale [%f, %f], trans [%f, %f]\n",
 158                                 cal.scalex, cal.scaley, cal.transx, cal.transy);
 159
 160                 fprint(mousectl, "calibrate %ld %ld %ld %ld",
 161                         (long)cal.scalex, (long)cal.scaley, (long)cal.transx, (long)cal.transy);
 162
 163                 cross(pts[4]);
 164                 pens[4] = getmouse(mouse);
 165
 166                 draw(screen, screen->r, display->white, nil, ZP);
 167                 flushimage(display, 1);
 168
 169                 if(abs(pts[4].x-pens[4].x) <= 4 && abs(pts[4].y-pens[4].y) <= 4){
 170                         fprint(2, "Calibration ok: %P -> %P\n", pts[4], pens[4]);
 171                         break;
 172                 }
 173                 fprint(2, "Calibration inaccurate: %P -> %P\n", pts[4], pens[4]);
 174         }
 175         print("calibrate='%ld %ld %ld %ld'\n",
 176                 (long)cal.scalex, (long)cal.scaley, (long)cal.transx, (long)cal.transy);
 177
 178         if(debug)
 179                 for(i = 0; i < 4; i++){
 180                         cross(pts[i]);
 181                         pens[i] = getmouse(mouse);
 182                         if (debug) fprint(2, "%P\n", pens[i]);
 183                 }
 184
 185         if(wctl >= 0)
 186                 fprint(wctl, "resize -r %R", oldsize);
 187
 188 }