--- /dev/null
+#include "affine.h"
+#include "bchash.h"
+#include "clip.h"
+#include "filexml.h"
+#include "fourier.h"
+#include "keyframe.h"
+#include "language.h"
+#include "mainerror.h"
+#include "motion51.h"
+#include "motionwindow51.h"
+#include "mutex.h"
+#include "transportque.inc"
+
+#include <errno.h>
+#include <math.h>
+#include <unistd.h>
+
+static const double passing = 0.92;
+static bool passible(double v) { return v > passing; }
+
+
+REGISTER_PLUGIN(Motion51Main)
+
+void Motion51Config::init()
+{
+ sample_steps = 1024;
+ sample_r = 50.f;
+ block_x = block_y = 50.f;
+ block_w = block_h = 50.f;
+ horiz_limit = vert_limit = twist_limit = 50.f;
+ shake_fade = twist_fade = 3.f;
+ draw_vectors = 1;
+ strcpy(tracking_file, TRACKING_FILE);
+ tracking = 0;
+}
+
+Motion51Config::Motion51Config()
+{
+ init();
+}
+
+int Motion51Config::equivalent(Motion51Config &that)
+{
+ return horiz_limit == that.horiz_limit &&
+ vert_limit == that.vert_limit &&
+ twist_limit == that.twist_limit &&
+ shake_fade == that.shake_fade &&
+ twist_fade == that.twist_fade &&
+ sample_r == that.sample_r &&
+ sample_steps == that.sample_steps &&
+ draw_vectors == that.draw_vectors &&
+ EQUIV(block_x, that.block_x) &&
+ EQUIV(block_y, that.block_y) &&
+ block_w == that.block_w &&
+ block_h == that.block_h &&
+ !strcmp(tracking_file, that.tracking_file) &&
+ tracking == that.tracking;
+}
+
+void Motion51Config::copy_from(Motion51Config &that)
+{
+ horiz_limit = that.horiz_limit;
+ vert_limit = that.vert_limit;
+ twist_limit = that.twist_limit;
+ shake_fade = that.shake_fade;
+ twist_fade = that.twist_fade;
+ sample_r = that.sample_r;
+ sample_steps = that.sample_steps;
+ draw_vectors = that.draw_vectors;
+ block_x = that.block_x;
+ block_y = that.block_y;
+ block_w = that.block_w;
+ block_h = that.block_h;
+ strcpy(tracking_file, that.tracking_file);
+ tracking = that.tracking;
+}
+
+void Motion51Config::interpolate(Motion51Config &prev, Motion51Config &next,
+ int64_t prev_frame, int64_t next_frame, int64_t current_frame)
+{
+ copy_from(prev);
+}
+
+
+Motion51Main::Motion51Main(PluginServer *server)
+ : PluginVClient(server)
+{
+ out_frame = 0; out_position = -1;
+ ref_frame = 0; ref_position = -1;
+ tmp_frame = 0;
+ affine = 0;
+ motion_scan = 0;
+
+ cache_file[0] = 0;
+ cache_fp = active_fp = 0;
+ cache_line[0] = 0;
+ cache_key = active_key = -1;
+ tracking_position = -1;
+
+ out_w = out_h = out_r = 0;
+ rx = ry = rw = rh = rr = 0;
+ current_dx = current_dy = 0;
+ x_steps = y_steps = 16;
+ r_steps = 4;
+ cir_sz = 0; cir_r = 0;
+ xpts = ypts = 0;
+ total_dx = total_dy = 0;
+ total_angle = 0;
+}
+
+Motion51Main::~Motion51Main()
+{
+ update_cache_file();
+ delete out_frame;
+ delete ref_frame;
+ delete tmp_frame;
+ delete affine;
+ delete motion_scan;
+ delete [] xpts;
+ delete [] ypts;
+}
+
+const char* Motion51Main::plugin_title() { return _("Motion51"); }
+int Motion51Main::is_realtime() { return 1; }
+int Motion51Main::is_multichannel() { return 1; }
+
+
+NEW_WINDOW_MACRO(Motion51Main, Motion51Window)
+LOAD_CONFIGURATION_MACRO(Motion51Main, Motion51Config)
+
+
+void Motion51Main::update_gui()
+{
+ if( !thread ) return;
+ if( !load_configuration() ) return;
+ thread->window->lock_window("Motion51Main::update_gui");
+ Motion51Window *window = (Motion51Window*)thread->window;
+ window->update_gui();
+ thread->window->unlock_window();
+}
+
+
+
+
+void Motion51Main::save_data(KeyFrame *keyframe)
+{
+ FileXML output;
+
+// cause data to be stored directly in text
+ output.set_shared_output(keyframe->get_data(), MESSAGESIZE);
+ output.tag.set_title("MOTION51");
+ output.tag.set_property("HORIZ_LIMIT", config.horiz_limit);
+ output.tag.set_property("VERT_LIMIT", config.vert_limit);
+ output.tag.set_property("TWIST_LIMIT", config.twist_limit);
+ output.tag.set_property("SHAKE_FADE", config.shake_fade);
+ output.tag.set_property("TWIST_FADE", config.twist_fade);
+ output.tag.set_property("SAMPLE_R", config.sample_r);
+ output.tag.set_property("SAMPLE_STEPS", config.sample_steps);
+ output.tag.set_property("DRAW_VECTORS", config.draw_vectors);
+ output.tag.set_property("BLOCK_W", config.block_w);
+ output.tag.set_property("BLOCK_H", config.block_h);
+ output.tag.set_property("BLOCK_X", config.block_x);
+ output.tag.set_property("BLOCK_Y", config.block_y);
+ output.tag.set_property("TRACKING_FILE", config.tracking_file);
+ output.tag.set_property("TRACKING", config.tracking);
+ output.append_tag();
+ output.tag.set_title("/MOTION51");
+ output.append_tag();
+ output.terminate_string();
+}
+
+void Motion51Main::read_data(KeyFrame *keyframe)
+{
+ FileXML input;
+ input.set_shared_input(keyframe->get_data(), strlen(keyframe->get_data()));
+ int result = 0;
+
+ while( !(result = input.read_tag()) ) {
+ if( input.tag.title_is("MOTION51") ) {
+ config.horiz_limit = input.tag.get_property("HORIZ_LIMIT", config.horiz_limit);
+ config.vert_limit = input.tag.get_property("VERT_LIMIT", config.vert_limit);
+ config.twist_limit = input.tag.get_property("TWIST_LIMIT", config.twist_limit);
+ config.shake_fade = input.tag.get_property("SHAKE_FADE", config.shake_fade);
+ config.twist_fade = input.tag.get_property("TWIST_FADE", config.twist_fade);
+ config.sample_r = input.tag.get_property("SAMPLE_R", config.sample_r);
+ config.sample_steps = input.tag.get_property("SAMPLE_STEPS", config.sample_steps);
+ config.draw_vectors = input.tag.get_property("DRAW_VECTORS", config.draw_vectors);
+ config.block_w = input.tag.get_property("BLOCK_W", config.block_w);
+ config.block_h = input.tag.get_property("BLOCK_H", config.block_h);
+ config.block_x = input.tag.get_property("BLOCK_X", config.block_x);
+ config.block_y = input.tag.get_property("BLOCK_Y", config.block_y);
+ input.tag.get_property("TRACKING_FILE", config.tracking_file);
+ config.tracking = input.tag.get_property("TRACKING", config.tracking);
+ }
+ }
+}
+
+#if 0
+static void snap(const char *fn, VFrame *img, float x, float y, float r)
+{
+ VFrame vfrm(img->get_w(),img->get_h(),img->get_color_model());
+ vfrm.copy_from(img);
+ vfrm.draw_smooth(x-r,y, x-r,y+r, x,y+r);
+ vfrm.draw_smooth(x,y+r, x+r,y+r, x+r,y);
+ vfrm.draw_smooth(x+r,y, x+r,y-r, x,y-r);
+ vfrm.draw_smooth(x,y-r, x-r,y-r, x-r,y);
+ vfrm.write_png(fn);
+}
+#endif
+
+#if 0
+// nearest sample
+static void nearest_uint8(double *pix[3], double rx, double ry,
+ uint8_t **rows, int psz, int iw1, int ih1)
+{
+ int ix = (int)rx, iy = (int)ry;
+ bclamp(ix, 0, iw1); bclamp(iy, 0, ih1);
+ uint8_t *cp = (uint8_t*)(rows[iy] + psz * ix);
+ for( int i=0; i<3; ++pix[i], ++cp, ++i ) *pix[i] = *cp;
+}
+static void nearest_float(double *pix[3], double rx, double ry,
+ uint8_t **rows, int psz, int iw1, int ih1)
+{
+ int ix = (int)rx, iy = (int)ry;
+ bclamp(ix, 0, iw1); bclamp(iy, 0, ih1);
+ float *fp = (float*)(rows[iy] + psz * ix);
+ for( int i=0; i<3; ++pix[i], ++fp, ++i ) *pix[i] = *fp;
+}
+#endif
+
+// corner interpolation sample
+static void corner_uint8(double *pix[3], double rx, double ry,
+ uint8_t **rows, int psz, int iw1, int ih1)
+{
+ bclamp(rx, 0, iw1); bclamp(ry, 0, ih1);
+ int iy = (int)ry;
+ double yf1 = ry - iy, yf0 = 1.0 - yf1;
+ uint8_t *row0 = rows[iy];
+ if( iy < ih1 ) ++iy;
+ uint8_t *row1 = rows[iy];
+ int ix = (int)rx;
+ double xf1 = rx - ix, xf0 = 1.0 - xf1;
+ int i0 = psz * ix;
+ if( ix < iw1 ) ++ix;
+ int i1 = psz * ix;
+ uint8_t *cp00 = (uint8_t*)&row0[i0], *cp01 = (uint8_t*)&row0[i1];
+ uint8_t *cp10 = (uint8_t*)&row1[i0], *cp11 = (uint8_t*)&row1[i1];
+ double a00 = xf0 * yf0, a01 = xf1 * yf0;
+ double a10 = xf0 * yf1, a11 = xf1 * yf1;
+ for( int i=0; i<3; ++pix[i], ++cp00, ++cp01, ++cp10, ++cp11, ++i )
+ *pix[i] = *cp00*a00 + *cp01*a01 + *cp10*a10 + *cp11*a11;
+}
+static void corner_float(double *pix[3], double rx, double ry,
+ uint8_t **rows, int psz, int iw1, int ih1)
+{
+ bclamp(rx, 0, iw1); bclamp(ry, 0, ih1);
+ int iy = (int)ry;
+ double yf1 = ry - iy, yf0 = 1.0 - yf1;
+ uint8_t *row0 = rows[iy];
+ if( iy < ih1 ) ++iy;
+ uint8_t *row1 = rows[iy];
+ int ix = (int)rx;
+ double xf1 = rx - ix, xf0 = 1.0 - xf1;
+ int i0 = psz * ix;
+ if( ix < iw1 ) ++ix;
+ int i1 = psz * ix;
+ float *fp00 = (float*)&row0[i0], *fp01 = (float*)&row0[i1];
+ float *fp10 = (float*)&row1[i0], *fp11 = (float*)&row1[i1];
+ double a00 = xf0 * yf0, a01 = xf1 * yf0;
+ double a10 = xf0 * yf1, a11 = xf1 * yf1;
+ for( int i=0; i<3; ++pix[i], ++fp00, ++fp01, ++fp10, ++fp11, ++i )
+ *pix[i] = *fp00*a00 + *fp01*a01 + *fp10*a10 + *fp11*a11;
+}
+
+
+static inline double cor(int n, double *ap, double *bp)
+{
+ double s = 0;
+ while( --n >= 0 ) s += *ap++ * *bp++;
+ return s;
+}
+
+static inline double sqr(double v) { return v*v; }
+
+static inline void cj_product(int n, int sf, double *rp, double *ip,
+ double *arp, double *aip, double *brp, double *bip)
+{
+ int m = !sf ? n-1 : n/2, i = 0;
+ while( i <= m ) {
+ double ar = arp[i], ai = aip[i];
+ double br = brp[i], bi = -bip[i];
+ rp[i] = ar*br - ai*bi; // complex a*ib'
+ ip[i] = ar*bi + ai*br;
+ ++i;
+ }
+ if( !sf ) return;
+ while( --m > 0 ) { rp[i] = rp[m]; ip[i] = -ip[m]; ++i; }
+}
+
+typedef struct { double x, y, d; } coord_t;
+static int coord_cmpr(const void *ap, const void *bp)
+{
+ coord_t *a = (coord_t *)ap, *b = (coord_t *)bp;
+ return a->d == b->d ? 0 : a->d < b->d ? -1 : 1;
+}
+
+
+int64_t Motion51Main::get_ref_position()
+{
+ int64_t position = out_position - 1;
+ if( position < 0 || position != ref_position ) {
+// clip to edit boundaries
+ int64_t pos = get_source_start();
+ if( position < pos )
+ position = pos;
+ else if( position >= (pos += get_total_len()) )
+ position = pos-1;
+// clip to keyframe boundaries
+ KeyFrame *next_keyframe = get_next_keyframe(out_position, 1);
+ int64_t keyframe_end = next_keyframe->position;
+ if( (pos=next_keyframe->position) > 0 && position > keyframe_end )
+ position = keyframe_end;
+ KeyFrame *prev_keyframe = get_prev_keyframe(out_position, 1);
+ int64_t keyframe_start = prev_keyframe->position;
+ if( keyframe_start > 0 && position < keyframe_start )
+ position = keyframe_start;
+ }
+ return position;
+}
+
+void Motion51Main::set_tracking_path()
+{
+ const char *sp = TRACKING_FILE;
+ char *cp = config.tracking_file, *ep = cp+sizeof(config.tracking_file)-1;
+ while( cp < ep && *sp != 0 ) *cp++ = *sp++;
+ if( cp < ep && (sp=get_source_path()) ) {
+ *cp++ = '-';
+ const char *bp = strrchr(sp,'/');
+ if( bp ) sp = bp+1;
+ while( cp < ep && *sp != 0 ) {
+ *cp++ = (*sp>='a' && *sp<='z') ||
+ (*sp>='A' && *sp<='Z') ||
+ (*sp>='0' && *sp<='9') ? *sp : '_';
+ ++sp;
+ }
+ }
+ *cp = 0;
+}
+
+void Motion51Main::update_tracking_cache()
+{
+ if( (!config.tracking && cache_fp) || (config.tracking && !cache_fp) ||
+ (active_fp && active_key > get_source_position()) )
+ update_cache_file();
+}
+
+int Motion51Main::load_tracking_cache(int64_t position)
+{
+ if( !config.tracking ) return 1;
+ if( get_cache_line(position) ) return 1;
+ if( sscanf(cache_line, "%jd %f %f %f", &position, &dx, &dy, &dt) != 4 ) return 1;
+ return 0;
+}
+
+void Motion51Main::save_tracking_cache(int64_t position)
+{
+ if( !config.tracking ) return;
+ char line[BCSTRLEN];
+ snprintf(line, sizeof(line), "%jd %f %f %f\n", position, dx, dy, dt);
+ put_cache_line(line);
+}
+
+void Motion51Main::match(VFrame *ref, VFrame *cur)
+{
+ if( !motion_scan ) {
+ int cpus = get_project_smp()+1;
+ motion_scan = new Motion51Scan(this, cpus, x_steps, y_steps);
+ }
+
+ motion_scan->scan(ref, cur, config.sample_steps);
+
+ if( passible(motion_scan->cor_value) ) {
+ dx = motion_scan->dx_result - rx;
+ dy = motion_scan->dy_result - ry;
+ dt = motion_scan->dt_result * 180./M_PI;
+ }
+ else {
+ total_dx = total_dy = total_angle = 0;
+ dx = dy = dt = 0;
+ }
+
+}
+
+int Motion51Main::transform_target(int use_opengl)
+{
+ if( dx || dy || dt ) {
+ int cpus = get_project_smp()+1;
+ if( !affine ) affine = new AffineEngine(cpus, cpus);
+ affine->set_in_pivot(rx, ry);
+ affine->set_out_pivot(rx-dx, ry-dy);
+ if( use_opengl )
+ return run_opengl();
+ new_temp(out_frame, out)->copy_from(out);
+ out->clear_frame();
+ affine->rotate(out, out_frame, dt);
+//printf("transform_target at %jd: rotate(%f, %f, %f)\n", out_position, dx, dy, dt);
+ }
+ return 0;
+}
+
+int Motion51Main::handle_opengl()
+{
+#ifdef HAVE_GL
+ affine->set_opengl(1);
+ affine->rotate(out, out, dt);
+ out->screen_to_ram();
+ affine->set_opengl(0);
+#endif
+ return 0;
+}
+
+
+int Motion51Main::process_buffer(VFrame **frame, int64_t position, double frame_rate)
+{
+ int need_reconfigure = load_configuration();
+
+ int target_layer = 0;
+ int reference_layer = PluginClient::total_in_buffers-1;
+ VFrame *ref_layer = frame[reference_layer];
+ out = frame[target_layer];
+ out_position = position;
+ if( !out_position ) total_dx = total_dy = total_angle = 0;
+ get_pixel = BC_CModels::is_float(out->get_color_model()) ?
+ &corner_float : &corner_uint8;
+
+ int use_opengl = get_use_opengl();
+ read_frame(out, target_layer, out_position, frame_rate, use_opengl);
+ out_w = out->get_w();
+ out_h = out->get_h();
+ out_r = 0.5 * (out_w < out_h ? out_w : out_h);
+ rw = out_w * config.block_w/100.;
+ rh = out_h * config.block_h/100.;
+ rx = out_w * config.block_x/100.;
+ ry = out_h * config.block_y/100.;
+ rr = out_r * config.sample_r/100.;
+ reset_sample(config.sample_steps, rr);
+ dx = 0; dy = 0; dt = 0;
+
+ update_tracking_cache();
+ if( load_tracking_cache(out_position) ) {
+ int64_t ref_pos = get_ref_position();
+ if( !ref_frame || ref_pos != ref_position || need_reconfigure ) {
+ new_temp(ref_frame, ref_layer);
+ read_frame(ref_frame, reference_layer, ref_pos, frame_rate, 0);
+ total_dx = total_dy = 0; total_angle = 0;
+ ref_position = ref_pos;
+ }
+ VFrame *cur_frame = out;
+ if( reference_layer != target_layer ) {
+ new_temp(tmp_frame, ref_layer);
+ read_frame(tmp_frame, reference_layer, out_position, frame_rate, 0);
+ cur_frame = tmp_frame;
+ }
+ match(ref_frame, cur_frame);
+ save_tracking_cache(out_position);
+ }
+ current_dx = dx; current_dy = dy;
+ double sf = 1. - config.shake_fade/100.;
+ dx += total_dx * sf; dy += total_dy * sf;
+ double rf = 1. - config.twist_fade/100.;
+ dt += total_angle * rf;
+ if( dt < -180. ) dt += 360.;
+ else if( dt > 180. ) dt -= 360.;
+
+ float tot_dx = out_w * config.horiz_limit/100.;
+ bclamp(dx, -tot_dx, tot_dx);
+ float tot_dy = out_h * config.vert_limit/100.;
+ bclamp(dy, -tot_dy, tot_dy);
+ float tot_dt = 180. * config.twist_limit/100.;
+ bclamp(dt, -tot_dt, +tot_dt);
+ total_dx = dx; total_dy = dy; total_angle = dt;
+ if( ref_frame && reference_layer == target_layer &&
+ ref_position+1 == out_position &&
+ ref_frame->get_w() == out->get_w() &&
+ ref_frame->get_h() == out->get_h() &&
+ ref_frame->get_color_model() == out->get_color_model() ) {
+ ref_frame->copy_from(out);
+ ref_position = out_position;
+ }
+ transform_target(use_opengl);
+
+ if( config.draw_vectors )
+ draw_vectors(out);
+
+ return 0;
+}
+
+VFrame* Motion51Main::new_temp(VFrame *&tmp, VFrame *ref)
+{
+ if( tmp && (tmp->get_w() != ref->get_w() || tmp->get_h() != ref->get_h() ||
+ tmp->get_color_model() != ref->get_color_model()) ) {
+ delete tmp; tmp = 0;
+ }
+ if( !tmp )
+ tmp = new VFrame(0, -1, ref->get_w(), ref->get_h(), ref->get_color_model(), -1);
+ return tmp;
+}
+
+void Motion51Main::reset_sample(int sz, double r)
+{
+ if( cir_sz == sz && cir_r == r ) return;
+ if( cir_sz != sz ) {
+ cir_sz = sz;
+ delete xpts; xpts = new double[cir_sz];
+ delete ypts; ypts = new double[cir_sz];
+ }
+ cir_r = r;
+ int n = cir_sz / r_steps;
+ double dt = (2*M_PI)/n;
+ double dr = r / r_steps;
+ for( int it=0; it<n; ++it ) {
+ double t = it * dt, cos_t = cos(t), sin_t = sin(t);
+ for( int i=0; i<r_steps; ) {
+ int k = i * n + it;
+ double r = ++i * dr;
+ xpts[k] = r * cos_t;
+ ypts[k] = r * sin_t;
+ }
+ }
+}
+
+void Motion51Main::get_samples(VFrame *img, double *pix[3], double x, double y)
+{
+ int iw = img->get_w(), iw1 = iw-1;
+ int ih = img->get_h(), ih1 = ih-1;
+ uint8_t **rows = img->get_rows();
+ int psz = BC_CModels::calculate_pixelsize(img->get_color_model());
+
+ double *xp = xpts, *yp = ypts;
+ for( int i=cir_sz; --i>=0; ++xp,++yp ) {
+ double px = x + *xp, py = y + *yp;
+ get_pixel(pix, px, py, rows, psz, iw1, ih1);
+ }
+}
+
+void Motion51Main::centroid(double *pix[3], double *ctr_v, double *ctr_x, double *ctr_y)
+{
+ for( int i=0; i<3; ++i )
+ ctr_v[i] = ctr_x[i] = ctr_y[i] = 0;
+ double *xp = xpts, *yp = ypts;
+ for( int k=cir_sz; --k>=0; ++xp,++yp ) {
+ double x = rx + *xp, y = ry + *yp;
+ for( int i=0; i<3; ++pix[i],++i ) {
+ double v = *pix[i];
+ ctr_v[i] += v;
+ ctr_x[i] += v * x;
+ ctr_y[i] += v * y;
+ }
+ }
+ for( int i=0; i<3; ++i ) {
+ if( !ctr_v[i] ) continue;
+ ctr_x[i] /= ctr_v[i];
+ ctr_y[i] /= ctr_v[i];
+ ctr_v[i] /= cir_sz;
+ }
+}
+
+
+void Motion51Main::draw_vectors(VFrame *img)
+{
+ img->draw_arrow(rx, ry, rx+current_dx, ry+current_dy);
+
+// img->draw_smooth(rx-rr,ry, rx-rr,ry+rr, rx,ry+rr);
+// img->draw_smooth(rx,ry+rr, rx+rr,ry+rr, rx+rr,ry);
+// img->draw_smooth(rx+rr,ry, rx+rr,ry-rr, rx,ry-rr);
+// img->draw_smooth(rx,ry-rr, rx-rr,ry-rr, rx-rr,ry);
+
+ float rx1 = rx - 0.5*rw;
+ float ry1 = ry - 0.5*rh;
+ float rx2 = rx1 + rw;
+ float ry2 = ry1 + rh;
+
+ img->draw_line(rx1, ry1, rx2, ry1);
+ img->draw_line(rx2, ry1, rx2, ry2);
+ img->draw_line(rx2, ry2, rx1, ry2);
+ img->draw_line(rx1, ry2, rx1, ry1);
+
+ float sx1 = rx1 - rr, sy1 = ry1 - rr;
+ float sx2 = rx2 + rr, sy2 = ry2 + rr;
+
+ img->draw_smooth(sx1, ry1, sx1, sy1, rx1, sy1);
+ img->draw_line(rx1, sy1, rx2, sy1);
+ img->draw_smooth(rx2, sy1, sx2, sy1, sx2, ry1);
+ img->draw_line(sx2, ry1, sx2, ry2);
+ img->draw_smooth(sx2, ry2, sx2, sy2, rx2, sy2);
+ img->draw_line(rx2, sy2, rx1, sy2);
+ img->draw_smooth(rx1, sy2, sx1, sy2, sx1, ry2);
+ img->draw_line(sx1, ry2, sx1, ry1);
+
+ double *xp = xpts, *yp = ypts;
+ for( int i=cir_sz; --i>=0; ++xp, ++yp )
+ img->draw_pixel(rx+*xp, ry+*yp);
+}
+
+int Motion51Main::open_cache_file()
+{
+ if( cache_fp ) return 0;
+ if( !cache_file[0] ) return 1;
+ if( !(cache_fp = fopen(cache_file, "r")) ) return 1;
+// match timestamp, asset path
+ char line[BCTEXTLEN], *cp = line, *ep = cp+sizeof(line);
+ if( fgets(line,sizeof(line),cache_fp) ) {
+ int64_t tm = strtoul(cp,&cp,0);
+// time 0 matches everything
+ if( !tm ) return 0;
+ const char *sp = get_source_path();
+ if( !sp ) return 0;
+ if( cp < ep && *cp == ' ' ) ++cp;
+ int n = strlen(cp);
+ if( n > 0 && cp[n-1] == '\n' ) cp[n-1] = 0;
+ struct stat st;
+ if( !strcmp(cp, sp) && !stat(cp,&st) && st.st_mtime == tm )
+ return 0;
+ }
+ fclose(cache_fp); cache_fp = 0;
+ return 1;
+}
+
+void Motion51Main::close_cache_file()
+{
+ if( !cache_fp ) return;
+ fclose(cache_fp);
+ cache_fp = 0; cache_key = -1;
+ tracking_position = -1;
+}
+
+int Motion51Main::load_cache_line()
+{
+ cache_key = -1;
+ if( open_cache_file() ) return 1;
+ if( !fgets(cache_line, sizeof(cache_line), cache_fp) ) return 1;
+ cache_key = strtol(cache_line, 0, 0);
+ return 0;
+}
+
+int Motion51Main::get_cache_line(int64_t key)
+{
+ if( cache_key == key ) return 0;
+ if( open_cache_file() ) return 1;
+ if( cache_key >= 0 && key > cache_key ) {
+ if( load_cache_line() ) return 1;
+ if( cache_key == key ) return 0;
+ if( cache_key > key ) return 1;
+ }
+// binary search file
+ fseek(cache_fp, 0, SEEK_END);
+ int64_t l = -1, r = ftell(cache_fp);
+ while( (r - l) > 1 ) {
+ int64_t m = (l + r) / 2;
+ fseek(cache_fp, m, SEEK_SET);
+// skip to start of next line
+ if( !fgets(cache_line, sizeof(cache_line), cache_fp) )
+ return -1;
+ if( !load_cache_line() ) {
+ if( cache_key == key )
+ return 0;
+ if( cache_key < key ) { l = m; continue; }
+ }
+ r = m;
+ }
+ return 1;
+}
+
+int Motion51Main::locate_cache_line(int64_t key)
+{
+ int ret = 1;
+ if( key < 0 || !(ret=get_cache_line(key)) ||
+ ( cache_key >= 0 && cache_key < key ) )
+ ret = load_cache_line();
+ return ret;
+}
+
+int Motion51Main::put_cache_line(const char *line)
+{
+ int64_t key = strtol(line, 0, 0);
+ if( key == active_key ) return 1;
+ if( !active_fp ) {
+ close_cache_file();
+ sprintf(cache_file, "%s.bak", config.tracking_file);
+ ::rename(config.tracking_file, cache_file);
+ if( !(active_fp = fopen(config.tracking_file, "w")) ) {
+ perror(config.tracking_file);
+ fprintf(stderr, "err writing key %jd\n", key);
+ return -1;
+ }
+ const char *sp = get_source_path();
+ int64_t tm = 0;
+ if( sp ) {
+ struct stat st;
+ if( !stat(sp,&st) ) tm = st.st_mtime;
+ }
+ fprintf(active_fp, "%jd %s\n",tm, sp);
+ active_key = -1;
+ }
+
+ if( active_key < key ) {
+ locate_cache_line(active_key);
+ while( cache_key >= 0 && key >= cache_key ) {
+ if( key > cache_key )
+ fputs(cache_line, active_fp);
+ load_cache_line();
+ }
+ }
+
+ active_key = key;
+ fputs(line, active_fp);
+ fflush(active_fp);
+ return 0;
+}
+
+void Motion51Main::update_cache_file()
+{
+ if( active_fp ) {
+ locate_cache_line(active_key);
+ while( cache_key >= 0 ) {
+ fputs(cache_line, active_fp);
+ load_cache_line();
+ }
+ close_cache_file();
+ ::remove(cache_file);
+ fclose(active_fp); active_fp = 0;
+ active_key = -1;
+ }
+ else
+ close_cache_file();
+ strcpy(cache_file, config.tracking_file);
+}
+
+Motion51Scan::Motion51Scan(Motion51Main *plugin, int n_thds, int x_steps, int y_steps)
+ : LoadServer(n_thds, x_steps*y_steps)
+{
+ this->plugin = plugin;
+ this->x_steps = x_steps;
+ this->y_steps = y_steps;
+ this->fft = new FFT;
+ this->result_lock = new Mutex("Motion51Scan::result_lock");
+
+ cur = ref = 0;
+ bx = by = br = bw = bh = 0;
+ rpix_sz = 0;
+ for( int i=0; i<3; ++i ) {
+ rpix[i] = 0; rpwr[i] = 0;
+ rctr_v[i] = rctr_x[i] = rctr_y[i] = 0;
+ ref_real[i] = ref_imag[i] = 0;
+ }
+ cor_value = value = 0;
+ dx_result = dy_result = 0;
+ dt_result = 0;
+}
+
+
+Motion51Scan::~Motion51Scan()
+{
+ for( int i=0; i<3; ++i ) {
+ delete [] rpix[i];
+ delete [] ref_real[i];
+ delete [] ref_imag[i];
+ }
+ delete fft;
+ delete result_lock;
+}
+
+// sum absolute diff of ref at (rx,ry) - (cur(cx,cy) rotated ct)
+// downsampled using corner_sample to x_steps, y_steps
+double Motion51Scan::compare(double cx, double cy, double ct)
+{
+ int iw = ref->get_w(), iw1 = iw-1;
+ int ih = ref->get_h(), ih1 = ih-1;
+ int xsz = x_steps;// iw;
+ int ysz = y_steps;// ih;
+ int psz = BC_CModels::calculate_pixelsize(cur->get_color_model());
+ uint8_t **ref_rows = ref->get_rows();
+ uint8_t **cur_rows = cur->get_rows();
+ double cos_ct = cos(ct), sin_ct = sin(ct);
+ double rx = plugin->rx, ry = plugin->ry;
+ double sx = (double)iw/xsz, sy = (double)ih/ysz;
+ double cpix[3][xsz], rpix[3][xsz];
+ double v = 0;
+ for( int iy=0; iy<y_steps; ++iy ) {
+ double y = iy * sy;
+ double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
+ double *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
+ for( int ix=0; ix<x_steps; ++ix ) {
+ double x = ix * sx;
+ plugin->get_pixel(ref_pix, x, y, ref_rows, psz, iw1, ih1);
+ double tx = x-rx, ty = y-ry;
+ double xt = cos_ct*tx - sin_ct*ty + cx;
+ double yt = cos_ct*ty + sin_ct*tx + cy;
+ plugin->get_pixel(cur_pix, xt, yt, cur_rows, psz, iw1, ih1);
+ }
+ for( int i=0; i<3; ++i ) {
+ double *rp = rpix[i], *cp = cpix[i];
+ for( int k=x_steps; --k>=0; ++rp,++cp ) v += fabs(*rp - *cp);
+ }
+ }
+ double mx = BC_CModels::calculate_max(ref->get_color_model());
+ v = 1.-v/(3*mx * x_steps*y_steps);
+ return v;
+}
+
+void Motion51Scan::scan(VFrame *ref, VFrame *cur, int sz)
+{
+ this->ref = ref;
+ this->cur = cur;
+ if( this->rpix_sz != sz ) {
+ this->rpix_sz = sz;
+ for( int i=0; i<3; ++i ) {
+ delete [] rpix[i]; rpix[i] = new double[sz];
+ delete [] ref_real[i]; ref_real[i] = new double[sz];
+ delete [] ref_imag[i]; ref_imag[i] = new double[sz];
+ }
+ }
+
+ bw = plugin->rw; bh = plugin->rh;
+ bx = plugin->rx; by = plugin->ry;
+ br = plugin->rr;
+
+ double *ref_pix[3] = { rpix[0], rpix[1], rpix[2] };
+ plugin->get_samples(ref, ref_pix, bx, by);
+ double *pix[3] = { rpix[0], rpix[1], rpix[2] };
+ plugin->centroid(&pix[0], &rctr_v[0], &rctr_x[0], &rctr_y[0]);
+ for( int i=0; i<3; ++i ) {
+ fft->do_fft(sz, 0, rpix[i], 0, ref_real[i], ref_imag[i]);
+ rpwr[i] = cor(sz, rpix[i], rpix[i]);
+ }
+ double scan_limit = 0.25; // quarter pixel resolution
+//printf("frame: %jd\n", plugin->get_source_position());
+ while( bw/x_steps > scan_limit || bh/y_steps > scan_limit ) {
+ dx_result = dy_result = dt_result = 0;
+ cor_value = value = 0;
+//printf(" bx,by %6.2f,%-6.2f bw,bh %6.2f,%-6.2f ",bx,by, bw,bh);
+ process_packages();
+ bx = dx_result;
+ by = dy_result;
+//printf(" r = %f(%f), %6.2f,%-6.2f\n",value,dt_result*180/M_PI,bx,by);
+ bw *= 0.5; bh *= 0.5;
+ }
+}
+
+void Motion51Scan::init_packages()
+{
+// sort in increasing distance from current displacement
+ double tx = plugin->rx + plugin->total_dx;
+ double ty = plugin->ry + plugin->total_dy;
+ int npkgs = get_total_packages();
+ coord_t coords[npkgs];
+ int i = 0;
+ double x0 = bx - bw/2, y0 = by - bh/2;
+ for( int iy=0; iy<y_steps; ++iy ) {
+ double y = y0 + iy*bh/y_steps;
+ for( int ix=0; ix<x_steps; ++ix ) {
+ double x = x0 + ix*bw/x_steps;
+ double d = sqrt(sqr(x-tx) + sqr(y-ty));
+ coord_t *cp = coords + i++;
+ cp->x = x; cp->y = y; cp->d = d;
+ }
+ }
+ qsort(&coords,npkgs,sizeof(coords[0]),coord_cmpr);
+
+ for( i=0; i<npkgs; ++i ) {
+ coord_t *cp = coords + i;
+ Motion51ScanPackage *pkg = (Motion51ScanPackage*)get_package(i);
+ pkg->x = cp->x; pkg->y = cp->y;
+ }
+}
+
+Motion51ScanUnit::Motion51ScanUnit(Motion51Scan *server, Motion51Main *plugin)
+ : LoadClient(server)
+{
+ this->server = server;
+ this->plugin = plugin;
+ fft = new FFT;
+}
+Motion51ScanUnit::~Motion51ScanUnit()
+{
+ delete fft;
+}
+
+void Motion51ScanUnit::process_package(LoadPackage *package)
+{
+ Motion51ScanPackage *pkg = (Motion51ScanPackage *)package;
+ int sz = server->rpix_sz;
+ double cur_real[3][sz], cur_imag[3][sz];
+ double cpwr[3], cpix[3][sz], *cur_pix[3] = { cpix[0], cpix[1], cpix[2] };
+ plugin->get_samples(server->cur, cur_pix, pkg->x, pkg->y);
+
+ double *pix[3] = { cpix[0], cpix[1], cpix[2] };
+ double cctr_v[3], cctr_x[3], cctr_y[3];
+ plugin->centroid(&pix[0], &cctr_v[0], &cctr_x[0], &cctr_y[0]);
+ double mx = BC_CModels::calculate_max(server->ref->get_color_model());
+ for( int i=0; i<3; ++i ) {
+ double v = 1. - fabs(server->rctr_v[i]-cctr_v[i]) / mx;
+ if( !passible(v) ) return;
+ double *rctr_x = server->rctr_x, *rctr_y = server->rctr_y;
+ double d = sqrt(sqr(rctr_x[i]-cctr_x[i]) + sqr(rctr_y[i]-cctr_y[i]));
+ v = 1 - d / plugin->cir_r;
+ if( !passible(v) ) return;
+ }
+ for( int i=0; i<3; ++i ) {
+ double cs = cor(sz, cpix[i], cpix[i]);
+ double rs = server->rpwr[i];
+ double ss = rs + cs;
+ if( ss == 0 ) ss = 1;
+ double v = 1. - fabs(rs - cs) / ss;
+ if( ! passible(v) ) return;
+ cpwr[i] = 1. / ss;
+ }
+
+ double cor_real[3][sz], cor_imag[3][sz];
+ for( int i=0; i<3; ++i ) {
+ fft->do_fft(sz, 0, cpix[i], 0, cur_real[i], cur_imag[i]);
+ cj_product(sz, 0, cur_real[i], cur_imag[i],
+ server->ref_real[i], server->ref_imag[i],
+ cur_real[i], cur_imag[i]);
+ fft->do_fft(sz, 1, cur_real[i], cur_imag[i], cor_real[i], cor_imag[i]);
+ }
+ double sv = 0;
+ int st = 0;
+ for( int t=0; t<sz; ++t ) {
+ double v = 0;
+ for( int i=0; i<3; ++i ) v += cor_real[i][t] * cpwr[i];
+ v = ((2*v) / 3);
+ if( sv >= v ) continue;
+ sv = v; st = t;
+ }
+ if( server->cor_value > sv ) return;
+ server->cor_value = sv;
+ if( st > sz/2 ) st -= sz;
+ int steps = plugin->r_steps;
+ double tt = steps*(2*M_PI);
+ double th = st*tt/sz, dt = th;
+ double value = 0;
+ double dth = (2*M_PI)/sz;
+ for( int i=-steps; i<=steps; ++i ) {
+ double t = th + i*dth;
+ double v = server->compare(pkg->x, pkg->y, -t);
+ if( value >= v ) continue;
+ value = v; dt = t;
+ }
+//static int dbg = 0;
+//if( dbg )
+//printf(" %d. %.3f,%.3f %f = %f / %f + %f\n",
+// package_number,pkg->x,pkg->y,dt*180./M_PI,value,sv);
+ server->result_lock->lock("Motion51Scan::process_package");
+ if( value > server->value ) {
+ server->value = value;
+ server->dt_result = dt;
+ server->dx_result = pkg->x;
+ server->dy_result = pkg->y;
+ }
+ server->result_lock->unlock();
+}
+
projects / goodguy / history.git / blobdiff