cinelerra-5.1/plugins/motion51/motion51.C

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