minor changes; mostly for new Context Help feature
[goodguy/cinelerra.git] / 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         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(ref->get_w(), ref->get_h(), ref->get_color_model(), 0);
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 Motion51VVFrame::Motion51VVFrame(VFrame *vfrm, int n)
569  : VFrame(vfrm->get_data(), -1, vfrm->get_y()-vfrm->get_data(),
570         vfrm->get_u()-vfrm->get_data(), vfrm->get_v()-vfrm->get_data(),
571         vfrm->get_w(), vfrm->get_h(), vfrm->get_color_model(),
572         vfrm->get_bytes_per_line())
573 {
574         this->n = n;
575 }
576
577 int Motion51VVFrame::draw_pixel(int x, int y)
578 {
579         VFrame::draw_pixel(x+0, y+0);
580         for( int i=1; i<n; ++i ) {
581                 VFrame::draw_pixel(x-i, y-i);
582                 VFrame::draw_pixel(x-i, y+i);
583                 VFrame::draw_pixel(x+i, y-i);
584                 VFrame::draw_pixel(x+i, y+i);
585         }
586         return 0;
587 }
588
589 void Motion51Main::draw_vectors(VFrame *img)
590 {
591         int iw = img->get_w(), ih = img->get_h();
592         int mx = iw > ih ? iw : ih;
593         int n = mx/800 + 1;
594         Motion51VVFrame vfrm(img, n);
595         vfrm.set_pixel_color(WHITE);
596         int m = 2;  while( m < n ) m <<= 1;
597         vfrm.set_stiple(2*m);
598
599         vfrm.draw_arrow(rx, ry, rx+current_dx, ry+current_dy);
600 //      vfrm.draw_smooth(rx-rr,ry, rx-rr,ry+rr, rx,ry+rr);
601 //      vfrm.draw_smooth(rx,ry+rr, rx+rr,ry+rr, rx+rr,ry);
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
605         float rx1 = rx - 0.5*rw;
606         float ry1 = ry - 0.5*rh;
607         float rx2 = rx1 + rw;
608         float ry2 = ry1 + rh;
609
610         vfrm.draw_line(rx1, ry1, rx2, ry1);
611         vfrm.draw_line(rx2, ry1, rx2, ry2);
612         vfrm.draw_line(rx2, ry2, rx1, ry2);
613         vfrm.draw_line(rx1, ry2, rx1, ry1);
614
615         float sx1 = rx1 - rr, sy1 = ry1 - rr;
616         float sx2 = rx2 + rr, sy2 = ry2 + rr;
617
618         vfrm.draw_smooth(sx1, ry1, sx1, sy1, rx1, sy1);
619         vfrm.draw_line(rx1, sy1, rx2, sy1);
620         vfrm.draw_smooth(rx2, sy1, sx2, sy1, sx2, ry1);
621         vfrm.draw_line(sx2, ry1, sx2, ry2);
622         vfrm.draw_smooth(sx2, ry2, sx2, sy2, rx2, sy2);
623         vfrm.draw_line(rx2, sy2, rx1, sy2);
624         vfrm.draw_smooth(rx1, sy2, sx1, sy2, sx1, ry2);
625         vfrm.draw_line(sx1, ry2, sx1, ry1);
626
627         double *xp = xpts, *yp = ypts;
628         for( int i=cir_sz; --i>=0; ++xp, ++yp )
629                 vfrm.draw_pixel(rx+*xp, ry+*yp);
630 }
631
632 int Motion51Main::open_cache_file()
633 {
634         if( cache_fp ) return 0;
635         if( !cache_file[0] ) return 1;
636         if( !(cache_fp = fopen(cache_file, "r")) ) return 1;
637 // match timestamp, asset path
638         char line[BCTEXTLEN], *cp = line, *ep = cp+sizeof(line);
639         if( fgets(line,sizeof(line),cache_fp) ) {
640                 int64_t tm = strtoul(cp,&cp,0);
641 // time 0 matches everything
642                 if( !tm ) return 0;
643                 const char *sp = get_source_path();
644                 if( !sp ) return 0;
645                 if( cp < ep && *cp == ' ' ) ++cp;
646                 int n = strlen(cp);
647                 if( n > 0 && cp[n-1] == '\n' ) cp[n-1] = 0;
648                 struct stat st;
649                 if( !strcmp(cp, sp) && !stat(cp,&st) && st.st_mtime == tm )
650                         return 0;
651         }
652         fclose(cache_fp);  cache_fp = 0;
653         return 1;
654 }
655
656 void Motion51Main::close_cache_file()
657 {
658         if( !cache_fp ) return;
659         fclose(cache_fp);
660         cache_fp = 0; cache_key = -1;
661         tracking_position = -1;
662 }
663
664 int Motion51Main::load_cache_line()
665 {
666         cache_key = -1;
667         if( open_cache_file() ) return 1;
668         if( !fgets(cache_line, sizeof(cache_line), cache_fp) ) return 1;
669         cache_key = strtol(cache_line, 0, 0);
670         return 0;
671 }
672
673 int Motion51Main::get_cache_line(int64_t key)
674 {
675         if( cache_key == key ) return 0;
676         if( open_cache_file() ) return 1;
677         if( cache_key >= 0 && key > cache_key ) {
678                 if( load_cache_line() ) return 1;
679                 if( cache_key == key ) return 0;
680                 if( cache_key > key ) return 1;
681         }
682 // binary search file
683         fseek(cache_fp, 0, SEEK_END);
684         int64_t l = -1, r = ftell(cache_fp);
685         while( (r - l) > 1 ) {
686                 int64_t m = (l + r) / 2;
687                 fseek(cache_fp, m, SEEK_SET);
688 // skip to start of next line
689                 if( !fgets(cache_line, sizeof(cache_line), cache_fp) )
690                         return -1;
691                 if( !load_cache_line() ) {
692                         if( cache_key == key )
693                                 return 0;
694                         if( cache_key < key ) { l = m; continue; }
695                 }
696                 r = m;
697         }
698         return 1;
699 }
700
701 int Motion51Main::locate_cache_line(int64_t key)
702 {
703         int ret = 1;
704         if( key < 0 || !(ret=get_cache_line(key)) ||
705             ( cache_key >= 0 && cache_key < key ) )
706                 ret = load_cache_line();
707         return ret;
708 }
709
710 int Motion51Main::put_cache_line(const char *line)
711 {
712         int64_t key = strtol(line, 0, 0);
713         if( key == active_key ) return 1;
714         if( !active_fp ) {
715                 close_cache_file();
716                 snprintf(cache_file, sizeof(cache_file), "%s.bak", config.tracking_file);
717                 ::rename(config.tracking_file, cache_file);
718                 if( !(active_fp = fopen(config.tracking_file, "w")) ) {
719                         perror(config.tracking_file);
720                         fprintf(stderr, "err writing key %jd\n", key);
721                         return -1;
722                 }
723                 const char *sp = get_source_path();
724                 int64_t tm = 0;
725                 if( sp ) {
726                         struct stat st;
727                         if( !stat(sp,&st) ) tm = st.st_mtime;
728                 }
729                 fprintf(active_fp, "%jd %s\n",tm, sp);
730                 active_key = -1;
731         }
732
733         if( active_key < key ) {
734                 locate_cache_line(active_key);
735                 while( cache_key >= 0 && key >= cache_key ) {
736                         if( key > cache_key )
737                                 fputs(cache_line, active_fp);
738                         load_cache_line();
739                 }
740         }
741
742         active_key = key;
743         fputs(line, active_fp);
744         fflush(active_fp);
745         return 0;
746 }
747
748 void Motion51Main::update_cache_file()
749 {
750         if( active_fp ) {
751                 locate_cache_line(active_key);
752                 while( cache_key >= 0 ) {
753                         fputs(cache_line, active_fp);
754                         load_cache_line();
755                 }
756                 close_cache_file();
757                 ::remove(cache_file);
758                 fclose(active_fp);  active_fp = 0;
759                 active_key = -1;
760         }
761         else
762                 close_cache_file();
763         strcpy(cache_file, config.tracking_file);
764 }
765
766 Motion51Scan::Motion51Scan(Motion51Main *plugin, int n_thds, int x_steps, int y_steps)
767  : LoadServer(n_thds, x_steps*y_steps)
768 {
769         this->plugin = plugin;
770         this->x_steps = x_steps;
771         this->y_steps = y_steps;
772         this->fft = new FFT;
773         this->result_lock = new Mutex("Motion51Scan::result_lock");
774
775         cur = ref = 0;
776         bx = by = br = bw = bh = 0;
777         rpix_sz = 0;
778         for( int i=0; i<3; ++i ) {
779                 rpix[i] = 0;  rpwr[i] = 0;
780                 rctr_v[i] = rctr_x[i] = rctr_y[i] = 0;
781                 ref_real[i] = ref_imag[i] = 0;
782         }
783         cor_value = value = 0;
784         dx_result = dy_result = 0;
785         dt_result = 0;
786 }
787
788
789 Motion51Scan::~Motion51Scan()
790 {
791         for( int i=0; i<3; ++i ) {
792                 delete [] rpix[i];
793                 delete [] ref_real[i];
794                 delete [] ref_imag[i];
795         }
796         delete fft;
797         delete result_lock;
798 }
799
800 // sum absolute diff of ref at (rx,ry) - (cur(cx,cy) rotated ct)
801 //   downsampled using corner_sample to x_steps, y_steps
802 double Motion51Scan::compare(double cx, double cy, double ct)
803 {
804         int iw = ref->get_w(), iw1 = iw-1;
805         int ih = ref->get_h(), ih1 = ih-1;
806         int xsz = x_steps;// iw;
807         int ysz = y_steps;// ih;
808         int psz = BC_CModels::calculate_pixelsize(cur->get_color_model());
809         uint8_t **ref_rows = ref->get_rows();
810         uint8_t **cur_rows = cur->get_rows();
811         double cos_ct = cos(ct), sin_ct = sin(ct);
812         double rx = plugin->rx, ry = plugin->ry;
813         double sx = (double)iw/xsz, sy = (double)ih/ysz;
814         double cpix[3][xsz], rpix[3][xsz];
815         double v = 0;
816         for( int iy=0; iy<y_steps; ++iy ) {
817                 double y = iy * sy;
818                 double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
819                 double *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
820                 for( int ix=0; ix<x_steps; ++ix ) {
821                         double x = ix * sx;
822                         plugin->get_pixel(ref_pix, x, y, ref_rows, psz, iw1, ih1);
823                         double tx = x-rx, ty = y-ry;
824                         double xt = cos_ct*tx - sin_ct*ty + cx;
825                         double yt = cos_ct*ty + sin_ct*tx + cy;
826                         plugin->get_pixel(cur_pix, xt, yt, cur_rows, psz, iw1, ih1);
827                 }
828                 for( int i=0; i<3; ++i ) {
829                         double *rp = rpix[i], *cp = cpix[i];
830                         for( int k=x_steps; --k>=0; ++rp,++cp ) v += fabs(*rp - *cp);
831                 }
832         }
833         double mx = BC_CModels::calculate_max(ref->get_color_model());
834         v = 1.-v/(3*mx * x_steps*y_steps);
835         return v;
836 }
837
838 void Motion51Scan::scan(VFrame *ref, VFrame *cur, int sz)
839 {
840         this->ref = ref;
841         this->cur = cur;
842         if( this->rpix_sz != sz ) {
843                 this->rpix_sz = sz;
844                 for( int i=0; i<3; ++i ) {
845                         delete [] rpix[i];      rpix[i] = new double[sz];
846                         delete [] ref_real[i];  ref_real[i] = new double[sz];
847                         delete [] ref_imag[i];  ref_imag[i] = new double[sz];
848                 }
849         }
850
851         bw = plugin->rw;  bh = plugin->rh;
852         bx = plugin->rx;  by = plugin->ry;
853         br = plugin->rr;
854
855         double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
856         plugin->get_samples(ref, ref_pix, bx, by);
857         double *pix[3] = { rpix[0], rpix[1], rpix[2] };
858         plugin->centroid(&pix[0], &rctr_v[0], &rctr_x[0], &rctr_y[0]);
859         for( int i=0; i<3; ++i ) {
860                 fft->do_fft(sz, 0, rpix[i], 0, ref_real[i], ref_imag[i]);
861                 rpwr[i] = cor(sz, rpix[i], rpix[i]);
862         }
863         double scan_limit = 0.25; // quarter pixel resolution
864 //printf("frame: %jd\n", plugin->get_source_position());
865         while( bw/x_steps > scan_limit || bh/y_steps > scan_limit ) {
866                 dx_result = dy_result = dt_result = 0;
867                 cor_value = value = 0;
868 //printf("  bx,by %6.2f,%-6.2f  bw,bh %6.2f,%-6.2f ",bx,by, bw,bh);
869                 process_packages();
870                 bx = dx_result;
871                 by = dy_result;
872 //printf(" r = %f(%f), %6.2f,%-6.2f\n",value,dt_result*180/M_PI,bx,by);
873                 bw *= 0.5;  bh *= 0.5;
874         }
875 }
876
877 void Motion51Scan::init_packages()
878 {
879 // sort in increasing distance from current displacement
880         double tx = plugin->rx + plugin->total_dx;
881         double ty = plugin->ry + plugin->total_dy;
882         int npkgs = get_total_packages();
883         coord_t coords[npkgs];
884         int i = 0;
885         double x0 = bx - bw/2, y0 = by - bh/2;
886         for( int iy=0; iy<y_steps; ++iy ) {
887                 double y = y0 + iy*bh/y_steps;
888                 for( int ix=0; ix<x_steps; ++ix ) {
889                         double x = x0 + ix*bw/x_steps;
890                         double d = sqrt(sqr(x-tx) + sqr(y-ty));
891                         coord_t *cp = coords + i++;
892                         cp->x = x; cp->y = y; cp->d = d;
893                 }
894         }
895         qsort(&coords,npkgs,sizeof(coords[0]),coord_cmpr);
896
897         for( i=0; i<npkgs; ++i ) {
898                 coord_t *cp = coords + i;
899                 Motion51ScanPackage *pkg = (Motion51ScanPackage*)get_package(i);
900                 pkg->x = cp->x;  pkg->y = cp->y;
901         }
902 }
903
904 Motion51ScanUnit::Motion51ScanUnit(Motion51Scan *server, Motion51Main *plugin)
905  : LoadClient(server)
906 {
907         this->server = server;
908         this->plugin = plugin;
909         fft = new FFT;
910 }
911 Motion51ScanUnit::~Motion51ScanUnit()
912 {
913         delete fft;
914 }
915
916 void Motion51ScanUnit::process_package(LoadPackage *package)
917 {
918         Motion51ScanPackage *pkg = (Motion51ScanPackage *)package;
919         int sz = server->rpix_sz;
920         double cur_real[3][sz], cur_imag[3][sz];
921         double cpwr[3], cpix[3][sz], *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
922         plugin->get_samples(server->cur, cur_pix, pkg->x, pkg->y);
923
924         double *pix[3] = { cpix[0], cpix[1], cpix[2] };
925         double cctr_v[3], cctr_x[3], cctr_y[3];
926         plugin->centroid(&pix[0], &cctr_v[0], &cctr_x[0], &cctr_y[0]);
927         double mx = BC_CModels::calculate_max(server->ref->get_color_model());
928         for( int i=0; i<3; ++i ) {
929                 double v = 1. - fabs(server->rctr_v[i]-cctr_v[i]) / mx;
930                 if( !passible(v) ) return;
931                 double *rctr_x = server->rctr_x, *rctr_y = server->rctr_y;
932                 double d = sqrt(sqr(rctr_x[i]-cctr_x[i]) + sqr(rctr_y[i]-cctr_y[i]));
933                 v = 1 - d / plugin->cir_r;
934                 if( !passible(v) ) return;
935         }
936         for( int i=0; i<3; ++i ) {
937                 double cs = cor(sz, cpix[i], cpix[i]);
938                 double rs = server->rpwr[i];
939                 double ss = rs + cs;
940                 if( ss == 0 ) ss = 1;
941                 double v = 1. - fabs(rs - cs) / ss;
942                 if( ! passible(v) ) return;
943                 cpwr[i] = 1. / ss;
944         }
945
946         double cor_real[3][sz], cor_imag[3][sz];
947         for( int i=0; i<3; ++i ) {
948                 fft->do_fft(sz, 0, cpix[i], 0, cur_real[i], cur_imag[i]);
949                 cj_product(sz, 0, cur_real[i], cur_imag[i],
950                         server->ref_real[i], server->ref_imag[i],
951                         cur_real[i], cur_imag[i]);
952                 fft->do_fft(sz, 1, cur_real[i], cur_imag[i], cor_real[i], cor_imag[i]);
953         }
954         double sv = 0;
955         int st = 0;
956         for( int t=0; t<sz; ++t ) {
957                 double v = 0;
958                 for( int i=0; i<3; ++i ) v += cor_real[i][t] * cpwr[i];
959                 v = ((2*v) / 3);
960                 if( sv >= v ) continue;
961                 sv = v;  st = t;
962         }
963         if( server->cor_value > sv ) return;
964         server->cor_value = sv;
965         if( st > sz/2 ) st -= sz;
966         int steps = plugin->r_steps;
967         double tt = steps*(2*M_PI);
968         double th = st*tt/sz, dt = th;
969         double value = 0;
970         double dth = (2*M_PI)/sz;
971         for( int i=-steps; i<=steps; ++i ) {
972                 double t = th + i*dth;
973                 double v = server->compare(pkg->x, pkg->y, -t);
974                 if( value >= v ) continue;
975                 value = v;  dt = t;
976         }
977 //static int dbg = 0;
978 //if( dbg )
979 //printf("  %d. %.3f,%.3f %f = %f / %f + %f\n",
980 // package_number,pkg->x,pkg->y,dt*180./M_PI,value,sv);
981         server->result_lock->lock("Motion51Scan::process_package");
982         if( value > server->value ) {
983                 server->value = value;
984                 server->dt_result = dt;
985                 server->dx_result = pkg->x;
986                 server->dy_result = pkg->y;
987         }
988         server->result_lock->unlock();
989 }
990