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 N_("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->xbuf);
 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->xbuf);
 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 //      Do NOT use opengl here because if you so than dissolve, flash, and zoome can cause problems.
 438 //      read_frame(out, target_layer, out_position, frame_rate, use_opengl);
 439         read_frame(out, target_layer, out_position, frame_rate, 0);
 440         out_w = out->get_w();
 441         out_h = out->get_h();
 442         out_r = 0.5 * (out_w < out_h ? out_w : out_h);
 443         rw = out_w * config.block_w/100.;
 444         rh = out_h * config.block_h/100.;
 445         rx = out_w * config.block_x/100.;
 446         ry = out_h * config.block_y/100.;
 447         rr = out_r * config.sample_r/100.;
 448         reset_sample(config.sample_steps, rr);
 449         dx = 0;  dy = 0;  dt = 0;
 450
 451         update_tracking_cache();
 452         if( load_tracking_cache(out_position) ) {
 453                 int64_t ref_pos = get_ref_position();
 454                 if( !ref_frame || ref_pos != ref_position || need_reconfigure ) {
 455                         new_temp(ref_frame, ref_layer);
 456                         read_frame(ref_frame, reference_layer, ref_pos, frame_rate, 0);
 457                         total_dx = total_dy = 0;  total_angle = 0;
 458                         ref_position = ref_pos;
 459                 }
 460                 VFrame *cur_frame = out;
 461                 if( reference_layer != target_layer ) {
 462                         new_temp(tmp_frame, ref_layer);
 463                         read_frame(tmp_frame, reference_layer, out_position, frame_rate, 0);
 464                         cur_frame = tmp_frame;
 465                 }
 466                 match(ref_frame, cur_frame);
 467                 save_tracking_cache(out_position);
 468         }
 469         current_dx = dx;  current_dy = dy;
 470         double sf = 1. - config.shake_fade/100.;
 471         dx += total_dx * sf;  dy += total_dy * sf;
 472         double rf = 1. - config.twist_fade/100.;
 473         dt += total_angle * rf;
 474         if( dt < -180. ) dt += 360.;
 475         else if( dt > 180. ) dt -= 360.;
 476
 477         float tot_dx = out_w * config.horiz_limit/100.;
 478         bclamp(dx, -tot_dx, tot_dx);
 479         float tot_dy = out_h * config.vert_limit/100.;
 480         bclamp(dy, -tot_dy, tot_dy);
 481         float tot_dt = 180. * config.twist_limit/100.;
 482         bclamp(dt, -tot_dt, +tot_dt);
 483         total_dx = dx;  total_dy = dy;  total_angle = dt;
 484         if( ref_frame && reference_layer == target_layer &&
 485             ref_position+1 == out_position &&
 486             ref_frame->get_w() == out->get_w() &&
 487             ref_frame->get_h() == out->get_h() &&
 488             ref_frame->get_color_model() == out->get_color_model() ) {
 489                 ref_frame->copy_from(out);
 490                 ref_position = out_position;
 491         }
 492         transform_target(use_opengl);
 493
 494         if( config.draw_vectors )
 495                 draw_vectors(out);
 496
 497         return 0;
 498 }
 499
 500 VFrame* Motion51Main::new_temp(VFrame *&tmp, VFrame *ref)
 501 {
 502         if( tmp && (tmp->get_w() != ref->get_w() || tmp->get_h() != ref->get_h() ||
 503             tmp->get_color_model() != ref->get_color_model()) ) {
 504                 delete tmp; tmp = 0;
 505         }
 506         if( !tmp )
 507                 tmp = new VFrame(ref->get_w(), ref->get_h(), ref->get_color_model(), 0);
 508         return tmp;
 509 }
 510
 511 void Motion51Main::reset_sample(int sz, double r)
 512 {
 513         if( cir_sz == sz && cir_r == r ) return;
 514         if( cir_sz != sz ) {
 515                 cir_sz = sz;
 516                 delete xpts;  xpts = new double[cir_sz];
 517                 delete ypts;  ypts = new double[cir_sz];
 518         }
 519         cir_r = r;
 520         int n = cir_sz / r_steps;
 521         double dt = (2*M_PI)/n;
 522         double dr = r / r_steps;
 523         for( int it=0; it<n; ++it ) {
 524                 double t = it * dt, cos_t = cos(t), sin_t = sin(t);
 525                 for( int i=0; i<r_steps; ) {
 526                         int k = i * n + it;
 527                         double r = ++i * dr;
 528                         xpts[k] = r * cos_t;
 529                         ypts[k] = r * sin_t;
 530                 }
 531         }
 532 }
 533
 534 void Motion51Main::get_samples(VFrame *img, double *pix[3], double x, double y)
 535 {
 536         int iw = img->get_w(), iw1 = iw-1;
 537         int ih = img->get_h(), ih1 = ih-1;
 538         uint8_t **rows = img->get_rows();
 539         int psz = BC_CModels::calculate_pixelsize(img->get_color_model());
 540
 541         double *xp = xpts, *yp = ypts;
 542         for( int i=cir_sz; --i>=0; ++xp,++yp ) {
 543                 double px = x + *xp, py = y + *yp;
 544                 get_pixel(pix, px, py, rows, psz, iw1, ih1);
 545         }
 546 }
 547
 548 void Motion51Main::centroid(double *pix[3], double *ctr_v, double *ctr_x, double *ctr_y)
 549 {
 550         for( int i=0; i<3; ++i )
 551                 ctr_v[i] = ctr_x[i] = ctr_y[i] = 0;
 552         double *xp = xpts, *yp = ypts;
 553         for( int k=cir_sz; --k>=0; ++xp,++yp ) {
 554                 double x = rx + *xp, y = ry + *yp;
 555                 for( int i=0; i<3; ++pix[i],++i ) {
 556                         double v = *pix[i];
 557                         ctr_v[i] += v;
 558                         ctr_x[i] += v * x;
 559                         ctr_y[i] += v * y;
 560                 }
 561         }
 562         for( int i=0; i<3; ++i ) {
 563                 if( !ctr_v[i] ) continue;
 564                 ctr_x[i] /= ctr_v[i];
 565                 ctr_y[i] /= ctr_v[i];
 566                 ctr_v[i] /= cir_sz;
 567         }
 568 }
 569
 570 Motion51VVFrame::Motion51VVFrame(VFrame *vfrm, int n)
 571  : VFrame(vfrm->get_data(), -1, vfrm->get_y()-vfrm->get_data(),
 572         vfrm->get_u()-vfrm->get_data(), vfrm->get_v()-vfrm->get_data(),
 573         vfrm->get_w(), vfrm->get_h(), vfrm->get_color_model(),
 574         vfrm->get_bytes_per_line())
 575 {
 576         this->n = n;
 577 }
 578
 579 int Motion51VVFrame::draw_pixel(int x, int y)
 580 {
 581         VFrame::draw_pixel(x+0, y+0);
 582         for( int i=1; i<n; ++i ) {
 583                 VFrame::draw_pixel(x-i, y-i);
 584                 VFrame::draw_pixel(x-i, y+i);
 585                 VFrame::draw_pixel(x+i, y-i);
 586                 VFrame::draw_pixel(x+i, y+i);
 587         }
 588         return 0;
 589 }
 590
 591 void Motion51Main::draw_vectors(VFrame *img)
 592 {
 593         int iw = img->get_w(), ih = img->get_h();
 594         int mx = iw > ih ? iw : ih;
 595         int n = mx/800 + 1;
 596         Motion51VVFrame vfrm(img, n);
 597         vfrm.set_pixel_color(WHITE);
 598         int m = 2;  while( m < n ) m <<= 1;
 599         vfrm.set_stiple(2*m);
 600
 601         vfrm.draw_arrow(rx, ry, rx+current_dx, ry+current_dy);
 602 //      vfrm.draw_smooth(rx-rr,ry, rx-rr,ry+rr, rx,ry+rr);
 603 //      vfrm.draw_smooth(rx,ry+rr, rx+rr,ry+rr, rx+rr,ry);
 604 //      vfrm.draw_smooth(rx+rr,ry, rx+rr,ry-rr, rx,ry-rr);
 605 //      vfrm.draw_smooth(rx,ry-rr, rx-rr,ry-rr, rx-rr,ry);
 606
 607         float rx1 = rx - 0.5*rw;
 608         float ry1 = ry - 0.5*rh;
 609         float rx2 = rx1 + rw;
 610         float ry2 = ry1 + rh;
 611
 612         vfrm.draw_line(rx1, ry1, rx2, ry1);
 613         vfrm.draw_line(rx2, ry1, rx2, ry2);
 614         vfrm.draw_line(rx2, ry2, rx1, ry2);
 615         vfrm.draw_line(rx1, ry2, rx1, ry1);
 616
 617         float sx1 = rx1 - rr, sy1 = ry1 - rr;
 618         float sx2 = rx2 + rr, sy2 = ry2 + rr;
 619
 620         vfrm.draw_smooth(sx1, ry1, sx1, sy1, rx1, sy1);
 621         vfrm.draw_line(rx1, sy1, rx2, sy1);
 622         vfrm.draw_smooth(rx2, sy1, sx2, sy1, sx2, ry1);
 623         vfrm.draw_line(sx2, ry1, sx2, ry2);
 624         vfrm.draw_smooth(sx2, ry2, sx2, sy2, rx2, sy2);
 625         vfrm.draw_line(rx2, sy2, rx1, sy2);
 626         vfrm.draw_smooth(rx1, sy2, sx1, sy2, sx1, ry2);
 627         vfrm.draw_line(sx1, ry2, sx1, ry1);
 628
 629         double *xp = xpts, *yp = ypts;
 630         for( int i=cir_sz; --i>=0; ++xp, ++yp )
 631                 vfrm.draw_pixel(rx+*xp, ry+*yp);
 632 }
 633
 634 int Motion51Main::open_cache_file()
 635 {
 636         if( cache_fp ) return 0;
 637         if( !cache_file[0] ) return 1;
 638         if( !(cache_fp = fopen(cache_file, "r")) ) return 1;
 639 // match timestamp, asset path
 640         char line[BCTEXTLEN], *cp = line, *ep = cp+sizeof(line);
 641         if( fgets(line,sizeof(line),cache_fp) ) {
 642                 int64_t tm = strtoul(cp,&cp,0);
 643 // time 0 matches everything
 644                 if( !tm ) return 0;
 645                 const char *sp = get_source_path();
 646                 if( !sp ) return 0;
 647                 if( cp < ep && *cp == ' ' ) ++cp;
 648                 int n = strlen(cp);
 649                 if( n > 0 && cp[n-1] == '\n' ) cp[n-1] = 0;
 650                 struct stat st;
 651                 if( !strcmp(cp, sp) && !stat(cp,&st) && st.st_mtime == tm )
 652                         return 0;
 653         }
 654         fclose(cache_fp);  cache_fp = 0;
 655         return 1;
 656 }
 657
 658 void Motion51Main::close_cache_file()
 659 {
 660         if( !cache_fp ) return;
 661         fclose(cache_fp);
 662         cache_fp = 0; cache_key = -1;
 663         tracking_position = -1;
 664 }
 665
 666 int Motion51Main::load_cache_line()
 667 {
 668         cache_key = -1;
 669         if( open_cache_file() ) return 1;
 670         if( !fgets(cache_line, sizeof(cache_line), cache_fp) ) return 1;
 671         cache_key = strtol(cache_line, 0, 0);
 672         return 0;
 673 }
 674
 675 int Motion51Main::get_cache_line(int64_t key)
 676 {
 677         if( cache_key == key ) return 0;
 678         if( open_cache_file() ) return 1;
 679         if( cache_key >= 0 && key > cache_key ) {
 680                 if( load_cache_line() ) return 1;
 681                 if( cache_key == key ) return 0;
 682                 if( cache_key > key ) return 1;
 683         }
 684 // binary search file
 685         fseek(cache_fp, 0, SEEK_END);
 686         int64_t l = -1, r = ftell(cache_fp);
 687         while( (r - l) > 1 ) {
 688                 int64_t m = (l + r) / 2;
 689                 fseek(cache_fp, m, SEEK_SET);
 690 // skip to start of next line
 691                 if( !fgets(cache_line, sizeof(cache_line), cache_fp) )
 692                         return -1;
 693                 if( !load_cache_line() ) {
 694                         if( cache_key == key )
 695                                 return 0;
 696                         if( cache_key < key ) { l = m; continue; }
 697                 }
 698                 r = m;
 699         }
 700         return 1;
 701 }
 702
 703 int Motion51Main::locate_cache_line(int64_t key)
 704 {
 705         int ret = 1;
 706         if( key < 0 || !(ret=get_cache_line(key)) ||
 707             ( cache_key >= 0 && cache_key < key ) )
 708                 ret = load_cache_line();
 709         return ret;
 710 }
 711
 712 int Motion51Main::put_cache_line(const char *line)
 713 {
 714         int64_t key = strtol(line, 0, 0);
 715         if( key == active_key ) return 1;
 716         if( !active_fp ) {
 717                 close_cache_file();
 718                 snprintf(cache_file, sizeof(cache_file), "%s.bak", config.tracking_file);
 719                 ::rename(config.tracking_file, cache_file);
 720                 if( !(active_fp = fopen(config.tracking_file, "w")) ) {
 721                         perror(config.tracking_file);
 722                         fprintf(stderr, "err writing key %jd\n", key);
 723                         return -1;
 724                 }
 725                 const char *sp = get_source_path();
 726                 int64_t tm = 0;
 727                 if( sp ) {
 728                         struct stat st;
 729                         if( !stat(sp,&st) ) tm = st.st_mtime;
 730                 }
 731                 fprintf(active_fp, "%jd %s\n",tm, sp);
 732                 active_key = -1;
 733         }
 734
 735         if( active_key < key ) {
 736                 locate_cache_line(active_key);
 737                 while( cache_key >= 0 && key >= cache_key ) {
 738                         if( key > cache_key )
 739                                 fputs(cache_line, active_fp);
 740                         load_cache_line();
 741                 }
 742         }
 743
 744         active_key = key;
 745         fputs(line, active_fp);
 746         fflush(active_fp);
 747         return 0;
 748 }
 749
 750 void Motion51Main::update_cache_file()
 751 {
 752         if( active_fp ) {
 753                 locate_cache_line(active_key);
 754                 while( cache_key >= 0 ) {
 755                         fputs(cache_line, active_fp);
 756                         load_cache_line();
 757                 }
 758                 close_cache_file();
 759                 ::remove(cache_file);
 760                 fclose(active_fp);  active_fp = 0;
 761                 active_key = -1;
 762         }
 763         else
 764                 close_cache_file();
 765         strcpy(cache_file, config.tracking_file);
 766 }
 767
 768 Motion51Scan::Motion51Scan(Motion51Main *plugin, int n_thds, int x_steps, int y_steps)
 769  : LoadServer(n_thds, x_steps*y_steps)
 770 {
 771         this->plugin = plugin;
 772         this->x_steps = x_steps;
 773         this->y_steps = y_steps;
 774         this->fft = new FFT;
 775         this->result_lock = new Mutex("Motion51Scan::result_lock");
 776
 777         cur = ref = 0;
 778         bx = by = br = bw = bh = 0;
 779         rpix_sz = 0;
 780         for( int i=0; i<3; ++i ) {
 781                 rpix[i] = 0;  rpwr[i] = 0;
 782                 rctr_v[i] = rctr_x[i] = rctr_y[i] = 0;
 783                 ref_real[i] = ref_imag[i] = 0;
 784         }
 785         cor_value = value = 0;
 786         dx_result = dy_result = 0;
 787         dt_result = 0;
 788 }
 789
 790
 791 Motion51Scan::~Motion51Scan()
 792 {
 793         for( int i=0; i<3; ++i ) {
 794                 delete [] rpix[i];
 795                 delete [] ref_real[i];
 796                 delete [] ref_imag[i];
 797         }
 798         delete fft;
 799         delete result_lock;
 800 }
 801
 802 // sum absolute diff of ref at (rx,ry) - (cur(cx,cy) rotated ct)
 803 //   downsampled using corner_sample to x_steps, y_steps
 804 double Motion51Scan::compare(double cx, double cy, double ct)
 805 {
 806         int iw = ref->get_w(), iw1 = iw-1;
 807         int ih = ref->get_h(), ih1 = ih-1;
 808         int xsz = x_steps;// iw;
 809         int ysz = y_steps;// ih;
 810         int psz = BC_CModels::calculate_pixelsize(cur->get_color_model());
 811         uint8_t **ref_rows = ref->get_rows();
 812         uint8_t **cur_rows = cur->get_rows();
 813         double cos_ct = cos(ct), sin_ct = sin(ct);
 814         double rx = plugin->rx, ry = plugin->ry;
 815         double sx = (double)iw/xsz, sy = (double)ih/ysz;
 816         double cpix[3][xsz], rpix[3][xsz];
 817         double v = 0;
 818         for( int iy=0; iy<y_steps; ++iy ) {
 819                 double y = iy * sy;
 820                 double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
 821                 double *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
 822                 for( int ix=0; ix<x_steps; ++ix ) {
 823                         double x = ix * sx;
 824                         plugin->get_pixel(ref_pix, x, y, ref_rows, psz, iw1, ih1);
 825                         double tx = x-rx, ty = y-ry;
 826                         double xt = cos_ct*tx - sin_ct*ty + cx;
 827                         double yt = cos_ct*ty + sin_ct*tx + cy;
 828                         plugin->get_pixel(cur_pix, xt, yt, cur_rows, psz, iw1, ih1);
 829                 }
 830                 for( int i=0; i<3; ++i ) {
 831                         double *rp = rpix[i], *cp = cpix[i];
 832                         for( int k=x_steps; --k>=0; ++rp,++cp ) v += fabs(*rp - *cp);
 833                 }
 834         }
 835         double mx = BC_CModels::calculate_max(ref->get_color_model());
 836         v = 1.-v/(3*mx * x_steps*y_steps);
 837         return v;
 838 }
 839
 840 void Motion51Scan::scan(VFrame *ref, VFrame *cur, int sz)
 841 {
 842         this->ref = ref;
 843         this->cur = cur;
 844         if( this->rpix_sz != sz ) {
 845                 this->rpix_sz = sz;
 846                 for( int i=0; i<3; ++i ) {
 847                         delete [] rpix[i];      rpix[i] = new double[sz];
 848                         delete [] ref_real[i];  ref_real[i] = new double[sz];
 849                         delete [] ref_imag[i];  ref_imag[i] = new double[sz];
 850                 }
 851         }
 852
 853         bw = plugin->rw;  bh = plugin->rh;
 854         bx = plugin->rx;  by = plugin->ry;
 855         br = plugin->rr;
 856
 857         double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
 858         plugin->get_samples(ref, ref_pix, bx, by);
 859         double *pix[3] = { rpix[0], rpix[1], rpix[2] };
 860         plugin->centroid(&pix[0], &rctr_v[0], &rctr_x[0], &rctr_y[0]);
 861         for( int i=0; i<3; ++i ) {
 862                 fft->do_fft(sz, 0, rpix[i], 0, ref_real[i], ref_imag[i]);
 863                 rpwr[i] = cor(sz, rpix[i], rpix[i]);
 864         }
 865         double scan_limit = 0.25; // quarter pixel resolution
 866 //printf("frame: %jd\n", plugin->get_source_position());
 867         while( bw/x_steps > scan_limit || bh/y_steps > scan_limit ) {
 868                 dx_result = dy_result = dt_result = 0;
 869                 cor_value = value = 0;
 870 //printf("  bx,by %6.2f,%-6.2f  bw,bh %6.2f,%-6.2f ",bx,by, bw,bh);
 871                 process_packages();
 872                 bx = dx_result;
 873                 by = dy_result;
 874 //printf(" r = %f(%f), %6.2f,%-6.2f\n",value,dt_result*180/M_PI,bx,by);
 875                 bw *= 0.5;  bh *= 0.5;
 876         }
 877 }
 878
 879 void Motion51Scan::init_packages()
 880 {
 881 // sort in increasing distance from current displacement
 882         double tx = plugin->rx + plugin->total_dx;
 883         double ty = plugin->ry + plugin->total_dy;
 884         int npkgs = get_total_packages();
 885         coord_t coords[npkgs];
 886         int i = 0;
 887         double x0 = bx - bw/2, y0 = by - bh/2;
 888         for( int iy=0; iy<y_steps; ++iy ) {
 889                 double y = y0 + iy*bh/y_steps;
 890                 for( int ix=0; ix<x_steps; ++ix ) {
 891                         double x = x0 + ix*bw/x_steps;
 892                         double d = sqrt(sqr(x-tx) + sqr(y-ty));
 893                         coord_t *cp = coords + i++;
 894                         cp->x = x; cp->y = y; cp->d = d;
 895                 }
 896         }
 897         qsort(&coords,npkgs,sizeof(coords[0]),coord_cmpr);
 898
 899         for( i=0; i<npkgs; ++i ) {
 900                 coord_t *cp = coords + i;
 901                 Motion51ScanPackage *pkg = (Motion51ScanPackage*)get_package(i);
 902                 pkg->x = cp->x;  pkg->y = cp->y;
 903         }
 904 }
 905
 906 Motion51ScanUnit::Motion51ScanUnit(Motion51Scan *server, Motion51Main *plugin)
 907  : LoadClient(server)
 908 {
 909         this->server = server;
 910         this->plugin = plugin;
 911         fft = new FFT;
 912 }
 913 Motion51ScanUnit::~Motion51ScanUnit()
 914 {
 915         delete fft;
 916 }
 917
 918 void Motion51ScanUnit::process_package(LoadPackage *package)
 919 {
 920         Motion51ScanPackage *pkg = (Motion51ScanPackage *)package;
 921         int sz = server->rpix_sz;
 922         double cur_real[3][sz], cur_imag[3][sz];
 923         double cpwr[3], cpix[3][sz], *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
 924         plugin->get_samples(server->cur, cur_pix, pkg->x, pkg->y);
 925
 926         double *pix[3] = { cpix[0], cpix[1], cpix[2] };
 927         double cctr_v[3], cctr_x[3], cctr_y[3];
 928         plugin->centroid(&pix[0], &cctr_v[0], &cctr_x[0], &cctr_y[0]);
 929         double mx = BC_CModels::calculate_max(server->ref->get_color_model());
 930         for( int i=0; i<3; ++i ) {
 931                 double v = 1. - fabs(server->rctr_v[i]-cctr_v[i]) / mx;
 932                 if( !passible(v) ) return;
 933                 double *rctr_x = server->rctr_x, *rctr_y = server->rctr_y;
 934                 double d = sqrt(sqr(rctr_x[i]-cctr_x[i]) + sqr(rctr_y[i]-cctr_y[i]));
 935                 v = 1 - d / plugin->cir_r;
 936                 if( !passible(v) ) return;
 937         }
 938         for( int i=0; i<3; ++i ) {
 939                 double cs = cor(sz, cpix[i], cpix[i]);
 940                 double rs = server->rpwr[i];
 941                 double ss = rs + cs;
 942                 if( ss == 0 ) ss = 1;
 943                 double v = 1. - fabs(rs - cs) / ss;
 944                 if( ! passible(v) ) return;
 945                 cpwr[i] = 1. / ss;
 946         }
 947
 948         double cor_real[3][sz], cor_imag[3][sz];
 949         for( int i=0; i<3; ++i ) {
 950                 fft->do_fft(sz, 0, cpix[i], 0, cur_real[i], cur_imag[i]);
 951                 cj_product(sz, 0, cur_real[i], cur_imag[i],
 952                         server->ref_real[i], server->ref_imag[i],
 953                         cur_real[i], cur_imag[i]);
 954                 fft->do_fft(sz, 1, cur_real[i], cur_imag[i], cor_real[i], cor_imag[i]);
 955         }
 956         double sv = 0;
 957         int st = 0;
 958         for( int t=0; t<sz; ++t ) {
 959                 double v = 0;
 960                 for( int i=0; i<3; ++i ) v += cor_real[i][t] * cpwr[i];
 961                 v = ((2*v) / 3);
 962                 if( sv >= v ) continue;
 963                 sv = v;  st = t;
 964         }
 965         if( server->cor_value > sv ) return;
 966         server->cor_value = sv;
 967         if( st > sz/2 ) st -= sz;
 968         int steps = plugin->r_steps;
 969         double tt = steps*(2*M_PI);
 970         double th = st*tt/sz, dt = th;
 971         double value = 0;
 972         double dth = (2*M_PI)/sz;
 973         for( int i=-steps; i<=steps; ++i ) {
 974                 double t = th + i*dth;
 975                 double v = server->compare(pkg->x, pkg->y, -t);
 976                 if( value >= v ) continue;
 977                 value = v;  dt = t;
 978         }
 979 //static int dbg = 0;
 980 //if( dbg )
 981 //printf("  %d. %.3f,%.3f %f = %f / %f + %f\n",
 982 // package_number,pkg->x,pkg->y,dt*180./M_PI,value,sv);
 983         server->result_lock->lock("Motion51Scan::process_package");
 984         if( value > server->value ) {
 985                 server->value = value;
 986                 server->dt_result = dt;
 987                 server->dx_result = pkg->x;
 988                 server->dy_result = pkg->y;
 989         }
 990         server->result_lock->unlock();
 991 }
 992