cinelerra-5.1/plugins/motion51/motion51.C

   1 #include "affine.h"
   2 #include "bchash.h"
   3 #include "clip.h"
   4 #include "filexml.h"
   5 #include "fourier.h"
   6 #include "keyframe.h"
   7 #include "language.h"
   8 #include "mainerror.h"
   9 #include "motion51.h"
  10 #include "motionwindow51.h"
  11 #include "mutex.h"
  12 #include "transportque.inc"
  13
  14 #include <errno.h>
  15 #include <math.h>
  16 #include <unistd.h>
  17
  18 static const double passing = 0.92;
  19 static bool passible(double v) { return v > passing; }
  20
  21
  22 REGISTER_PLUGIN(Motion51Main)
  23
  24 void Motion51Config::init()
  25 {
  26         sample_steps = 1024;
  27         sample_r = 50.f;
  28         block_x = block_y = 50.f;
  29         block_w = block_h = 50.f;
  30         horiz_limit = vert_limit = twist_limit = 50.f;
  31         shake_fade = twist_fade = 3.f;
  32         draw_vectors = 1;
  33         strcpy(tracking_file, TRACKING_FILE);
  34         tracking = 0;
  35 }
  36
  37 Motion51Config::Motion51Config()
  38 {
  39         init();
  40 }
  41
  42 int Motion51Config::equivalent(Motion51Config &that)
  43 {
  44         return horiz_limit == that.horiz_limit &&
  45                 vert_limit == that.vert_limit &&
  46                 twist_limit == that.twist_limit &&
  47                 shake_fade == that.shake_fade &&
  48                 twist_fade == that.twist_fade &&
  49                 sample_r == that.sample_r &&
  50                 sample_steps == that.sample_steps &&
  51                 draw_vectors == that.draw_vectors &&
  52                 EQUIV(block_x, that.block_x) &&
  53                 EQUIV(block_y, that.block_y) &&
  54                 block_w == that.block_w &&
  55                 block_h == that.block_h &&
  56                 !strcmp(tracking_file, that.tracking_file) &&
  57                 tracking == that.tracking;
  58 }
  59
  60 void Motion51Config::copy_from(Motion51Config &that)
  61 {
  62         horiz_limit = that.horiz_limit;
  63         vert_limit = that.vert_limit;
  64         twist_limit = that.twist_limit;
  65         shake_fade = that.shake_fade;
  66         twist_fade = that.twist_fade;
  67         sample_r = that.sample_r;
  68         sample_steps = that.sample_steps;
  69         draw_vectors = that.draw_vectors;
  70         block_x = that.block_x;
  71         block_y = that.block_y;
  72         block_w = that.block_w;
  73         block_h = that.block_h;
  74         strcpy(tracking_file, that.tracking_file);
  75         tracking = that.tracking;
  76 }
  77
  78 void Motion51Config::interpolate(Motion51Config &prev, Motion51Config &next,
  79         int64_t prev_frame, int64_t next_frame, int64_t current_frame)
  80 {
  81         copy_from(prev);
  82 }
  83
  84
  85 Motion51Main::Motion51Main(PluginServer *server)
  86  : PluginVClient(server)
  87 {
  88         out_frame = 0;  out_position = -1;
  89         ref_frame = 0;  ref_position = -1;
  90         tmp_frame = 0;
  91         affine = 0;
  92         motion_scan = 0;
  93
  94         cache_file[0] = 0;
  95         cache_fp = active_fp = 0;
  96         cache_line[0] = 0;
  97         cache_key = active_key = -1;
  98         tracking_position = -1;
  99
 100         out_w = out_h = out_r = 0;
 101         rx = ry = rw = rh = rr = 0;
 102         current_dx = current_dy = 0;
 103         x_steps = y_steps = 16;
 104         r_steps = 4;
 105         cir_sz = 0;  cir_r = 0;
 106         xpts = ypts = 0;
 107         total_dx = total_dy = 0;
 108         total_angle = 0;
 109 }
 110
 111 Motion51Main::~Motion51Main()
 112 {
 113         update_cache_file();
 114         delete out_frame;
 115         delete ref_frame;
 116         delete tmp_frame;
 117         delete affine;
 118         delete motion_scan;
 119         delete [] xpts;
 120         delete [] ypts;
 121 }
 122
 123 const char* Motion51Main::plugin_title() { return _("Motion51"); }
 124 int Motion51Main::is_realtime() { return 1; }
 125 int Motion51Main::is_multichannel() { return 1; }
 126
 127
 128 NEW_WINDOW_MACRO(Motion51Main, Motion51Window)
 129 LOAD_CONFIGURATION_MACRO(Motion51Main, Motion51Config)
 130
 131
 132 void Motion51Main::update_gui()
 133 {
 134         if( !thread ) return;
 135         if( !load_configuration() ) return;
 136         thread->window->lock_window("Motion51Main::update_gui");
 137         Motion51Window *window = (Motion51Window*)thread->window;
 138         window->update_gui();
 139         thread->window->unlock_window();
 140 }
 141
 142
 143
 144
 145 void Motion51Main::save_data(KeyFrame *keyframe)
 146 {
 147         FileXML output;
 148
 149 // cause data to be stored directly in text
 150         output.set_shared_output(keyframe->get_data(), MESSAGESIZE);
 151         output.tag.set_title("MOTION51");
 152         output.tag.set_property("HORIZ_LIMIT", config.horiz_limit);
 153         output.tag.set_property("VERT_LIMIT", config.vert_limit);
 154         output.tag.set_property("TWIST_LIMIT", config.twist_limit);
 155         output.tag.set_property("SHAKE_FADE", config.shake_fade);
 156         output.tag.set_property("TWIST_FADE", config.twist_fade);
 157         output.tag.set_property("SAMPLE_R", config.sample_r);
 158         output.tag.set_property("SAMPLE_STEPS", config.sample_steps);
 159         output.tag.set_property("DRAW_VECTORS", config.draw_vectors);
 160         output.tag.set_property("BLOCK_W", config.block_w);
 161         output.tag.set_property("BLOCK_H", config.block_h);
 162         output.tag.set_property("BLOCK_X", config.block_x);
 163         output.tag.set_property("BLOCK_Y", config.block_y);
 164         output.tag.set_property("TRACKING_FILE", config.tracking_file);
 165         output.tag.set_property("TRACKING", config.tracking);
 166         output.append_tag();
 167         output.tag.set_title("/MOTION51");
 168         output.append_tag();
 169         output.terminate_string();
 170 }
 171
 172 void Motion51Main::read_data(KeyFrame *keyframe)
 173 {
 174         FileXML input;
 175         input.set_shared_input(keyframe->get_data(), strlen(keyframe->get_data()));
 176         int result = 0;
 177
 178         while( !(result = input.read_tag()) ) {
 179                 if( input.tag.title_is("MOTION51") ) {
 180                         config.horiz_limit = input.tag.get_property("HORIZ_LIMIT", config.horiz_limit);
 181                         config.vert_limit = input.tag.get_property("VERT_LIMIT", config.vert_limit);
 182                         config.twist_limit = input.tag.get_property("TWIST_LIMIT", config.twist_limit);
 183                         config.shake_fade = input.tag.get_property("SHAKE_FADE", config.shake_fade);
 184                         config.twist_fade = input.tag.get_property("TWIST_FADE", config.twist_fade);
 185                         config.sample_r = input.tag.get_property("SAMPLE_R", config.sample_r);
 186                         config.sample_steps = input.tag.get_property("SAMPLE_STEPS", config.sample_steps);
 187                         config.draw_vectors = input.tag.get_property("DRAW_VECTORS", config.draw_vectors);
 188                         config.block_w = input.tag.get_property("BLOCK_W", config.block_w);
 189                         config.block_h = input.tag.get_property("BLOCK_H", config.block_h);
 190                         config.block_x = input.tag.get_property("BLOCK_X", config.block_x);
 191                         config.block_y = input.tag.get_property("BLOCK_Y", config.block_y);
 192                         input.tag.get_property("TRACKING_FILE", config.tracking_file);
 193                         config.tracking = input.tag.get_property("TRACKING", config.tracking);
 194                 }
 195         }
 196 }
 197
 198 #if 0
 199 static void snap(const char *fn, VFrame *img, float x, float y, float r)
 200 {
 201         VFrame vfrm(img->get_w(),img->get_h(),img->get_color_model());
 202         vfrm.copy_from(img);
 203         vfrm.draw_smooth(x-r,y, x-r,y+r, x,y+r);
 204         vfrm.draw_smooth(x,y+r, x+r,y+r, x+r,y);
 205         vfrm.draw_smooth(x+r,y, x+r,y-r, x,y-r);
 206         vfrm.draw_smooth(x,y-r, x-r,y-r, x-r,y);
 207         vfrm.write_png(fn);
 208 }
 209 #endif
 210
 211 #if 0
 212 // nearest sample
 213 static void nearest_uint8(double *pix[3], double rx, double ry,
 214                 uint8_t **rows, int psz, int iw1, int ih1)
 215 {
 216         int ix = (int)rx, iy = (int)ry;
 217         bclamp(ix, 0, iw1);  bclamp(iy, 0, ih1);
 218         uint8_t *cp = (uint8_t*)(rows[iy] + psz * ix);
 219         for( int i=0; i<3; ++pix[i], ++cp, ++i ) *pix[i] = *cp;
 220 }
 221 static void nearest_float(double *pix[3], double rx, double ry,
 222                 uint8_t **rows, int psz, int iw1, int ih1)
 223 {
 224         int ix = (int)rx, iy = (int)ry;
 225         bclamp(ix, 0, iw1);  bclamp(iy, 0, ih1);
 226         float *fp = (float*)(rows[iy] + psz * ix);
 227         for( int i=0; i<3; ++pix[i], ++fp, ++i ) *pix[i] = *fp;
 228 }
 229 #endif
 230
 231 // corner interpolation sample
 232 static void corner_uint8(double *pix[3], double rx, double ry,
 233                 uint8_t **rows, int psz, int iw1, int ih1)
 234 {
 235         bclamp(rx, 0, iw1);  bclamp(ry, 0, ih1);
 236         int iy = (int)ry;
 237         double yf1 = ry - iy, yf0 = 1.0 - yf1;
 238         uint8_t *row0 = rows[iy];
 239         if( iy < ih1 ) ++iy;
 240         uint8_t *row1 = rows[iy];
 241         int ix = (int)rx;
 242         double xf1 = rx - ix, xf0 = 1.0 - xf1;
 243         int i0 = psz * ix;
 244         if( ix < iw1 ) ++ix;
 245         int i1 = psz * ix;
 246         uint8_t *cp00 = (uint8_t*)&row0[i0], *cp01 = (uint8_t*)&row0[i1];
 247         uint8_t *cp10 = (uint8_t*)&row1[i0], *cp11 = (uint8_t*)&row1[i1];
 248         double a00 = xf0 * yf0, a01 = xf1 * yf0;
 249         double a10 = xf0 * yf1, a11 = xf1 * yf1;
 250         for( int i=0; i<3; ++pix[i], ++cp00, ++cp01, ++cp10, ++cp11, ++i )
 251                 *pix[i] = *cp00*a00 + *cp01*a01 + *cp10*a10 + *cp11*a11;
 252 }
 253 static void corner_float(double *pix[3], double rx, double ry,
 254                 uint8_t **rows, int psz, int iw1, int ih1)
 255 {
 256         bclamp(rx, 0, iw1);  bclamp(ry, 0, ih1);
 257         int iy = (int)ry;
 258         double yf1 = ry - iy, yf0 = 1.0 - yf1;
 259         uint8_t *row0 = rows[iy];
 260         if( iy < ih1 ) ++iy;
 261         uint8_t *row1 = rows[iy];
 262         int ix = (int)rx;
 263         double xf1 = rx - ix, xf0 = 1.0 - xf1;
 264         int i0 = psz * ix;
 265         if( ix < iw1 ) ++ix;
 266         int i1 = psz * ix;
 267         float *fp00 = (float*)&row0[i0], *fp01 = (float*)&row0[i1];
 268         float *fp10 = (float*)&row1[i0], *fp11 = (float*)&row1[i1];
 269         double a00 = xf0 * yf0, a01 = xf1 * yf0;
 270         double a10 = xf0 * yf1, a11 = xf1 * yf1;
 271         for( int i=0; i<3; ++pix[i], ++fp00, ++fp01, ++fp10, ++fp11, ++i )
 272                 *pix[i] = *fp00*a00 + *fp01*a01 + *fp10*a10 + *fp11*a11;
 273 }
 274
 275
 276 static inline double cor(int n, double *ap, double *bp)
 277 {
 278         double s = 0;
 279         while( --n >= 0 ) s += *ap++ * *bp++;
 280         return s;
 281 }
 282
 283 static inline double sqr(double v) { return v*v; }
 284
 285 static inline void cj_product(int n, int sf, double *rp, double *ip,
 286                 double *arp, double *aip, double *brp, double *bip)
 287 {
 288         int m = !sf ? n-1 : n/2, i = 0;
 289         while( i <= m ) {
 290                 double ar = arp[i], ai = aip[i];
 291                 double br = brp[i], bi = -bip[i];
 292                 rp[i] = ar*br - ai*bi;  // complex a*ib'
 293                 ip[i] = ar*bi + ai*br;
 294                 ++i;
 295         }
 296         if( !sf ) return;
 297         while( --m > 0 ) { rp[i] = rp[m];  ip[i] = -ip[m];  ++i; }
 298 }
 299
 300 typedef struct { double x, y, d; } coord_t;
 301 static int coord_cmpr(const void *ap, const void *bp)
 302 {
 303         coord_t *a = (coord_t *)ap, *b = (coord_t *)bp;
 304         return a->d == b->d ? 0 : a->d < b->d ? -1 : 1;
 305 }
 306
 307
 308 int64_t Motion51Main::get_ref_position()
 309 {
 310         int64_t position = out_position - 1;
 311         if( position < 0 || position != ref_position ) {
 312 // clip to edit boundaries
 313                 int64_t pos = get_source_start();
 314                 if( position < pos )
 315                         position = pos;
 316                 else if( position >= (pos += get_total_len()) )
 317                         position = pos-1;
 318 // clip to keyframe boundaries
 319                 KeyFrame *next_keyframe = get_next_keyframe(out_position, 1);
 320                 int64_t keyframe_end = next_keyframe->position;
 321                 if( (pos=next_keyframe->position) > 0 && position > keyframe_end )
 322                         position = keyframe_end;
 323                 KeyFrame *prev_keyframe = get_prev_keyframe(out_position, 1);
 324                 int64_t keyframe_start = prev_keyframe->position;
 325                 if( keyframe_start > 0 && position < keyframe_start )
 326                         position = keyframe_start;
 327         }
 328         return position;
 329 }
 330
 331 void Motion51Main::set_tracking_path()
 332 {
 333         const char *sp = TRACKING_FILE;
 334         char *cp = config.tracking_file, *ep = cp+sizeof(config.tracking_file)-1;
 335         while( cp < ep && *sp != 0 ) *cp++ = *sp++;
 336         if( cp < ep && (sp=get_source_path()) ) {
 337                 *cp++ = '-';
 338                 const char *bp = strrchr(sp,'/');
 339                 if( bp ) sp = bp+1;
 340                 while( cp < ep && *sp != 0 ) {
 341                         *cp++ = (*sp>='a' && *sp<='z') ||
 342                                 (*sp>='A' && *sp<='Z') ||
 343                                 (*sp>='0' && *sp<='9') ? *sp : '_';
 344                         ++sp;
 345                 }
 346         }
 347         *cp = 0;
 348 }
 349
 350 void Motion51Main::update_tracking_cache()
 351 {
 352         if( (!config.tracking && cache_fp) || (config.tracking && !cache_fp) ||
 353             (active_fp && active_key > get_source_position()) )
 354                 update_cache_file();
 355 }
 356
 357 int Motion51Main::load_tracking_cache(int64_t position)
 358 {
 359         if( !config.tracking ) return 1;
 360         if( get_cache_line(position) ) return 1;
 361         if( sscanf(cache_line, "%jd %f %f %f", &position, &dx, &dy, &dt) != 4 ) return 1;
 362         return 0;
 363 }
 364
 365 void Motion51Main::save_tracking_cache(int64_t position)
 366 {
 367         if( !config.tracking ) return;
 368         char line[BCSTRLEN];
 369         snprintf(line, sizeof(line), "%jd %f %f %f\n", position, dx, dy, dt);
 370         put_cache_line(line);
 371 }
 372
 373 void Motion51Main::match(VFrame *ref, VFrame *cur)
 374 {
 375         if( !motion_scan ) {
 376                 int cpus = get_project_smp()+1;
 377                 motion_scan = new Motion51Scan(this, cpus, x_steps, y_steps);
 378         }
 379
 380         motion_scan->scan(ref, cur, config.sample_steps);
 381
 382         if( passible(motion_scan->cor_value) ) {
 383                 dx = motion_scan->dx_result - rx;
 384                 dy = motion_scan->dy_result - ry;
 385                 dt = motion_scan->dt_result * 180./M_PI;
 386         }
 387         else {
 388                 total_dx = total_dy = total_angle = 0;
 389                 dx = dy = dt = 0;
 390         }
 391
 392 }
 393
 394 int Motion51Main::transform_target(int use_opengl)
 395 {
 396         if( dx || dy || dt ) {
 397                 int cpus = get_project_smp()+1;
 398                 if( !affine ) affine = new AffineEngine(cpus, cpus);
 399                 affine->set_in_pivot(rx, ry);
 400                 affine->set_out_pivot(rx-dx, ry-dy);
 401                 if( use_opengl )
 402                         return run_opengl();
 403                 new_temp(out_frame, out)->copy_from(out);
 404                 out->clear_frame();
 405                 affine->rotate(out, out_frame, dt);
 406 //printf("transform_target at %jd: rotate(%f, %f, %f)\n", out_position, dx, dy, dt);
 407         }
 408         return 0;
 409 }
 410
 411 int Motion51Main::handle_opengl()
 412 {
 413 #ifdef HAVE_GL
 414         affine->set_opengl(1);
 415         affine->rotate(out, out, dt);
 416         out->screen_to_ram();
 417         affine->set_opengl(0);
 418 #endif
 419         return 0;
 420 }
 421
 422
 423 int Motion51Main::process_buffer(VFrame **frame, int64_t position, double frame_rate)
 424 {
 425         int need_reconfigure = load_configuration();
 426
 427         int target_layer = 0;
 428         int reference_layer = PluginClient::total_in_buffers-1;
 429         VFrame *ref_layer = frame[reference_layer];
 430         out = frame[target_layer];
 431         out_position = position;
 432         if( !out_position ) total_dx = total_dy = total_angle = 0;
 433         get_pixel = BC_CModels::is_float(out->get_color_model()) ?
 434                 &corner_float : &corner_uint8;
 435
 436         int use_opengl = get_use_opengl();
 437         read_frame(out, target_layer, out_position, frame_rate, use_opengl);
 438         out_w = out->get_w();
 439         out_h = out->get_h();
 440         out_r = 0.5 * (out_w < out_h ? out_w : out_h);
 441         rw = out_w * config.block_w/100.;
 442         rh = out_h * config.block_h/100.;
 443         rx = out_w * config.block_x/100.;
 444         ry = out_h * config.block_y/100.;
 445         rr = out_r * config.sample_r/100.;
 446         reset_sample(config.sample_steps, rr);
 447         dx = 0;  dy = 0;  dt = 0;
 448
 449         update_tracking_cache();
 450         if( load_tracking_cache(out_position) ) {
 451                 int64_t ref_pos = get_ref_position();
 452                 if( !ref_frame || ref_pos != ref_position || need_reconfigure ) {
 453                         new_temp(ref_frame, ref_layer);
 454                         read_frame(ref_frame, reference_layer, ref_pos, frame_rate, 0);
 455                         total_dx = total_dy = 0;  total_angle = 0;
 456                         ref_position = ref_pos;
 457                 }
 458                 VFrame *cur_frame = out;
 459                 if( reference_layer != target_layer ) {
 460                         new_temp(tmp_frame, ref_layer);
 461                         read_frame(tmp_frame, reference_layer, out_position, frame_rate, 0);
 462                         cur_frame = tmp_frame;
 463                 }
 464                 match(ref_frame, cur_frame);
 465                 save_tracking_cache(out_position);
 466         }
 467         current_dx = dx;  current_dy = dy;
 468         double sf = 1. - config.shake_fade/100.;
 469         dx += total_dx * sf;  dy += total_dy * sf;
 470         double rf = 1. - config.twist_fade/100.;
 471         dt += total_angle * rf;
 472         if( dt < -180. ) dt += 360.;
 473         else if( dt > 180. ) dt -= 360.;
 474
 475         float tot_dx = out_w * config.horiz_limit/100.;
 476         bclamp(dx, -tot_dx, tot_dx);
 477         float tot_dy = out_h * config.vert_limit/100.;
 478         bclamp(dy, -tot_dy, tot_dy);
 479         float tot_dt = 180. * config.twist_limit/100.;
 480         bclamp(dt, -tot_dt, +tot_dt);
 481         total_dx = dx;  total_dy = dy;  total_angle = dt;
 482         if( ref_frame && reference_layer == target_layer &&
 483             ref_position+1 == out_position &&
 484             ref_frame->get_w() == out->get_w() &&
 485             ref_frame->get_h() == out->get_h() &&
 486             ref_frame->get_color_model() == out->get_color_model() ) {
 487                 ref_frame->copy_from(out);
 488                 ref_position = out_position;
 489         }
 490         transform_target(use_opengl);
 491
 492         if( config.draw_vectors )
 493                 draw_vectors(out);
 494
 495         return 0;
 496 }
 497
 498 VFrame* Motion51Main::new_temp(VFrame *&tmp, VFrame *ref)
 499 {
 500         if( tmp && (tmp->get_w() != ref->get_w() || tmp->get_h() != ref->get_h() ||
 501             tmp->get_color_model() != ref->get_color_model()) ) {
 502                 delete tmp; tmp = 0;
 503         }
 504         if( !tmp )
 505                 tmp = new VFrame(0, -1, ref->get_w(), ref->get_h(), ref->get_color_model(), -1);
 506         return tmp;
 507 }
 508
 509 void Motion51Main::reset_sample(int sz, double r)
 510 {
 511         if( cir_sz == sz && cir_r == r ) return;
 512         if( cir_sz != sz ) {
 513                 cir_sz = sz;
 514                 delete xpts;  xpts = new double[cir_sz];
 515                 delete ypts;  ypts = new double[cir_sz];
 516         }
 517         cir_r = r;
 518         int n = cir_sz / r_steps;
 519         double dt = (2*M_PI)/n;
 520         double dr = r / r_steps;
 521         for( int it=0; it<n; ++it ) {
 522                 double t = it * dt, cos_t = cos(t), sin_t = sin(t);
 523                 for( int i=0; i<r_steps; ) {
 524                         int k = i * n + it;
 525                         double r = ++i * dr;
 526                         xpts[k] = r * cos_t;
 527                         ypts[k] = r * sin_t;
 528                 }
 529         }
 530 }
 531
 532 void Motion51Main::get_samples(VFrame *img, double *pix[3], double x, double y)
 533 {
 534         int iw = img->get_w(), iw1 = iw-1;
 535         int ih = img->get_h(), ih1 = ih-1;
 536         uint8_t **rows = img->get_rows();
 537         int psz = BC_CModels::calculate_pixelsize(img->get_color_model());
 538
 539         double *xp = xpts, *yp = ypts;
 540         for( int i=cir_sz; --i>=0; ++xp,++yp ) {
 541                 double px = x + *xp, py = y + *yp;
 542                 get_pixel(pix, px, py, rows, psz, iw1, ih1);
 543         }
 544 }
 545
 546 void Motion51Main::centroid(double *pix[3], double *ctr_v, double *ctr_x, double *ctr_y)
 547 {
 548         for( int i=0; i<3; ++i )
 549                 ctr_v[i] = ctr_x[i] = ctr_y[i] = 0;
 550         double *xp = xpts, *yp = ypts;
 551         for( int k=cir_sz; --k>=0; ++xp,++yp ) {
 552                 double x = rx + *xp, y = ry + *yp;
 553                 for( int i=0; i<3; ++pix[i],++i ) {
 554                         double v = *pix[i];
 555                         ctr_v[i] += v;
 556                         ctr_x[i] += v * x;
 557                         ctr_y[i] += v * y;
 558                 }
 559         }
 560         for( int i=0; i<3; ++i ) {
 561                 if( !ctr_v[i] ) continue;
 562                 ctr_x[i] /= ctr_v[i];
 563                 ctr_y[i] /= ctr_v[i];
 564                 ctr_v[i] /= cir_sz;
 565         }
 566 }
 567
 568
 569 void Motion51Main::draw_vectors(VFrame *img)
 570 {
 571         img->draw_arrow(rx, ry, rx+current_dx, ry+current_dy);
 572
 573 //      img->draw_smooth(rx-rr,ry, rx-rr,ry+rr, rx,ry+rr);
 574 //      img->draw_smooth(rx,ry+rr, rx+rr,ry+rr, rx+rr,ry);
 575 //      img->draw_smooth(rx+rr,ry, rx+rr,ry-rr, rx,ry-rr);
 576 //      img->draw_smooth(rx,ry-rr, rx-rr,ry-rr, rx-rr,ry);
 577
 578         float rx1 = rx - 0.5*rw;
 579         float ry1 = ry - 0.5*rh;
 580         float rx2 = rx1 + rw;
 581         float ry2 = ry1 + rh;
 582
 583         img->draw_line(rx1, ry1, rx2, ry1);
 584         img->draw_line(rx2, ry1, rx2, ry2);
 585         img->draw_line(rx2, ry2, rx1, ry2);
 586         img->draw_line(rx1, ry2, rx1, ry1);
 587
 588         float sx1 = rx1 - rr, sy1 = ry1 - rr;
 589         float sx2 = rx2 + rr, sy2 = ry2 + rr;
 590
 591         img->draw_smooth(sx1, ry1, sx1, sy1, rx1, sy1);
 592         img->draw_line(rx1, sy1, rx2, sy1);
 593         img->draw_smooth(rx2, sy1, sx2, sy1, sx2, ry1);
 594         img->draw_line(sx2, ry1, sx2, ry2);
 595         img->draw_smooth(sx2, ry2, sx2, sy2, rx2, sy2);
 596         img->draw_line(rx2, sy2, rx1, sy2);
 597         img->draw_smooth(rx1, sy2, sx1, sy2, sx1, ry2);
 598         img->draw_line(sx1, ry2, sx1, ry1);
 599
 600         double *xp = xpts, *yp = ypts;
 601         for( int i=cir_sz; --i>=0; ++xp, ++yp )
 602                 img->draw_pixel(rx+*xp, ry+*yp);
 603 }
 604
 605 int Motion51Main::open_cache_file()
 606 {
 607         if( cache_fp ) return 0;
 608         if( !cache_file[0] ) return 1;
 609         if( !(cache_fp = fopen(cache_file, "r")) ) return 1;
 610 // match timestamp, asset path
 611         char line[BCTEXTLEN], *cp = line, *ep = cp+sizeof(line);
 612         if( fgets(line,sizeof(line),cache_fp) ) {
 613                 int64_t tm = strtoul(cp,&cp,0);
 614 // time 0 matches everything
 615                 if( !tm ) return 0;
 616                 const char *sp = get_source_path();
 617                 if( !sp ) return 0;
 618                 if( cp < ep && *cp == ' ' ) ++cp;
 619                 int n = strlen(cp);
 620                 if( n > 0 && cp[n-1] == '\n' ) cp[n-1] = 0;
 621                 struct stat st;
 622                 if( !strcmp(cp, sp) && !stat(cp,&st) && st.st_mtime == tm )
 623                         return 0;
 624         }
 625         fclose(cache_fp);  cache_fp = 0;
 626         return 1;
 627 }
 628
 629 void Motion51Main::close_cache_file()
 630 {
 631         if( !cache_fp ) return;
 632         fclose(cache_fp);
 633         cache_fp = 0; cache_key = -1;
 634         tracking_position = -1;
 635 }
 636
 637 int Motion51Main::load_cache_line()
 638 {
 639         cache_key = -1;
 640         if( open_cache_file() ) return 1;
 641         if( !fgets(cache_line, sizeof(cache_line), cache_fp) ) return 1;
 642         cache_key = strtol(cache_line, 0, 0);
 643         return 0;
 644 }
 645
 646 int Motion51Main::get_cache_line(int64_t key)
 647 {
 648         if( cache_key == key ) return 0;
 649         if( open_cache_file() ) return 1;
 650         if( cache_key >= 0 && key > cache_key ) {
 651                 if( load_cache_line() ) return 1;
 652                 if( cache_key == key ) return 0;
 653                 if( cache_key > key ) return 1;
 654         }
 655 // binary search file
 656         fseek(cache_fp, 0, SEEK_END);
 657         int64_t l = -1, r = ftell(cache_fp);
 658         while( (r - l) > 1 ) {
 659                 int64_t m = (l + r) / 2;
 660                 fseek(cache_fp, m, SEEK_SET);
 661 // skip to start of next line
 662                 if( !fgets(cache_line, sizeof(cache_line), cache_fp) )
 663                         return -1;
 664                 if( !load_cache_line() ) {
 665                         if( cache_key == key )
 666                                 return 0;
 667                         if( cache_key < key ) { l = m; continue; }
 668                 }
 669                 r = m;
 670         }
 671         return 1;
 672 }
 673
 674 int Motion51Main::locate_cache_line(int64_t key)
 675 {
 676         int ret = 1;
 677         if( key < 0 || !(ret=get_cache_line(key)) ||
 678             ( cache_key >= 0 && cache_key < key ) )
 679                 ret = load_cache_line();
 680         return ret;
 681 }
 682
 683 int Motion51Main::put_cache_line(const char *line)
 684 {
 685         int64_t key = strtol(line, 0, 0);
 686         if( key == active_key ) return 1;
 687         if( !active_fp ) {
 688                 close_cache_file();
 689                 snprintf(cache_file, sizeof(cache_file), "%s.bak", config.tracking_file);
 690                 ::rename(config.tracking_file, cache_file);
 691                 if( !(active_fp = fopen(config.tracking_file, "w")) ) {
 692                         perror(config.tracking_file);
 693                         fprintf(stderr, "err writing key %jd\n", key);
 694                         return -1;
 695                 }
 696                 const char *sp = get_source_path();
 697                 int64_t tm = 0;
 698                 if( sp ) {
 699                         struct stat st;
 700                         if( !stat(sp,&st) ) tm = st.st_mtime;
 701                 }
 702                 fprintf(active_fp, "%jd %s\n",tm, sp);
 703                 active_key = -1;
 704         }
 705
 706         if( active_key < key ) {
 707                 locate_cache_line(active_key);
 708                 while( cache_key >= 0 && key >= cache_key ) {
 709                         if( key > cache_key )
 710                                 fputs(cache_line, active_fp);
 711                         load_cache_line();
 712                 }
 713         }
 714
 715         active_key = key;
 716         fputs(line, active_fp);
 717         fflush(active_fp);
 718         return 0;
 719 }
 720
 721 void Motion51Main::update_cache_file()
 722 {
 723         if( active_fp ) {
 724                 locate_cache_line(active_key);
 725                 while( cache_key >= 0 ) {
 726                         fputs(cache_line, active_fp);
 727                         load_cache_line();
 728                 }
 729                 close_cache_file();
 730                 ::remove(cache_file);
 731                 fclose(active_fp);  active_fp = 0;
 732                 active_key = -1;
 733         }
 734         else
 735                 close_cache_file();
 736         strcpy(cache_file, config.tracking_file);
 737 }
 738
 739 Motion51Scan::Motion51Scan(Motion51Main *plugin, int n_thds, int x_steps, int y_steps)
 740  : LoadServer(n_thds, x_steps*y_steps)
 741 {
 742         this->plugin = plugin;
 743         this->x_steps = x_steps;
 744         this->y_steps = y_steps;
 745         this->fft = new FFT;
 746         this->result_lock = new Mutex("Motion51Scan::result_lock");
 747
 748         cur = ref = 0;
 749         bx = by = br = bw = bh = 0;
 750         rpix_sz = 0;
 751         for( int i=0; i<3; ++i ) {
 752                 rpix[i] = 0;  rpwr[i] = 0;
 753                 rctr_v[i] = rctr_x[i] = rctr_y[i] = 0;
 754                 ref_real[i] = ref_imag[i] = 0;
 755         }
 756         cor_value = value = 0;
 757         dx_result = dy_result = 0;
 758         dt_result = 0;
 759 }
 760
 761
 762 Motion51Scan::~Motion51Scan()
 763 {
 764         for( int i=0; i<3; ++i ) {
 765                 delete [] rpix[i];
 766                 delete [] ref_real[i];
 767                 delete [] ref_imag[i];
 768         }
 769         delete fft;
 770         delete result_lock;
 771 }
 772
 773 // sum absolute diff of ref at (rx,ry) - (cur(cx,cy) rotated ct)
 774 //   downsampled using corner_sample to x_steps, y_steps
 775 double Motion51Scan::compare(double cx, double cy, double ct)
 776 {
 777         int iw = ref->get_w(), iw1 = iw-1;
 778         int ih = ref->get_h(), ih1 = ih-1;
 779         int xsz = x_steps;// iw;
 780         int ysz = y_steps;// ih;
 781         int psz = BC_CModels::calculate_pixelsize(cur->get_color_model());
 782         uint8_t **ref_rows = ref->get_rows();
 783         uint8_t **cur_rows = cur->get_rows();
 784         double cos_ct = cos(ct), sin_ct = sin(ct);
 785         double rx = plugin->rx, ry = plugin->ry;
 786         double sx = (double)iw/xsz, sy = (double)ih/ysz;
 787         double cpix[3][xsz], rpix[3][xsz];
 788         double v = 0;
 789         for( int iy=0; iy<y_steps; ++iy ) {
 790                 double y = iy * sy;
 791                 double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
 792                 double *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
 793                 for( int ix=0; ix<x_steps; ++ix ) {
 794                         double x = ix * sx;
 795                         plugin->get_pixel(ref_pix, x, y, ref_rows, psz, iw1, ih1);
 796                         double tx = x-rx, ty = y-ry;
 797                         double xt = cos_ct*tx - sin_ct*ty + cx;
 798                         double yt = cos_ct*ty + sin_ct*tx + cy;
 799                         plugin->get_pixel(cur_pix, xt, yt, cur_rows, psz, iw1, ih1);
 800                 }
 801                 for( int i=0; i<3; ++i ) {
 802                         double *rp = rpix[i], *cp = cpix[i];
 803                         for( int k=x_steps; --k>=0; ++rp,++cp ) v += fabs(*rp - *cp);
 804                 }
 805         }
 806         double mx = BC_CModels::calculate_max(ref->get_color_model());
 807         v = 1.-v/(3*mx * x_steps*y_steps);
 808         return v;
 809 }
 810
 811 void Motion51Scan::scan(VFrame *ref, VFrame *cur, int sz)
 812 {
 813         this->ref = ref;
 814         this->cur = cur;
 815         if( this->rpix_sz != sz ) {
 816                 this->rpix_sz = sz;
 817                 for( int i=0; i<3; ++i ) {
 818                         delete [] rpix[i];      rpix[i] = new double[sz];
 819                         delete [] ref_real[i];  ref_real[i] = new double[sz];
 820                         delete [] ref_imag[i];  ref_imag[i] = new double[sz];
 821                 }
 822         }
 823
 824         bw = plugin->rw;  bh = plugin->rh;
 825         bx = plugin->rx;  by = plugin->ry;
 826         br = plugin->rr;
 827
 828         double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
 829         plugin->get_samples(ref, ref_pix, bx, by);
 830         double *pix[3] = { rpix[0], rpix[1], rpix[2] };
 831         plugin->centroid(&pix[0], &rctr_v[0], &rctr_x[0], &rctr_y[0]);
 832         for( int i=0; i<3; ++i ) {
 833                 fft->do_fft(sz, 0, rpix[i], 0, ref_real[i], ref_imag[i]);
 834                 rpwr[i] = cor(sz, rpix[i], rpix[i]);
 835         }
 836         double scan_limit = 0.25; // quarter pixel resolution
 837 //printf("frame: %jd\n", plugin->get_source_position());
 838         while( bw/x_steps > scan_limit || bh/y_steps > scan_limit ) {
 839                 dx_result = dy_result = dt_result = 0;
 840                 cor_value = value = 0;
 841 //printf("  bx,by %6.2f,%-6.2f  bw,bh %6.2f,%-6.2f ",bx,by, bw,bh);
 842                 process_packages();
 843                 bx = dx_result;
 844                 by = dy_result;
 845 //printf(" r = %f(%f), %6.2f,%-6.2f\n",value,dt_result*180/M_PI,bx,by);
 846                 bw *= 0.5;  bh *= 0.5;
 847         }
 848 }
 849
 850 void Motion51Scan::init_packages()
 851 {
 852 // sort in increasing distance from current displacement
 853         double tx = plugin->rx + plugin->total_dx;
 854         double ty = plugin->ry + plugin->total_dy;
 855         int npkgs = get_total_packages();
 856         coord_t coords[npkgs];
 857         int i = 0;
 858         double x0 = bx - bw/2, y0 = by - bh/2;
 859         for( int iy=0; iy<y_steps; ++iy ) {
 860                 double y = y0 + iy*bh/y_steps;
 861                 for( int ix=0; ix<x_steps; ++ix ) {
 862                         double x = x0 + ix*bw/x_steps;
 863                         double d = sqrt(sqr(x-tx) + sqr(y-ty));
 864                         coord_t *cp = coords + i++;
 865                         cp->x = x; cp->y = y; cp->d = d;
 866                 }
 867         }
 868         qsort(&coords,npkgs,sizeof(coords[0]),coord_cmpr);
 869
 870         for( i=0; i<npkgs; ++i ) {
 871                 coord_t *cp = coords + i;
 872                 Motion51ScanPackage *pkg = (Motion51ScanPackage*)get_package(i);
 873                 pkg->x = cp->x;  pkg->y = cp->y;
 874         }
 875 }
 876
 877 Motion51ScanUnit::Motion51ScanUnit(Motion51Scan *server, Motion51Main *plugin)
 878  : LoadClient(server)
 879 {
 880         this->server = server;
 881         this->plugin = plugin;
 882         fft = new FFT;
 883 }
 884 Motion51ScanUnit::~Motion51ScanUnit()
 885 {
 886         delete fft;
 887 }
 888
 889 void Motion51ScanUnit::process_package(LoadPackage *package)
 890 {
 891         Motion51ScanPackage *pkg = (Motion51ScanPackage *)package;
 892         int sz = server->rpix_sz;
 893         double cur_real[3][sz], cur_imag[3][sz];
 894         double cpwr[3], cpix[3][sz], *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
 895         plugin->get_samples(server->cur, cur_pix, pkg->x, pkg->y);
 896
 897         double *pix[3] = { cpix[0], cpix[1], cpix[2] };
 898         double cctr_v[3], cctr_x[3], cctr_y[3];
 899         plugin->centroid(&pix[0], &cctr_v[0], &cctr_x[0], &cctr_y[0]);
 900         double mx = BC_CModels::calculate_max(server->ref->get_color_model());
 901         for( int i=0; i<3; ++i ) {
 902                 double v = 1. - fabs(server->rctr_v[i]-cctr_v[i]) / mx;
 903                 if( !passible(v) ) return;
 904                 double *rctr_x = server->rctr_x, *rctr_y = server->rctr_y;
 905                 double d = sqrt(sqr(rctr_x[i]-cctr_x[i]) + sqr(rctr_y[i]-cctr_y[i]));
 906                 v = 1 - d / plugin->cir_r;
 907                 if( !passible(v) ) return;
 908         }
 909         for( int i=0; i<3; ++i ) {
 910                 double cs = cor(sz, cpix[i], cpix[i]);
 911                 double rs = server->rpwr[i];
 912                 double ss = rs + cs;
 913                 if( ss == 0 ) ss = 1;
 914                 double v = 1. - fabs(rs - cs) / ss;
 915                 if( ! passible(v) ) return;
 916                 cpwr[i] = 1. / ss;
 917         }
 918
 919         double cor_real[3][sz], cor_imag[3][sz];
 920         for( int i=0; i<3; ++i ) {
 921                 fft->do_fft(sz, 0, cpix[i], 0, cur_real[i], cur_imag[i]);
 922                 cj_product(sz, 0, cur_real[i], cur_imag[i],
 923                         server->ref_real[i], server->ref_imag[i],
 924                         cur_real[i], cur_imag[i]);
 925                 fft->do_fft(sz, 1, cur_real[i], cur_imag[i], cor_real[i], cor_imag[i]);
 926         }
 927         double sv = 0;
 928         int st = 0;
 929         for( int t=0; t<sz; ++t ) {
 930                 double v = 0;
 931                 for( int i=0; i<3; ++i ) v += cor_real[i][t] * cpwr[i];
 932                 v = ((2*v) / 3);
 933                 if( sv >= v ) continue;
 934                 sv = v;  st = t;
 935         }
 936         if( server->cor_value > sv ) return;
 937         server->cor_value = sv;
 938         if( st > sz/2 ) st -= sz;
 939         int steps = plugin->r_steps;
 940         double tt = steps*(2*M_PI);
 941         double th = st*tt/sz, dt = th;
 942         double value = 0;
 943         double dth = (2*M_PI)/sz;
 944         for( int i=-steps; i<=steps; ++i ) {
 945                 double t = th + i*dth;
 946                 double v = server->compare(pkg->x, pkg->y, -t);
 947                 if( value >= v ) continue;
 948                 value = v;  dt = t;
 949         }
 950 //static int dbg = 0;
 951 //if( dbg )
 952 //printf("  %d. %.3f,%.3f %f = %f / %f + %f\n",
 953 // package_number,pkg->x,pkg->y,dt*180./M_PI,value,sv);
 954         server->result_lock->lock("Motion51Scan::process_package");
 955         if( value > server->value ) {
 956                 server->value = value;
 957                 server->dt_result = dt;
 958                 server->dx_result = pkg->x;
 959                 server->dy_result = pkg->y;
 960         }
 961         server->result_lock->unlock();
 962 }
 963