X-Git-Url: http://git.cinelerra-gg.org/git/?a=blobdiff_plain;f=cinelerra-5.1%2Fplugins%2Fmotion-hv%2Fmotionscan-hv.C;fp=cinelerra-5.1%2Fplugins%2Fmotion-hv%2Fmotionscan-hv.C;h=db8617eaf301f7d30726a49bbc637b89194eecd4;hb=1e154071bd323f756625f8172ef67133a561450a;hp=0000000000000000000000000000000000000000;hpb=3c944af1216990dd2d963a6b0231f94df0af7947;p=goodguy%2Fhistory.git diff --git a/cinelerra-5.1/plugins/motion-hv/motionscan-hv.C b/cinelerra-5.1/plugins/motion-hv/motionscan-hv.C new file mode 100644 index 00000000..db8617ea --- /dev/null +++ b/cinelerra-5.1/plugins/motion-hv/motionscan-hv.C @@ -0,0 +1,1936 @@ + +/* + * CINELERRA + * Copyright (C) 2016 Adam Williams + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +#include "affine.h" +#include "bcsignals.h" +#include "clip.h" +#include "motionscan-hv.h" +#include "mutex.h" +#include "vframe.h" + + +#include +#include +#include + +// The module which does the actual scanning + +// starting level of detail +#define STARTING_DOWNSAMPLE 16 +// minimum size in each level of detail +#define MIN_DOWNSAMPLED_SIZE 16 +// minimum scan range +#define MIN_DOWNSAMPLED_SCAN 4 +// scan range for subpixel mode +#define SUBPIXEL_RANGE 4 + +MotionHVScanPackage::MotionHVScanPackage() + : LoadPackage() +{ + valid = 1; +} + + + + + + +MotionHVScanUnit::MotionHVScanUnit(MotionHVScan *server) + : LoadClient(server) +{ + this->server = server; +} + +MotionHVScanUnit::~MotionHVScanUnit() +{ +} + + +void MotionHVScanUnit::single_pixel(MotionHVScanPackage *pkg) +{ + //int w = server->current_frame->get_w(); + //int h = server->current_frame->get_h(); + int color_model = server->current_frame->get_color_model(); + int pixel_size = BC_CModels::calculate_pixelsize(color_model); + int row_bytes = server->current_frame->get_bytes_per_line(); + +// printf("MotionHVScanUnit::process_package %d search_x=%d search_y=%d scan_x1=%d scan_y1=%d scan_x2=%d scan_y2=%d x_steps=%d y_steps=%d\n", +// __LINE__, +// pkg->search_x, +// pkg->search_y, +// pkg->scan_x1, +// pkg->scan_y1, +// pkg->scan_x2, +// pkg->scan_y2, +// server->x_steps, +// server->y_steps); + +// Pointers to first pixel in each block + unsigned char *prev_ptr = server->previous_frame->get_rows()[ + pkg->search_y] + + pkg->search_x * pixel_size; + unsigned char *current_ptr = 0; + + if(server->do_rotate) + { + current_ptr = server->rotated_current[pkg->angle_step]->get_rows()[ + pkg->block_y1] + + pkg->block_x1 * pixel_size; + } + else + { + current_ptr = server->current_frame->get_rows()[ + pkg->block_y1] + + pkg->block_x1 * pixel_size; + } + +// Scan block + pkg->difference1 = MotionHVScan::abs_diff(prev_ptr, + current_ptr, + row_bytes, + pkg->block_x2 - pkg->block_x1, + pkg->block_y2 - pkg->block_y1, + color_model); + +// printf("MotionHVScanUnit::process_package %d angle_step=%d diff=%d\n", +// __LINE__, +// pkg->angle_step, +// pkg->difference1); +// printf("MotionHVScanUnit::process_package %d search_x=%d search_y=%d diff=%lld\n", +// __LINE__, server->block_x1 - pkg->search_x, server->block_y1 - pkg->search_y, pkg->difference1); +} + +void MotionHVScanUnit::subpixel(MotionHVScanPackage *pkg) +{ +//PRINT_TRACE + //int w = server->current_frame->get_w(); + //int h = server->current_frame->get_h(); + int color_model = server->current_frame->get_color_model(); + int pixel_size = BC_CModels::calculate_pixelsize(color_model); + int row_bytes = server->current_frame->get_bytes_per_line(); + unsigned char *prev_ptr = server->previous_frame->get_rows()[ + pkg->search_y] + + pkg->search_x * pixel_size; +// neglect rotation + unsigned char *current_ptr = server->current_frame->get_rows()[ + pkg->block_y1] + + pkg->block_x1 * pixel_size; + +// With subpixel, there are two ways to compare each position, one by shifting +// the previous frame and two by shifting the current frame. + pkg->difference1 = MotionHVScan::abs_diff_sub(prev_ptr, + current_ptr, + row_bytes, + pkg->block_x2 - pkg->block_x1, + pkg->block_y2 - pkg->block_y1, + color_model, + pkg->sub_x, + pkg->sub_y); + pkg->difference2 = MotionHVScan::abs_diff_sub(current_ptr, + prev_ptr, + row_bytes, + pkg->block_x2 - pkg->block_x1, + pkg->block_y2 - pkg->block_y1, + color_model, + pkg->sub_x, + pkg->sub_y); +// printf("MotionHVScanUnit::process_package sub_x=%d sub_y=%d search_x=%d search_y=%d diff1=%lld diff2=%lld\n", +// pkg->sub_x, +// pkg->sub_y, +// pkg->search_x, +// pkg->search_y, +// pkg->difference1, +// pkg->difference2); +} + +void MotionHVScanUnit::process_package(LoadPackage *package) +{ + MotionHVScanPackage *pkg = (MotionHVScanPackage*)package; + + +// Single pixel + if(!server->subpixel) + { + single_pixel(pkg); + } + else +// Sub pixel + { + subpixel(pkg); + } + + + + +} + + + + + + + + + + + + + + + + + + +MotionHVScan::MotionHVScan(int total_clients, + int total_packages) + : LoadServer( +// DEBUG +//1, 1 +total_clients, total_packages +) +{ + test_match = 1; + downsampled_previous = 0; + downsampled_current = 0; + rotated_current = 0; + rotater = 0; +} + +MotionHVScan::~MotionHVScan() +{ + delete downsampled_previous; + delete downsampled_current; + if(rotated_current) + { + for(int i = 0; i < total_rotated; i++) + { + delete rotated_current[i]; + } + + delete [] rotated_current; + } + delete rotater; +} + + +void MotionHVScan::init_packages() +{ +// Set package coords +// Total range of positions to scan with downsampling + int downsampled_scan_x1 = scan_x1 / current_downsample; + //int downsampled_scan_x2 = scan_x2 / current_downsample; + int downsampled_scan_y1 = scan_y1 / current_downsample; + //int downsampled_scan_y2 = scan_y2 / current_downsample; + int downsampled_block_x1 = block_x1 / current_downsample; + int downsampled_block_x2 = block_x2 / current_downsample; + int downsampled_block_y1 = block_y1 / current_downsample; + int downsampled_block_y2 = block_y2 / current_downsample; + + +//printf("MotionHVScan::init_packages %d %d\n", __LINE__, get_total_packages()); +// printf("MotionHVScan::init_packages %d current_downsample=%d scan_x1=%d scan_x2=%d block_x1=%d block_x2=%d\n", +// __LINE__, +// current_downsample, +// downsampled_scan_x1, +// downsampled_scan_x2, +// downsampled_block_x1, +// downsampled_block_x2); +// if(current_downsample == 8 && downsampled_scan_x1 == 47) +// { +// downsampled_previous->write_png("/tmp/previous"); +// downsampled_current->write_png("/tmp/current"); +// } + + for(int i = 0; i < get_total_packages(); i++) + { + MotionHVScanPackage *pkg = (MotionHVScanPackage*)get_package(i); + + pkg->block_x1 = downsampled_block_x1; + pkg->block_x2 = downsampled_block_x2; + pkg->block_y1 = downsampled_block_y1; + pkg->block_y2 = downsampled_block_y2; + pkg->difference1 = 0; + pkg->difference2 = 0; + pkg->dx = 0; + pkg->dy = 0; + pkg->valid = 1; + pkg->angle_step = 0; + + if(!subpixel) + { + if(rotation_pass) + { + pkg->search_x = scan_x1 / current_downsample; + pkg->search_y = scan_y1 / current_downsample; + pkg->angle_step = i; + } + else + { + + int current_x_step = (i % x_steps); + int current_y_step = (i / x_steps); + + //printf("MotionHVScan::init_packages %d i=%d x_step=%d y_step=%d angle_step=%d\n", + //__LINE__, i, current_x_step, current_y_step, current_angle_step); + pkg->search_x = downsampled_scan_x1 + current_x_step * + (scan_x2 - scan_x1) / current_downsample / x_steps; + pkg->search_y = downsampled_scan_y1 + current_y_step * + (scan_y2 - scan_y1) / current_downsample / y_steps; + + if(do_rotate) + { + pkg->angle_step = angle_steps / 2; + } + else + { + pkg->angle_step = 0; + } + } + + pkg->sub_x = 0; + pkg->sub_y = 0; + } + else + { + pkg->sub_x = i % (OVERSAMPLE * SUBPIXEL_RANGE); + pkg->sub_y = i / (OVERSAMPLE * SUBPIXEL_RANGE); + +// if(horizontal_only) +// { +// pkg->sub_y = 0; +// } +// +// if(vertical_only) +// { +// pkg->sub_x = 0; +// } + + pkg->search_x = scan_x1 + pkg->sub_x / OVERSAMPLE + 1; + pkg->search_y = scan_y1 + pkg->sub_y / OVERSAMPLE + 1; + pkg->sub_x %= OVERSAMPLE; + pkg->sub_y %= OVERSAMPLE; + + + +// printf("MotionHVScan::init_packages %d i=%d search_x=%d search_y=%d sub_x=%d sub_y=%d\n", +// __LINE__, +// i, +// pkg->search_x, +// pkg->search_y, +// pkg->sub_x, +// pkg->sub_y); + } + +// printf("MotionHVScan::init_packages %d %d,%d %d,%d %d,%d\n", +// __LINE__, +// scan_x1, +// scan_x2, +// scan_y1, +// scan_y2, +// pkg->search_x, +// pkg->search_y); + } +} + +LoadClient* MotionHVScan::new_client() +{ + return new MotionHVScanUnit(this); +} + +LoadPackage* MotionHVScan::new_package() +{ + return new MotionHVScanPackage; +} + + +void MotionHVScan::set_test_match(int value) +{ + this->test_match = value; +} + + + + +#define DOWNSAMPLE(type, temp_type, components, max) \ +{ \ + temp_type r; \ + temp_type g; \ + temp_type b; \ + temp_type a; \ + type **in_rows = (type**)src->get_rows(); \ + type **out_rows = (type**)dst->get_rows(); \ + \ + for(int i = 0; i < h; i += downsample) \ + { \ + int y1 = MAX(i, 0); \ + int y2 = MIN(i + downsample, h); \ + \ + \ + for(int j = 0; \ + j < w; \ + j += downsample) \ + { \ + int x1 = MAX(j, 0); \ + int x2 = MIN(j + downsample, w); \ + \ + temp_type scale = (x2 - x1) * (y2 - y1); \ + if(x2 > x1 && y2 > y1) \ + { \ + \ +/* Read in values */ \ + r = 0; \ + g = 0; \ + b = 0; \ + if(components == 4) a = 0; \ + \ + for(int k = y1; k < y2; k++) \ + { \ + type *row = in_rows[k] + x1 * components; \ + for(int l = x1; l < x2; l++) \ + { \ + r += *row++; \ + g += *row++; \ + b += *row++; \ + if(components == 4) a += *row++; \ + } \ + } \ + \ +/* Write average */ \ + r /= scale; \ + g /= scale; \ + b /= scale; \ + if(components == 4) a /= scale; \ + \ + type *row = out_rows[y1 / downsample] + \ + x1 / downsample * components; \ + *row++ = r; \ + *row++ = g; \ + *row++ = b; \ + if(components == 4) *row++ = a; \ + } \ + } \ +/*printf("DOWNSAMPLE 3 %d\n", i);*/ \ + } \ +} + + + + +void MotionHVScan::downsample_frame(VFrame *dst, + VFrame *src, + int downsample) +{ + int h = src->get_h(); + int w = src->get_w(); + +//PRINT_TRACE +//printf("downsample=%d w=%d h=%d dst=%d %d\n", downsample, w, h, dst->get_w(), dst->get_h()); + switch(src->get_color_model()) + { + case BC_RGB888: + DOWNSAMPLE(uint8_t, int64_t, 3, 0xff) + break; + case BC_RGB_FLOAT: + DOWNSAMPLE(float, float, 3, 1.0) + break; + case BC_RGBA8888: + DOWNSAMPLE(uint8_t, int64_t, 4, 0xff) + break; + case BC_RGBA_FLOAT: + DOWNSAMPLE(float, float, 4, 1.0) + break; + case BC_YUV888: + DOWNSAMPLE(uint8_t, int64_t, 3, 0xff) + break; + case BC_YUVA8888: + DOWNSAMPLE(uint8_t, int64_t, 4, 0xff) + break; + } +//PRINT_TRACE +} + +double MotionHVScan::step_to_angle(int step, double center) +{ + if(step < angle_steps / 2) + { + return center - angle_step * (angle_steps / 2 - step); + } + else + if(step > angle_steps / 2) + { + return center + angle_step * (step - angle_steps / 2); + } + else + { + return center; + } +} + +#ifdef STDDEV_TEST +static int compare(const void *p1, const void *p2) +{ + double value1 = *(double*)p1; + double value2 = *(double*)p2; + +//printf("compare %d value1=%f value2=%f\n", __LINE__, value1, value2); + return value1 > value2; +} +#endif + +// reject vectors based on content. It's the reason Goog can't stabilize timelapses. +//#define STDDEV_TEST + +// pixel accurate motion search +void MotionHVScan::pixel_search(int &x_result, int &y_result, double &r_result) +{ +// reduce level of detail until enough steps + while(current_downsample > 1 && + ((block_x2 - block_x1) / current_downsample < MIN_DOWNSAMPLED_SIZE || + (block_y2 - block_y1) / current_downsample < MIN_DOWNSAMPLED_SIZE + || + (scan_x2 - scan_x1) / current_downsample < MIN_DOWNSAMPLED_SCAN || + (scan_y2 - scan_y1) / current_downsample < MIN_DOWNSAMPLED_SCAN + )) + { + current_downsample /= 2; + } + + + +// create downsampled images. +// Need to keep entire frame to search for rotation. + int downsampled_prev_w = previous_frame_arg->get_w() / current_downsample; + int downsampled_prev_h = previous_frame_arg->get_h() / current_downsample; + int downsampled_current_w = current_frame_arg->get_w() / current_downsample; + int downsampled_current_h = current_frame_arg->get_h() / current_downsample; + +// printf("MotionHVScan::pixel_search %d current_downsample=%d current_frame_arg->get_w()=%d downsampled_current_w=%d\n", +// __LINE__, +// current_downsample, +// current_frame_arg->get_w(), +// downsampled_current_w); + + x_steps = (scan_x2 - scan_x1) / current_downsample; + y_steps = (scan_y2 - scan_y1) / current_downsample; + +// in rads + double test_angle1 = atan2((double)downsampled_current_h / 2 - 1, (double)downsampled_current_w / 2); + double test_angle2 = atan2((double)downsampled_current_h / 2, (double)downsampled_current_w / 2 - 1); + +// in deg + angle_step = 360.0f * fabs(test_angle1 - test_angle2) / 2 / M_PI; + +// printf("MotionHVScan::pixel_search %d test_angle1=%f test_angle2=%f angle_step=%f\n", +// __LINE__, +// 360.0f * test_angle1 / 2 / M_PI, +// 360.0f * test_angle2 / 2 / M_PI, +// angle_step); + + + if(do_rotate && angle_step < rotation_range) + { + angle_steps = 1 + (int)((scan_angle2 - scan_angle1) / angle_step + 0.5); + } + else + { + angle_steps = 1; + } + + + if(current_downsample > 1) + { + if(!downsampled_previous || + downsampled_previous->get_w() != downsampled_prev_w || + downsampled_previous->get_h() != downsampled_prev_h) + { + delete downsampled_previous; + downsampled_previous = new VFrame(); + downsampled_previous->set_use_shm(0); + downsampled_previous->reallocate(0, + -1, + 0, + 0, + 0, + downsampled_prev_w + 1, + downsampled_prev_h + 1, + previous_frame_arg->get_color_model(), + -1); + } + + if(!downsampled_current || + downsampled_current->get_w() != downsampled_current_w || + downsampled_current->get_h() != downsampled_current_h) + { + delete downsampled_current; + downsampled_current = new VFrame(); + downsampled_current->set_use_shm(0); + downsampled_current->reallocate(0, + -1, + 0, + 0, + 0, + downsampled_current_w + 1, + downsampled_current_h + 1, + current_frame_arg->get_color_model(), + -1); + } + + + downsample_frame(downsampled_previous, + previous_frame_arg, + current_downsample); + downsample_frame(downsampled_current, + current_frame_arg, + current_downsample); + previous_frame = downsampled_previous; + current_frame = downsampled_current; + + } + else + { + previous_frame = previous_frame_arg; + current_frame = current_frame_arg; + } + + + +// printf("MotionHVScan::pixel_search %d x_steps=%d y_steps=%d angle_steps=%d total_steps=%d\n", +// __LINE__, +// x_steps, +// y_steps, +// angle_steps, +// total_steps); + + + +// test variance of constant macroblock + int color_model = current_frame->get_color_model(); + int pixel_size = BC_CModels::calculate_pixelsize(color_model); + int row_bytes = current_frame->get_bytes_per_line(); + int block_w = block_x2 - block_x1; + int block_h = block_y2 - block_y1; + + unsigned char *current_ptr = + current_frame->get_rows()[block_y1 / current_downsample] + + (block_x1 / current_downsample) * pixel_size; + unsigned char *previous_ptr = + previous_frame->get_rows()[scan_y1 / current_downsample] + + (scan_x1 / current_downsample) * pixel_size; + + + +// test detail in prev & current frame + double range1 = calculate_range(current_ptr, + row_bytes, + block_w / current_downsample, + block_h / current_downsample, + color_model); + + if(range1 < 1) + { +printf("MotionHVScan::pixel_search %d range fail range1=%f\n", __LINE__, range1); + failed = 1; + return; + } + + double range2 = calculate_range(previous_ptr, + row_bytes, + block_w / current_downsample, + block_h / current_downsample, + color_model); + + if(range2 < 1) + { +printf("MotionHVScan::pixel_search %d range fail range2=%f\n", __LINE__, range2); + failed = 1; + return; + } + + +// create rotated images + if(rotated_current && + (total_rotated != angle_steps || + rotated_current[0]->get_w() != downsampled_current_w || + rotated_current[0]->get_h() != downsampled_current_h)) + { + for(int i = 0; i < total_rotated; i++) + { + delete rotated_current[i]; + } + + delete [] rotated_current; + rotated_current = 0; + total_rotated = 0; + } + + if(do_rotate) + { + total_rotated = angle_steps; + + + if(!rotated_current) + { + rotated_current = new VFrame*[total_rotated]; + bzero(rotated_current, sizeof(VFrame*) * total_rotated); + } + +// printf("MotionHVScan::pixel_search %d total_rotated=%d w=%d h=%d block_w=%d block_h=%d\n", +// __LINE__, +// total_rotated, +// downsampled_current_w, +// downsampled_current_h, +// (block_x2 - block_x1) / current_downsample, +// (block_y2 - block_y1) / current_downsample); + for(int i = 0; i < angle_steps; i++) + { + +// printf("MotionHVScan::pixel_search %d w=%d h=%d x=%d y=%d angle=%f\n", +// __LINE__, +// downsampled_current_w, +// downsampled_current_h, +// (block_x1 + block_x2) / 2 / current_downsample, +// (block_y1 + block_y2) / 2 / current_downsample, +// step_to_angle(i, r_result)); + +// printf("MotionHVScan::pixel_search %d i=%d rotated_current[i]=%p\n", +// __LINE__, +// i, +// rotated_current[i]); + if(!rotated_current[i]) + { + rotated_current[i] = new VFrame(); + rotated_current[i]->set_use_shm(0); + rotated_current[i]->reallocate(0, + -1, + 0, + 0, + 0, + downsampled_current_w + 1, + downsampled_current_h + 1, + current_frame_arg->get_color_model(), + -1); +//printf("MotionHVScan::pixel_search %d\n", __LINE__); + } + + + if(!rotater) + { + rotater = new AffineEngine(get_total_clients(), + get_total_clients()); + } + +// get smallest viewport size required for the angle + double diag = hypot((block_x2 - block_x1) / current_downsample, + (block_y2 - block_y1) / current_downsample); + double angle1 = atan2(block_y2 - block_y1, block_x2 - block_x1) + + TO_RAD(step_to_angle(i, r_result)); + double angle2 = -atan2(block_y2 - block_y1, block_x2 - block_x1) + + TO_RAD(step_to_angle(i, r_result)); + double max_horiz = MAX(abs(diag * cos(angle1)), abs(diag * cos(angle2))); + double max_vert = MAX(abs(diag * sin(angle1)), abs(diag * sin(angle2))); + int center_x = (block_x1 + block_x2) / 2 / current_downsample; + int center_y = (block_y1 + block_y2) / 2 / current_downsample; + int x1 = center_x - max_horiz / 2; + int y1 = center_y - max_vert / 2; + int x2 = x1 + max_horiz; + int y2 = y1 + max_vert; + CLAMP(x1, 0, downsampled_current_w - 1); + CLAMP(y1, 0, downsampled_current_h - 1); + CLAMP(x2, 0, downsampled_current_w - 1); + CLAMP(y2, 0, downsampled_current_h - 1); + +//printf("MotionHVScan::pixel_search %d %f %f %d %d\n", +//__LINE__, TO_DEG(angle1), TO_DEG(angle2), (int)max_horiz, (int)max_vert); + rotater->set_in_viewport(x1, + y1, + x2 - x1, + y2 - y1); + rotater->set_out_viewport(x1, + y1, + x2 - x1, + y2 - y1); + +// rotater->set_in_viewport(0, +// 0, +// downsampled_current_w, +// downsampled_current_h); +// rotater->set_out_viewport(0, +// 0, +// downsampled_current_w, +// downsampled_current_h); + + rotater->set_in_pivot(center_x, center_y); + rotater->set_out_pivot(center_x, center_y); + + rotater->rotate(rotated_current[i], + current_frame, + step_to_angle(i, r_result)); + +// rotated_current[i]->draw_rect(block_x1 / current_downsample, +// block_y1 / current_downsample, +// block_x2 / current_downsample, +// block_y2 / current_downsample); +// char string[BCTEXTLEN]; +// sprintf(string, "/tmp/rotated%d", i); +// rotated_current[i]->write_png(string); +//downsampled_previous->write_png("/tmp/previous"); +//printf("MotionHVScan::pixel_search %d\n", __LINE__); + } + } + + + + + + +// printf("MotionHVScan::pixel_search %d block x=%d y=%d w=%d h=%d\n", +// __LINE__, +// block_x1 / current_downsample, +// block_y1 / current_downsample, +// block_w / current_downsample, +// block_h / current_downsample); + + + + + + + +//exit(1); +// Test only translation of the middle rotated frame + rotation_pass = 0; + total_steps = x_steps * y_steps; + set_package_count(total_steps); + process_packages(); + + + + + + +// Get least difference + int64_t min_difference = -1; +#ifdef STDDEV_TEST + double stddev_table[get_total_packages()]; +#endif + for(int i = 0; i < get_total_packages(); i++) + { + MotionHVScanPackage *pkg = (MotionHVScanPackage*)get_package(i); + +#ifdef STDDEV_TEST + double stddev = sqrt(pkg->difference1) / + (block_w / current_downsample) / + (block_h / current_downsample) / + 3; +// printf("MotionHVScan::pixel_search %d current_downsample=%d search_x=%d search_y=%d diff1=%f\n", +// __LINE__, +// current_downsample, +// pkg->search_x, +// pkg->search_y, +// sqrt(pkg->difference1) / block_w / current_downsample / block_h / 3 /* / variance */); + +// printf("MotionHVScan::pixel_search %d range1=%f stddev=%f\n", +// __LINE__, +// range1, +// stddev); + + stddev_table[i] = stddev; +#endif // STDDEV_TEST + + if(pkg->difference1 < min_difference || i == 0) + { + min_difference = pkg->difference1; + x_result = pkg->search_x * current_downsample * OVERSAMPLE; + y_result = pkg->search_y * current_downsample * OVERSAMPLE; + +// printf("MotionHVScan::pixel_search %d x_result=%d y_result=%d angle_step=%d diff=%lld\n", +// __LINE__, +// block_x1 * OVERSAMPLE - x_result, +// block_y1 * OVERSAMPLE - y_result, +// pkg->angle_step, +// pkg->difference1); + + } + } + + +#ifdef STDDEV_TEST + qsort(stddev_table, get_total_packages(), sizeof(double), compare); + + +// reject motion vector if not similar enough +// if(stddev_table[0] > 0.2) +// { +// if(debug) +// { +// printf("MotionHVScan::pixel_search %d stddev fail min_stddev=%f\n", +// __LINE__, +// stddev_table[0]); +// } +// failed = 1; +// return; +// } + +if(debug) +{ + printf("MotionHVScan::pixel_search %d\n", __LINE__); + for(int i = 0; i < get_total_packages(); i++) + { + printf("%f\n", stddev_table[i]); + } +} + +// reject motion vector if not a sigmoid curve +// TODO: use linear interpolation + int steps = 2; + int step = get_total_packages() / steps; + double curve[steps]; + for(int i = 0; i < steps; i++) + { + int start = get_total_packages() * i / steps; + int end = get_total_packages() * (i + 1) / steps; + end = MIN(end, get_total_packages() - 1); + curve[i] = stddev_table[end] - stddev_table[start]; + } + + +// if(curve[0] < (curve[1] * 1.01) || +// curve[2] < (curve[1] * 1.01) || +// curve[0] < (curve[2] * 0.75)) +// if(curve[0] < curve[1]) +// { +// if(debug) +// { +// printf("MotionHVScan::pixel_search %d curve fail %f %f\n", +// __LINE__, +// curve[0], +// curve[1]); +// } +// failed = 1; +// return; +// } + +if(debug) +{ +printf("MotionHVScan::pixel_search %d curve=%f %f ranges=%f %f min_stddev=%f\n", +__LINE__, +curve[0], +curve[1], +range1, +range2, +stddev_table[0]); +} +#endif // STDDEV_TEST + + + + + + if(do_rotate) + { + rotation_pass = 1;; + total_steps = angle_steps; + scan_x1 = x_result / OVERSAMPLE; + scan_y1 = y_result / OVERSAMPLE; + set_package_count(total_steps); + process_packages(); + + + + min_difference = -1; + double prev_r_result = r_result; + for(int i = 0; i < get_total_packages(); i++) + { + MotionHVScanPackage *pkg = (MotionHVScanPackage*)get_package(i); + +// printf("MotionHVScan::pixel_search %d search_x=%d search_y=%d angle_step=%d sub_x=%d sub_y=%d diff1=%lld diff2=%lld\n", +// __LINE__, +// pkg->search_x, +// pkg->search_y, +// pkg->search_angle_step, +// pkg->sub_x, +// pkg->sub_y, +// pkg->difference1, +// pkg->difference2); + if(pkg->difference1 < min_difference || i == 0) + { + min_difference = pkg->difference1; + r_result = step_to_angle(i, prev_r_result); + + // printf("MotionHVScan::pixel_search %d x_result=%d y_result=%d angle_step=%d diff=%lld\n", + // __LINE__, + // block_x1 * OVERSAMPLE - x_result, + // block_y1 * OVERSAMPLE - y_result, + // pkg->angle_step, + // pkg->difference1); + } + } + } + + +// printf("MotionHVScan::scan_frame %d current_downsample=%d x_result=%f y_result=%f r_result=%f\n", +// __LINE__, +// current_downsample, +// (float)x_result / OVERSAMPLE, +// (float)y_result / OVERSAMPLE, +// r_result); + +} + + +// subpixel motion search +void MotionHVScan::subpixel_search(int &x_result, int &y_result) +{ + rotation_pass = 0; + previous_frame = previous_frame_arg; + current_frame = current_frame_arg; + +//printf("MotionHVScan::scan_frame %d %d %d\n", __LINE__, x_result, y_result); +// Scan every subpixel in a SUBPIXEL_RANGE * SUBPIXEL_RANGE square + total_steps = (SUBPIXEL_RANGE * OVERSAMPLE) * (SUBPIXEL_RANGE * OVERSAMPLE); + +// These aren't used in subpixel + x_steps = OVERSAMPLE * SUBPIXEL_RANGE; + y_steps = OVERSAMPLE * SUBPIXEL_RANGE; + angle_steps = 1; + + set_package_count(this->total_steps); + process_packages(); + +// Get least difference + int64_t min_difference = -1; + for(int i = 0; i < get_total_packages(); i++) + { + MotionHVScanPackage *pkg = (MotionHVScanPackage*)get_package(i); +//printf("MotionHVScan::scan_frame %d search_x=%d search_y=%d sub_x=%d sub_y=%d diff1=%lld diff2=%lld\n", +//__LINE__, pkg->search_x, pkg->search_y, pkg->sub_x, pkg->sub_y, pkg->difference1, pkg->difference2); + if(pkg->difference1 < min_difference || min_difference == -1) + { + min_difference = pkg->difference1; + +// The sub coords are 1 pixel up & left of the block coords + x_result = pkg->search_x * OVERSAMPLE + pkg->sub_x; + y_result = pkg->search_y * OVERSAMPLE + pkg->sub_y; + + +// Fill in results + dx_result = block_x1 * OVERSAMPLE - x_result; + dy_result = block_y1 * OVERSAMPLE - y_result; +//printf("MotionHVScan::scan_frame %d dx_result=%d dy_result=%d diff=%lld\n", +//__LINE__, dx_result, dy_result, min_difference); + } + + if(pkg->difference2 < min_difference) + { + min_difference = pkg->difference2; + + x_result = pkg->search_x * OVERSAMPLE - pkg->sub_x; + y_result = pkg->search_y * OVERSAMPLE - pkg->sub_y; + + dx_result = block_x1 * OVERSAMPLE - x_result; + dy_result = block_y1 * OVERSAMPLE - y_result; +//printf("MotionHVScan::scan_frame %d dx_result=%d dy_result=%d diff=%lld\n", +//__LINE__, dx_result, dy_result, min_difference); + } + } +} + + +void MotionHVScan::scan_frame(VFrame *previous_frame, + VFrame *current_frame, + int global_range_w, + int global_range_h, + int global_block_w, + int global_block_h, + int block_x, + int block_y, + int frame_type, + int tracking_type, + int action_type, + int horizontal_only, + int vertical_only, + int source_position, + int total_dx, + int total_dy, + int global_origin_x, + int global_origin_y, + int do_motion, + int do_rotate, + double rotation_center, + double rotation_range) +{ + this->previous_frame_arg = previous_frame; + this->current_frame_arg = current_frame; + this->horizontal_only = horizontal_only; + this->vertical_only = vertical_only; + this->previous_frame = previous_frame_arg; + this->current_frame = current_frame_arg; + this->global_origin_x = global_origin_x; + this->global_origin_y = global_origin_y; + this->action_type = action_type; + this->do_motion = do_motion; + this->do_rotate = do_rotate; + this->rotation_center = rotation_center; + this->rotation_range = rotation_range; + +//printf("MotionHVScan::scan_frame %d\n", __LINE__); + dx_result = 0; + dy_result = 0; + dr_result = 0; + failed = 0; + + subpixel = 0; +// starting level of detail +// TODO: base it on a table of resolutions + current_downsample = STARTING_DOWNSAMPLE; + angle_step = 0; + +// Single macroblock + int w = current_frame->get_w(); + int h = current_frame->get_h(); + +// Initial search parameters + scan_w = global_range_w; + scan_h = global_range_h; + + int block_w = global_block_w; + int block_h = global_block_h; + +// printf("MotionHVScan::scan_frame %d %d %d %d %d %d %d %d %d\n", +// __LINE__, +// global_range_w, +// global_range_h, +// global_block_w, +// global_block_h, +// scan_w, +// scan_h, +// block_w, +// block_h); + +// Location of block in previous frame + block_x1 = (int)(block_x - block_w / 2); + block_y1 = (int)(block_y - block_h / 2); + block_x2 = (int)(block_x + block_w / 2); + block_y2 = (int)(block_y + block_h / 2); + +// Offset to location of previous block. This offset needn't be very accurate +// since it's the offset of the previous image and current image we want. + if(frame_type == MotionHVScan::TRACK_PREVIOUS) + { + block_x1 += total_dx / OVERSAMPLE; + block_y1 += total_dy / OVERSAMPLE; + block_x2 += total_dx / OVERSAMPLE; + block_y2 += total_dy / OVERSAMPLE; + } + + skip = 0; + + switch(tracking_type) + { +// Don't calculate + case MotionHVScan::NO_CALCULATE: + dx_result = 0; + dy_result = 0; + dr_result = rotation_center; + skip = 1; + break; + + case MotionHVScan::LOAD: + { +// Load result from disk + char string[BCTEXTLEN]; + + skip = 1; + if(do_motion) + { + sprintf(string, "%s%06d", + MOTION_FILE, + source_position); +//printf("MotionHVScan::scan_frame %d %s\n", __LINE__, string); + FILE *input = fopen(string, "r"); + if(input) + { + int temp = fscanf(input, + "%d %d", + &dx_result, + &dy_result); + if( temp != 2 ) + printf("MotionHVScan::scan_frame %d %s\n", __LINE__, string); +// HACK +//dx_result *= 2; +//dy_result *= 2; +//printf("MotionHVScan::scan_frame %d %d %d\n", __LINE__, dx_result, dy_result); + fclose(input); + } + else + { + skip = 0; + } + } + + if(do_rotate) + { + sprintf(string, + "%s%06d", + ROTATION_FILE, + source_position); + FILE *input = fopen(string, "r"); + if(input) + { + int temp = fscanf(input, "%f", &dr_result); + if( temp != 1 ) + printf("MotionHVScan::scan_frame %d %s\n", __LINE__, string); +// DEBUG +//dr_result += 0.25; + fclose(input); + } + else + { + skip = 0; + } + } + break; + } + +// Scan from scratch + default: + skip = 0; + break; + } + + + + if(!skip && test_match) + { + if(previous_frame->data_matches(current_frame)) + { +printf("MotionHVScan::scan_frame: data matches. skipping.\n"); + dx_result = 0; + dy_result = 0; + dr_result = rotation_center; + skip = 1; + } + } + + +// Perform scan + if(!skip) + { +// Location of block in current frame + int origin_offset_x = this->global_origin_x; + int origin_offset_y = this->global_origin_y; + int x_result = block_x1 + origin_offset_x; + int y_result = block_y1 + origin_offset_y; + double r_result = rotation_center; + +// printf("MotionHVScan::scan_frame 1 %d %d %d %d %d %d %d %d\n", +// block_x1 + block_w / 2, +// block_y1 + block_h / 2, +// block_w, +// block_h, +// block_x1, +// block_y1, +// block_x2, +// block_y2); + + while(!failed) + { + scan_x1 = x_result - scan_w / 2; + scan_y1 = y_result - scan_h / 2; + scan_x2 = x_result + scan_w / 2; + scan_y2 = y_result + scan_h / 2; + scan_angle1 = r_result - rotation_range; + scan_angle2 = r_result + rotation_range; + + + +// Zero out requested values +// if(horizontal_only) +// { +// scan_y1 = block_y1; +// scan_y2 = block_y1 + 1; +// } +// if(vertical_only) +// { +// scan_x1 = block_x1; +// scan_x2 = block_x1 + 1; +// } + +// printf("MotionHVScan::scan_frame %d block_x1=%d block_y1=%d block_x2=%d block_y2=%d scan_x1=%d scan_y1=%d scan_x2=%d scan_y2=%d\n", +// __LINE__, +// block_x1, +// block_y1, +// block_x2, +// block_y2, +// scan_x1, +// scan_y1, +// scan_x2, +// scan_y2); + + +// Clamp the block coords before the scan so we get useful scan coords. + clamp_scan(w, + h, + &block_x1, + &block_y1, + &block_x2, + &block_y2, + &scan_x1, + &scan_y1, + &scan_x2, + &scan_y2, + 0); + + +// printf("MotionHVScan::scan_frame %d block_x1=%d block_y1=%d block_x2=%d block_y2=%d scan_x1=%d scan_y1=%d scan_x2=%d scan_y2=%d x_result=%d y_result=%d\n", +// __LINE__, +// block_x1, +// block_y1, +// block_x2, +// block_y2, +// scan_x1, +// scan_y1, +// scan_x2, +// scan_y2, +// x_result, +// y_result); +//if(y_result == 88) exit(0); + + +// Give up if invalid coords. + if(scan_y2 <= scan_y1 || + scan_x2 <= scan_x1 || + block_x2 <= block_x1 || + block_y2 <= block_y1) + { + break; + } + +// For subpixel, the top row and left column are skipped + if(subpixel) + { + + subpixel_search(x_result, y_result); +// printf("MotionHVScan::scan_frame %d x_result=%d y_result=%d\n", +// __LINE__, +// x_result / OVERSAMPLE, +// y_result / OVERSAMPLE); + + break; + } + else +// Single pixel + { + pixel_search(x_result, y_result, r_result); +//printf("MotionHVScan::scan_frame %d x_result=%d y_result=%d\n", __LINE__, x_result / OVERSAMPLE, y_result / OVERSAMPLE); + + if(failed) + { + dr_result = 0; + dx_result = 0; + dy_result = 0; + } + else + if(current_downsample <= 1) + { + // Single pixel accuracy reached. Now do exhaustive subpixel search. + if(action_type == MotionHVScan::STABILIZE || + action_type == MotionHVScan::TRACK || + action_type == MotionHVScan::NOTHING) + { + x_result /= OVERSAMPLE; + y_result /= OVERSAMPLE; +//printf("MotionHVScan::scan_frame %d x_result=%d y_result=%d\n", __LINE__, x_result, y_result); + scan_w = SUBPIXEL_RANGE; + scan_h = SUBPIXEL_RANGE; +// Final R result + dr_result = rotation_center - r_result; + subpixel = 1; + } + else + { +// Fill in results and quit + dx_result = block_x1 * OVERSAMPLE - x_result; + dy_result = block_y1 * OVERSAMPLE - y_result; + dr_result = rotation_center - r_result; + break; + } + } + else +// Reduce scan area and try again + { +// scan_w = (scan_x2 - scan_x1) / 2; +// scan_h = (scan_y2 - scan_y1) / 2; +// need slightly more than 2x downsampling factor + + if(current_downsample * 3 < scan_w && + current_downsample * 3 < scan_h) + { + scan_w = current_downsample * 3; + scan_h = current_downsample * 3; + } + + if(angle_step * 1.5 < rotation_range) + { + rotation_range = angle_step * 1.5; + } +//printf("MotionHVScan::scan_frame %d %f %f\n", __LINE__, angle_step, rotation_range); + + current_downsample /= 2; + +// convert back to pixels + x_result /= OVERSAMPLE; + y_result /= OVERSAMPLE; +// debug +//exit(1); + } + + } + } + + dx_result *= -1; + dy_result *= -1; + dr_result *= -1; + } +// printf("MotionHVScan::scan_frame %d dx=%f dy=%f dr=%f\n", +// __LINE__, +// (float)dx_result / OVERSAMPLE, +// (float)dy_result / OVERSAMPLE, +// dr_result); + + + + +// Write results + if(!skip && tracking_type == MotionHVScan::SAVE) + { + char string[BCTEXTLEN]; + + + if(do_motion) + { + sprintf(string, + "%s%06d", + MOTION_FILE, + source_position); + FILE *output = fopen(string, "w"); + if(output) + { + fprintf(output, + "%d %d\n", + dx_result, + dy_result); + fclose(output); + } + else + { + printf("MotionHVScan::scan_frame %d: save motion failed\n", __LINE__); + } + } + + if(do_rotate) + { + sprintf(string, + "%s%06d", + ROTATION_FILE, + source_position); + FILE *output = fopen(string, "w"); + if(output) + { + fprintf(output, "%f\n", dr_result); + fclose(output); + } + else + { + printf("MotionHVScan::scan_frame %d save rotation failed\n", __LINE__); + } + } + } + + + if(vertical_only) dx_result = 0; + if(horizontal_only) dy_result = 0; + +// printf("MotionHVScan::scan_frame %d dx=%d dy=%d\n", +// __LINE__, +// this->dx_result, +// this->dy_result); +} + + + + + + + + + + + + + + + + + + + +#define ABS_DIFF(type, temp_type, multiplier, components) \ +{ \ + temp_type result_temp = 0; \ + for(int i = 0; i < h; i++) \ + { \ + type *prev_row = (type*)prev_ptr; \ + type *current_row = (type*)current_ptr; \ + for(int j = 0; j < w; j++) \ + { \ + for(int k = 0; k < 3; k++) \ + { \ + temp_type difference; \ + difference = *prev_row++ - *current_row++; \ + difference *= difference; \ + result_temp += difference; \ + } \ + if(components == 4) \ + { \ + prev_row++; \ + current_row++; \ + } \ + } \ + prev_ptr += row_bytes; \ + current_ptr += row_bytes; \ + } \ + result = (int64_t)(result_temp * multiplier); \ +} + +int64_t MotionHVScan::abs_diff(unsigned char *prev_ptr, + unsigned char *current_ptr, + int row_bytes, + int w, + int h, + int color_model) +{ + int64_t result = 0; + switch(color_model) + { + case BC_RGB888: + ABS_DIFF(unsigned char, int64_t, 1, 3) + break; + case BC_RGBA8888: + ABS_DIFF(unsigned char, int64_t, 1, 4) + break; + case BC_RGB_FLOAT: + ABS_DIFF(float, double, 0x10000, 3) + break; + case BC_RGBA_FLOAT: + ABS_DIFF(float, double, 0x10000, 4) + break; + case BC_YUV888: + ABS_DIFF(unsigned char, int64_t, 1, 3) + break; + case BC_YUVA8888: + ABS_DIFF(unsigned char, int64_t, 1, 4) + break; + } + return result; +} + + + +#define ABS_DIFF_SUB(type, temp_type, multiplier, components) \ +{ \ + temp_type result_temp = 0; \ + temp_type y2_fraction = sub_y * 0x100 / OVERSAMPLE; \ + temp_type y1_fraction = 0x100 - y2_fraction; \ + temp_type x2_fraction = sub_x * 0x100 / OVERSAMPLE; \ + temp_type x1_fraction = 0x100 - x2_fraction; \ + for(int i = 0; i < h_sub; i++) \ + { \ + type *prev_row1 = (type*)prev_ptr; \ + type *prev_row2 = (type*)prev_ptr + components; \ + type *prev_row3 = (type*)(prev_ptr + row_bytes); \ + type *prev_row4 = (type*)(prev_ptr + row_bytes) + components; \ + type *current_row = (type*)current_ptr; \ + for(int j = 0; j < w_sub; j++) \ + { \ +/* Scan each component */ \ + for(int k = 0; k < 3; k++) \ + { \ + temp_type difference; \ + temp_type prev_value = \ + (*prev_row1++ * x1_fraction * y1_fraction + \ + *prev_row2++ * x2_fraction * y1_fraction + \ + *prev_row3++ * x1_fraction * y2_fraction + \ + *prev_row4++ * x2_fraction * y2_fraction) / \ + 0x100 / 0x100; \ + temp_type current_value = *current_row++; \ + difference = prev_value - current_value; \ + difference *= difference; \ + result_temp += difference; \ + } \ + \ +/* skip alpha */ \ + if(components == 4) \ + { \ + prev_row1++; \ + prev_row2++; \ + prev_row3++; \ + prev_row4++; \ + current_row++; \ + } \ + } \ + prev_ptr += row_bytes; \ + current_ptr += row_bytes; \ + } \ + result = (int64_t)(result_temp * multiplier); \ +} + + + + +int64_t MotionHVScan::abs_diff_sub(unsigned char *prev_ptr, + unsigned char *current_ptr, + int row_bytes, + int w, + int h, + int color_model, + int sub_x, + int sub_y) +{ + int h_sub = h - 1; + int w_sub = w - 1; + int64_t result = 0; + + switch(color_model) + { + case BC_RGB888: + ABS_DIFF_SUB(unsigned char, int64_t, 1, 3) + break; + case BC_RGBA8888: + ABS_DIFF_SUB(unsigned char, int64_t, 1, 4) + break; + case BC_RGB_FLOAT: + ABS_DIFF_SUB(float, double, 0x10000, 3) + break; + case BC_RGBA_FLOAT: + ABS_DIFF_SUB(float, double, 0x10000, 4) + break; + case BC_YUV888: + ABS_DIFF_SUB(unsigned char, int64_t, 1, 3) + break; + case BC_YUVA8888: + ABS_DIFF_SUB(unsigned char, int64_t, 1, 4) + break; + } + return result; +} + + +#if 0 +#define VARIANCE(type, temp_type, multiplier, components) \ +{ \ + temp_type average[3] = { 0 }; \ + temp_type variance[3] = { 0 }; \ + \ + for(int i = 0; i < h; i++) \ + { \ + type *row = (type*)current_ptr + i * row_bytes; \ + for(int j = 0; j < w; j++) \ + { \ + for(int k = 0; k < 3; k++) \ + { \ + average[k] += row[k]; \ + } \ + row += components; \ + } \ + } \ + for(int k = 0; k < 3; k++) \ + { \ + average[k] /= w * h; \ + } \ + \ + for(int i = 0; i < h; i++) \ + { \ + type *row = (type*)current_ptr + i * row_bytes; \ + for(int j = 0; j < w; j++) \ + { \ + for(int k = 0; k < 3; k++) \ + { \ + variance[k] += SQR(row[k] - average[k]); \ + } \ + row += components; \ + } \ + } \ + result = (double)multiplier * \ + sqrt((variance[0] + variance[1] + variance[2]) / w / h / 3); \ +} + +double MotionHVScan::calculate_variance(unsigned char *current_ptr, + int row_bytes, + int w, + int h, + int color_model) +{ + double result = 0; + + switch(color_model) + { + case BC_RGB888: + VARIANCE(unsigned char, int, 1, 3) + break; + case BC_RGBA8888: + VARIANCE(unsigned char, int, 1, 4) + break; + case BC_RGB_FLOAT: + VARIANCE(float, double, 255, 3) + break; + case BC_RGBA_FLOAT: + VARIANCE(float, double, 255, 4) + break; + case BC_YUV888: + VARIANCE(unsigned char, int, 1, 3) + break; + case BC_YUVA8888: + VARIANCE(unsigned char, int, 1, 4) + break; + } + + + return result; +} +#endif // 0 + + + + +#define RANGE(type, temp_type, multiplier, components) \ +{ \ + temp_type min[3]; \ + temp_type max[3]; \ + min[0] = 0x7fff; \ + min[1] = 0x7fff; \ + min[2] = 0x7fff; \ + max[0] = 0; \ + max[1] = 0; \ + max[2] = 0; \ + \ + for(int i = 0; i < h; i++) \ + { \ + type *row = (type*)current_ptr + i * row_bytes; \ + for(int j = 0; j < w; j++) \ + { \ + for(int k = 0; k < 3; k++) \ + { \ + if(row[k] > max[k]) max[k] = row[k]; \ + if(row[k] < min[k]) min[k] = row[k]; \ + } \ + row += components; \ + } \ + } \ + \ + for(int k = 0; k < 3; k++) \ + { \ + /* printf("MotionHVScan::calculate_range %d k=%d max=%d min=%d\n", __LINE__, k, max[k], min[k]); */ \ + if(max[k] - min[k] > result) result = max[k] - min[k]; \ + } \ + \ +} + +double MotionHVScan::calculate_range(unsigned char *current_ptr, + int row_bytes, + int w, + int h, + int color_model) +{ + double result = 0; + + switch(color_model) + { + case BC_RGB888: + RANGE(unsigned char, int, 1, 3) + break; + case BC_RGBA8888: + RANGE(unsigned char, int, 1, 4) + break; + case BC_RGB_FLOAT: + RANGE(float, float, 255, 3) + break; + case BC_RGBA_FLOAT: + RANGE(float, float, 255, 4) + break; + case BC_YUV888: + RANGE(unsigned char, int, 1, 3) + break; + case BC_YUVA8888: + RANGE(unsigned char, int, 1, 4) + break; + } + + + return result; +} + + +//#define CLAMP_BLOCK + +// this truncates the scan area but not the macroblock unless the macro is defined +void MotionHVScan::clamp_scan(int w, + int h, + int *block_x1, + int *block_y1, + int *block_x2, + int *block_y2, + int *scan_x1, + int *scan_y1, + int *scan_x2, + int *scan_y2, + int use_absolute) +{ +// printf("MotionHVMain::clamp_scan 1 w=%d h=%d block=%d %d %d %d scan=%d %d %d %d absolute=%d\n", +// w, +// h, +// *block_x1, +// *block_y1, +// *block_x2, +// *block_y2, +// *scan_x1, +// *scan_y1, +// *scan_x2, +// *scan_y2, +// use_absolute); + + if(use_absolute) + { +// Limit size of scan area +// Used for drawing vectors +// scan is always out of range before block. + if(*scan_x1 < 0) + { +#ifdef CLAMP_BLOCK + int difference = -*scan_x1; + *block_x1 += difference; +#endif + *scan_x1 = 0; + } + + if(*scan_y1 < 0) + { +#ifdef CLAMP_BLOCK + int difference = -*scan_y1; + *block_y1 += difference; +#endif + *scan_y1 = 0; + } + + if(*scan_x2 > w) + { + int difference = *scan_x2 - w; +#ifdef CLAMP_BLOCK + *block_x2 -= difference; +#endif + *scan_x2 -= difference; + } + + if(*scan_y2 > h) + { + int difference = *scan_y2 - h; +#ifdef CLAMP_BLOCK + *block_y2 -= difference; +#endif + *scan_y2 -= difference; + } + + CLAMP(*scan_x1, 0, w); + CLAMP(*scan_y1, 0, h); + CLAMP(*scan_x2, 0, w); + CLAMP(*scan_y2, 0, h); + } + else + { +// Limit range of upper left block coordinates +// Used for motion tracking + if(*scan_x1 < 0) + { + int difference = -*scan_x1; +#ifdef CLAMP_BLOCK + *block_x1 += difference; +#endif + *scan_x2 += difference; + *scan_x1 = 0; + } + + if(*scan_y1 < 0) + { + int difference = -*scan_y1; +#ifdef CLAMP_BLOCK + *block_y1 += difference; +#endif + *scan_y2 += difference; + *scan_y1 = 0; + } + + if(*scan_x2 - *block_x1 + *block_x2 > w) + { + int difference = *scan_x2 - *block_x1 + *block_x2 - w; + *scan_x2 -= difference; +#ifdef CLAMP_BLOCK + *block_x2 -= difference; +#endif + } + + if(*scan_y2 - *block_y1 + *block_y2 > h) + { + int difference = *scan_y2 - *block_y1 + *block_y2 - h; + *scan_y2 -= difference; +#ifdef CLAMP_BLOCK + *block_y2 -= difference; +#endif + } + +// CLAMP(*scan_x1, 0, w - (*block_x2 - *block_x1)); +// CLAMP(*scan_y1, 0, h - (*block_y2 - *block_y1)); +// CLAMP(*scan_x2, 0, w - (*block_x2 - *block_x1)); +// CLAMP(*scan_y2, 0, h - (*block_y2 - *block_y1)); + } + +// Sanity checks which break the calculation but should never happen if the +// center of the block is inside the frame. + CLAMP(*block_x1, 0, w); + CLAMP(*block_x2, 0, w); + CLAMP(*block_y1, 0, h); + CLAMP(*block_y2, 0, h); + +// printf("MotionHVMain::clamp_scan 2 w=%d h=%d block=%d %d %d %d scan=%d %d %d %d absolute=%d\n", +// w, +// h, +// *block_x1, +// *block_y1, +// *block_x2, +// *block_y2, +// *scan_x1, +// *scan_y1, +// *scan_x2, +// *scan_y2, +// use_absolute); +} + + +