cinelerra-5.1/plugins/motion/motionscan.C

   1
   2 /*
   3  * CINELERRA
   4  * Copyright (C) 2016 Adam Williams <broadcast at earthling dot net>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  19  *
  20  */
  21
  22 #include "affine.h"
  23 #include "bcsignals.h"
  24 #include "clip.h"
  25 #include "motionscan.h"
  26 #include "mutex.h"
  27 #include "vframe.h"
  28
  29
  30 #include <math.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33
  34 // The module which does the actual scanning
  35
  36 // starting level of detail
  37 #define STARTING_DOWNSAMPLE 16
  38 // minimum size in each level of detail
  39 #define MIN_DOWNSAMPLED_SIZE 16
  40 // minimum scan range
  41 #define MIN_DOWNSAMPLED_SCAN 4
  42 // scan range for subpixel mode
  43 #define SUBPIXEL_RANGE 4
  44
  45 MotionScanPackage::MotionScanPackage()
  46  : LoadPackage()
  47 {
  48         valid = 1;
  49 }
  50
  51
  52
  53
  54
  55
  56 MotionScanUnit::MotionScanUnit(MotionScan *server)
  57  : LoadClient(server)
  58 {
  59         this->server = server;
  60 }
  61
  62 MotionScanUnit::~MotionScanUnit()
  63 {
  64 }
  65
  66
  67 void MotionScanUnit::single_pixel(MotionScanPackage *pkg)
  68 {
  69         //int w = server->current_frame->get_w();
  70         //int h = server->current_frame->get_h();
  71         int color_model = server->current_frame->get_color_model();
  72         int pixel_size = BC_CModels::calculate_pixelsize(color_model);
  73         int row_bytes = server->current_frame->get_bytes_per_line();
  74
  75 // printf("MotionScanUnit::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",
  76 // __LINE__,
  77 // pkg->search_x,
  78 // pkg->search_y,
  79 // pkg->scan_x1,
  80 // pkg->scan_y1,
  81 // pkg->scan_x2,
  82 // pkg->scan_y2,
  83 // server->x_steps,
  84 // server->y_steps);
  85
  86 // Pointers to first pixel in each block
  87         unsigned char *prev_ptr = server->previous_frame->get_rows()[
  88                 pkg->search_y] +
  89                 pkg->search_x * pixel_size;
  90         unsigned char *current_ptr = 0;
  91
  92         if(server->do_rotate)
  93         {
  94                 current_ptr = server->rotated_current[pkg->angle_step]->get_rows()[
  95                         pkg->block_y1] +
  96                         pkg->block_x1 * pixel_size;
  97         }
  98         else
  99         {
 100                 current_ptr = server->current_frame->get_rows()[
 101                         pkg->block_y1] +
 102                         pkg->block_x1 * pixel_size;
 103         }
 104
 105 // Scan block
 106         pkg->difference1 = MotionScan::abs_diff(prev_ptr,
 107                 current_ptr,
 108                 row_bytes,
 109                 pkg->block_x2 - pkg->block_x1,
 110                 pkg->block_y2 - pkg->block_y1,
 111                 color_model);
 112
 113 // printf("MotionScanUnit::process_package %d angle_step=%d diff=%d\n",
 114 // __LINE__,
 115 // pkg->angle_step,
 116 // pkg->difference1);
 117 // printf("MotionScanUnit::process_package %d search_x=%d search_y=%d diff=%lld\n",
 118 // __LINE__, server->block_x1 - pkg->search_x, server->block_y1 - pkg->search_y, pkg->difference1);
 119 }
 120
 121 void MotionScanUnit::subpixel(MotionScanPackage *pkg)
 122 {
 123 //PRINT_TRACE
 124         //int w = server->current_frame->get_w();
 125         //int h = server->current_frame->get_h();
 126         int color_model = server->current_frame->get_color_model();
 127         int pixel_size = BC_CModels::calculate_pixelsize(color_model);
 128         int row_bytes = server->current_frame->get_bytes_per_line();
 129         unsigned char *prev_ptr = server->previous_frame->get_rows()[
 130                 pkg->search_y] +
 131                 pkg->search_x * pixel_size;
 132 // neglect rotation
 133         unsigned char *current_ptr = server->current_frame->get_rows()[
 134                 pkg->block_y1] +
 135                 pkg->block_x1 * pixel_size;
 136
 137 // With subpixel, there are two ways to compare each position, one by shifting
 138 // the previous frame and two by shifting the current frame.
 139         pkg->difference1 = MotionScan::abs_diff_sub(prev_ptr,
 140                 current_ptr,
 141                 row_bytes,
 142                 pkg->block_x2 - pkg->block_x1,
 143                 pkg->block_y2 - pkg->block_y1,
 144                 color_model,
 145                 pkg->sub_x,
 146                 pkg->sub_y);
 147         pkg->difference2 = MotionScan::abs_diff_sub(current_ptr,
 148                 prev_ptr,
 149                 row_bytes,
 150                 pkg->block_x2 - pkg->block_x1,
 151                 pkg->block_y2 - pkg->block_y1,
 152                 color_model,
 153                 pkg->sub_x,
 154                 pkg->sub_y);
 155 // printf("MotionScanUnit::process_package sub_x=%d sub_y=%d search_x=%d search_y=%d diff1=%lld diff2=%lld\n",
 156 // pkg->sub_x,
 157 // pkg->sub_y,
 158 // pkg->search_x,
 159 // pkg->search_y,
 160 // pkg->difference1,
 161 // pkg->difference2);
 162 }
 163
 164 void MotionScanUnit::process_package(LoadPackage *package)
 165 {
 166         MotionScanPackage *pkg = (MotionScanPackage*)package;
 167
 168
 169 // Single pixel
 170         if(!server->subpixel)
 171         {
 172                 single_pixel(pkg);
 173         }
 174         else
 175 // Sub pixel
 176         {
 177                 subpixel(pkg);
 178         }
 179
 180
 181
 182
 183 }
 184
 185
 186
 187
 188
 189
 190
 191
 192
 193
 194
 195
 196
 197
 198
 199
 200
 201
 202 MotionScan::MotionScan(int total_clients,
 203         int total_packages)
 204  : LoadServer(
 205 // DEBUG
 206 //1, 1
 207 total_clients, total_packages
 208 )
 209 {
 210         test_match = 1;
 211         downsampled_previous = 0;
 212         downsampled_current = 0;
 213         rotated_current = 0;
 214         rotater = 0;
 215 }
 216
 217 MotionScan::~MotionScan()
 218 {
 219         delete downsampled_previous;
 220         delete downsampled_current;
 221         if(rotated_current)
 222         {
 223                 for(int i = 0; i < total_rotated; i++)
 224                 {
 225                         delete rotated_current[i];
 226                 }
 227
 228                 delete [] rotated_current;
 229         }
 230         delete rotater;
 231 }
 232
 233
 234 void MotionScan::init_packages()
 235 {
 236 // Set package coords
 237 // Total range of positions to scan with downsampling
 238         int downsampled_scan_x1 = scan_x1 / current_downsample;
 239         //int downsampled_scan_x2 = scan_x2 / current_downsample;
 240         int downsampled_scan_y1 = scan_y1 / current_downsample;
 241         //int downsampled_scan_y2 = scan_y2 / current_downsample;
 242         int downsampled_block_x1 = block_x1 / current_downsample;
 243         int downsampled_block_x2 = block_x2 / current_downsample;
 244         int downsampled_block_y1 = block_y1 / current_downsample;
 245         int downsampled_block_y2 = block_y2 / current_downsample;
 246
 247
 248 //printf("MotionScan::init_packages %d %d\n", __LINE__, get_total_packages());
 249 // printf("MotionScan::init_packages %d current_downsample=%d scan_x1=%d scan_x2=%d block_x1=%d block_x2=%d\n",
 250 // __LINE__,
 251 // current_downsample,
 252 // downsampled_scan_x1,
 253 // downsampled_scan_x2,
 254 // downsampled_block_x1,
 255 // downsampled_block_x2);
 256 // if(current_downsample == 8 && downsampled_scan_x1 == 47)
 257 // {
 258 // downsampled_previous->write_png("/tmp/previous");
 259 // downsampled_current->write_png("/tmp/current");
 260 // }
 261
 262         for(int i = 0; i < get_total_packages(); i++)
 263         {
 264                 MotionScanPackage *pkg = (MotionScanPackage*)get_package(i);
 265
 266                 pkg->block_x1 = downsampled_block_x1;
 267                 pkg->block_x2 = downsampled_block_x2;
 268                 pkg->block_y1 = downsampled_block_y1;
 269                 pkg->block_y2 = downsampled_block_y2;
 270                 pkg->difference1 = 0;
 271                 pkg->difference2 = 0;
 272                 pkg->dx = 0;
 273                 pkg->dy = 0;
 274                 pkg->valid = 1;
 275                 pkg->angle_step = 0;
 276
 277                 if(!subpixel)
 278                 {
 279                         if(rotation_pass)
 280                         {
 281                                 pkg->search_x = scan_x1 / current_downsample;
 282                                 pkg->search_y = scan_y1 / current_downsample;
 283                                 pkg->angle_step = i;
 284                         }
 285                         else
 286                         {
 287
 288                                 int current_x_step = (i % x_steps);
 289                                 int current_y_step = (i / x_steps);
 290
 291         //printf("MotionScan::init_packages %d i=%d x_step=%d y_step=%d angle_step=%d\n",
 292         //__LINE__, i, current_x_step, current_y_step, current_angle_step);
 293                                 pkg->search_x = downsampled_scan_x1 + current_x_step *
 294                                         (scan_x2 - scan_x1) / current_downsample / x_steps;
 295                                 pkg->search_y = downsampled_scan_y1 + current_y_step *
 296                                         (scan_y2 - scan_y1) / current_downsample / y_steps;
 297
 298                                 if(do_rotate)
 299                                 {
 300                                         pkg->angle_step = angle_steps / 2;
 301                                 }
 302                                 else
 303                                 {
 304                                         pkg->angle_step = 0;
 305                                 }
 306                         }
 307
 308                         pkg->sub_x = 0;
 309                         pkg->sub_y = 0;
 310                 }
 311                 else
 312                 {
 313                         pkg->sub_x = i % (OVERSAMPLE * SUBPIXEL_RANGE);
 314                         pkg->sub_y = i / (OVERSAMPLE * SUBPIXEL_RANGE);
 315
 316 //                      if(horizontal_only)
 317 //                      {
 318 //                              pkg->sub_y = 0;
 319 //                      }
 320 //
 321 //                      if(vertical_only)
 322 //                      {
 323 //                              pkg->sub_x = 0;
 324 //                      }
 325
 326                         pkg->search_x = scan_x1 + pkg->sub_x / OVERSAMPLE + 1;
 327                         pkg->search_y = scan_y1 + pkg->sub_y / OVERSAMPLE + 1;
 328                         pkg->sub_x %= OVERSAMPLE;
 329                         pkg->sub_y %= OVERSAMPLE;
 330
 331
 332
 333 // printf("MotionScan::init_packages %d i=%d search_x=%d search_y=%d sub_x=%d sub_y=%d\n",
 334 // __LINE__,
 335 // i,
 336 // pkg->search_x,
 337 // pkg->search_y,
 338 // pkg->sub_x,
 339 // pkg->sub_y);
 340                 }
 341
 342 // printf("MotionScan::init_packages %d %d,%d %d,%d %d,%d\n",
 343 // __LINE__,
 344 // scan_x1,
 345 // scan_x2,
 346 // scan_y1,
 347 // scan_y2,
 348 // pkg->search_x,
 349 // pkg->search_y);
 350         }
 351 }
 352
 353 LoadClient* MotionScan::new_client()
 354 {
 355         return new MotionScanUnit(this);
 356 }
 357
 358 LoadPackage* MotionScan::new_package()
 359 {
 360         return new MotionScanPackage;
 361 }
 362
 363
 364 void MotionScan::set_test_match(int value)
 365 {
 366         this->test_match = value;
 367 }
 368
 369
 370
 371
 372 #define DOWNSAMPLE(type, temp_type, components, max) \
 373 { \
 374         temp_type r; \
 375         temp_type g; \
 376         temp_type b; \
 377         temp_type a; \
 378         type **in_rows = (type**)src->get_rows(); \
 379         type **out_rows = (type**)dst->get_rows(); \
 380  \
 381         for(int i = 0; i < h; i += downsample) \
 382         { \
 383                 int y1 = MAX(i, 0); \
 384                 int y2 = MIN(i + downsample, h); \
 385  \
 386  \
 387                 for(int j = 0; \
 388                         j < w; \
 389                         j += downsample) \
 390                 { \
 391                         int x1 = MAX(j, 0); \
 392                         int x2 = MIN(j + downsample, w); \
 393  \
 394                         temp_type scale = (x2 - x1) * (y2 - y1); \
 395                         if(x2 > x1 && y2 > y1) \
 396                         { \
 397  \
 398 /* Read in values */ \
 399                                 r = 0; \
 400                                 g = 0; \
 401                                 b = 0; \
 402                                 if(components == 4) a = 0; \
 403  \
 404                                 for(int k = y1; k < y2; k++) \
 405                                 { \
 406                                         type *row = in_rows[k] + x1 * components; \
 407                                         for(int l = x1; l < x2; l++) \
 408                                         { \
 409                                                 r += *row++; \
 410                                                 g += *row++; \
 411                                                 b += *row++; \
 412                                                 if(components == 4) a += *row++; \
 413                                         } \
 414                                 } \
 415  \
 416 /* Write average */ \
 417                                 r /= scale; \
 418                                 g /= scale; \
 419                                 b /= scale; \
 420                                 if(components == 4) a /= scale; \
 421  \
 422                                 type *row = out_rows[y1 / downsample] + \
 423                                         x1 / downsample * components; \
 424                                 *row++ = r; \
 425                                 *row++ = g; \
 426                                 *row++ = b; \
 427                                 if(components == 4) *row++ = a; \
 428                         } \
 429                 } \
 430 /*printf("DOWNSAMPLE 3 %d\n", i);*/ \
 431         } \
 432 }
 433
 434
 435
 436
 437 void MotionScan::downsample_frame(VFrame *dst,
 438         VFrame *src,
 439         int downsample)
 440 {
 441         int h = src->get_h();
 442         int w = src->get_w();
 443
 444 //PRINT_TRACE
 445 //printf("downsample=%d w=%d h=%d dst=%d %d\n", downsample, w, h, dst->get_w(), dst->get_h());
 446         switch(src->get_color_model())
 447         {
 448                 case BC_RGB888:
 449                         DOWNSAMPLE(uint8_t, int64_t, 3, 0xff)
 450                         break;
 451                 case BC_RGB_FLOAT:
 452                         DOWNSAMPLE(float, float, 3, 1.0)
 453                         break;
 454                 case BC_RGBA8888:
 455                         DOWNSAMPLE(uint8_t, int64_t, 4, 0xff)
 456                         break;
 457                 case BC_RGBA_FLOAT:
 458                         DOWNSAMPLE(float, float, 4, 1.0)
 459                         break;
 460                 case BC_YUV888:
 461                         DOWNSAMPLE(uint8_t, int64_t, 3, 0xff)
 462                         break;
 463                 case BC_YUVA8888:
 464                         DOWNSAMPLE(uint8_t, int64_t, 4, 0xff)
 465                         break;
 466         }
 467 //PRINT_TRACE
 468 }
 469
 470 double MotionScan::step_to_angle(int step, double center)
 471 {
 472         if(step < angle_steps / 2)
 473         {
 474                 return center - angle_step * (angle_steps / 2 - step);
 475         }
 476         else
 477         if(step > angle_steps / 2)
 478         {
 479                 return center + angle_step * (step - angle_steps / 2);
 480         }
 481         else
 482         {
 483                 return center;
 484         }
 485 }
 486
 487 #ifdef STDDEV_TEST
 488 static int compare(const void *p1, const void *p2)
 489 {
 490         double value1 = *(double*)p1;
 491         double value2 = *(double*)p2;
 492
 493 //printf("compare %d value1=%f value2=%f\n", __LINE__, value1, value2);
 494         return value1 > value2;
 495 }
 496 #endif
 497
 498 // reject vectors based on content.  It's the reason Goog can't stabilize timelapses.
 499 //#define STDDEV_TEST
 500
 501 // pixel accurate motion search
 502 void MotionScan::pixel_search(int &x_result, int &y_result, double &r_result)
 503 {
 504 // reduce level of detail until enough steps
 505         while(current_downsample > 1 &&
 506                 ((block_x2 - block_x1) / current_downsample < MIN_DOWNSAMPLED_SIZE ||
 507                 (block_y2 - block_y1) / current_downsample < MIN_DOWNSAMPLED_SIZE
 508                 ||
 509                  (scan_x2 - scan_x1) / current_downsample < MIN_DOWNSAMPLED_SCAN ||
 510                 (scan_y2 - scan_y1) / current_downsample < MIN_DOWNSAMPLED_SCAN
 511                 ))
 512         {
 513                 current_downsample /= 2;
 514         }
 515
 516
 517
 518 // create downsampled images.
 519 // Need to keep entire frame to search for rotation.
 520         int downsampled_prev_w = previous_frame_arg->get_w() / current_downsample;
 521         int downsampled_prev_h = previous_frame_arg->get_h() / current_downsample;
 522         int downsampled_current_w = current_frame_arg->get_w() / current_downsample;
 523         int downsampled_current_h = current_frame_arg->get_h() / current_downsample;
 524
 525 // printf("MotionScan::pixel_search %d current_downsample=%d current_frame_arg->get_w()=%d downsampled_current_w=%d\n",
 526 // __LINE__,
 527 // current_downsample,
 528 // current_frame_arg->get_w(),
 529 // downsampled_current_w);
 530
 531         x_steps = (scan_x2 - scan_x1) / current_downsample;
 532         y_steps = (scan_y2 - scan_y1) / current_downsample;
 533
 534 // in rads
 535         double test_angle1 = atan2((double)downsampled_current_h / 2 - 1, (double)downsampled_current_w / 2);
 536         double test_angle2 = atan2((double)downsampled_current_h / 2, (double)downsampled_current_w / 2 - 1);
 537
 538 // in deg
 539         angle_step = 360.0f * fabs(test_angle1 - test_angle2) / 2 / M_PI;
 540
 541 // printf("MotionScan::pixel_search %d test_angle1=%f test_angle2=%f angle_step=%f\n",
 542 // __LINE__,
 543 // 360.0f * test_angle1 / 2 / M_PI,
 544 // 360.0f * test_angle2 / 2 / M_PI,
 545 // angle_step);
 546
 547
 548         if(do_rotate && angle_step < rotation_range)
 549         {
 550                 angle_steps = 1 + (int)((scan_angle2 - scan_angle1) / angle_step + 0.5);
 551         }
 552         else
 553         {
 554                 angle_steps = 1;
 555         }
 556
 557
 558         if(current_downsample > 1)
 559         {
 560                 if(!downsampled_previous ||
 561                         downsampled_previous->get_w() != downsampled_prev_w ||
 562                         downsampled_previous->get_h() != downsampled_prev_h)
 563                 {
 564                         delete downsampled_previous;
 565                         downsampled_previous = new VFrame();
 566                         downsampled_previous->set_use_shm(0);
 567                         downsampled_previous->reallocate(0,
 568                                 -1,
 569                                 0,
 570                                 0,
 571                                 0,
 572                                 downsampled_prev_w + 1,
 573                                 downsampled_prev_h + 1,
 574                                 previous_frame_arg->get_color_model(),
 575                                 -1);
 576                 }
 577
 578                 if(!downsampled_current ||
 579                         downsampled_current->get_w() != downsampled_current_w ||
 580                         downsampled_current->get_h() != downsampled_current_h)
 581                 {
 582                         delete downsampled_current;
 583                         downsampled_current = new VFrame();
 584                         downsampled_current->set_use_shm(0);
 585                         downsampled_current->reallocate(0,
 586                                 -1,
 587                                 0,
 588                                 0,
 589                                 0,
 590                                 downsampled_current_w + 1,
 591                                 downsampled_current_h + 1,
 592                                 current_frame_arg->get_color_model(),
 593                                 -1);
 594                 }
 595
 596
 597                 downsample_frame(downsampled_previous,
 598                         previous_frame_arg,
 599                         current_downsample);
 600                 downsample_frame(downsampled_current,
 601                         current_frame_arg,
 602                         current_downsample);
 603                 previous_frame = downsampled_previous;
 604                 current_frame = downsampled_current;
 605
 606         }
 607         else
 608         {
 609                 previous_frame = previous_frame_arg;
 610                 current_frame = current_frame_arg;
 611         }
 612
 613
 614
 615 // printf("MotionScan::pixel_search %d x_steps=%d y_steps=%d angle_steps=%d total_steps=%d\n",
 616 // __LINE__,
 617 // x_steps,
 618 // y_steps,
 619 // angle_steps,
 620 // total_steps);
 621
 622
 623
 624 // test variance of constant macroblock
 625         int color_model = current_frame->get_color_model();
 626         int pixel_size = BC_CModels::calculate_pixelsize(color_model);
 627         int row_bytes = current_frame->get_bytes_per_line();
 628         int block_w = block_x2 - block_x1;
 629         int block_h = block_y2 - block_y1;
 630
 631         unsigned char *current_ptr =
 632                 current_frame->get_rows()[block_y1 / current_downsample] +
 633                 (block_x1 / current_downsample) * pixel_size;
 634         unsigned char *previous_ptr =
 635                 previous_frame->get_rows()[scan_y1 / current_downsample] +
 636                 (scan_x1 / current_downsample) * pixel_size;
 637
 638
 639
 640 // test detail in prev & current frame
 641         double range1 = calculate_range(current_ptr,
 642                 row_bytes,
 643                 block_w / current_downsample,
 644                 block_h / current_downsample,
 645                 color_model);
 646
 647         if(range1 < 1)
 648         {
 649 printf("MotionScan::pixel_search %d range fail range1=%f\n", __LINE__, range1);
 650                 failed = 1;
 651                 return;
 652         }
 653
 654         double range2 = calculate_range(previous_ptr,
 655                 row_bytes,
 656                 block_w / current_downsample,
 657                 block_h / current_downsample,
 658                 color_model);
 659
 660         if(range2 < 1)
 661         {
 662 printf("MotionScan::pixel_search %d range fail range2=%f\n", __LINE__, range2);
 663                 failed = 1;
 664                 return;
 665         }
 666
 667
 668 // create rotated images
 669         if(rotated_current &&
 670                 (total_rotated != angle_steps ||
 671                 rotated_current[0]->get_w() != downsampled_current_w ||
 672                 rotated_current[0]->get_h() != downsampled_current_h))
 673         {
 674                 for(int i = 0; i < total_rotated; i++)
 675                 {
 676                         delete rotated_current[i];
 677                 }
 678
 679                 delete [] rotated_current;
 680                 rotated_current = 0;
 681                 total_rotated = 0;
 682         }
 683
 684         if(do_rotate)
 685         {
 686                 total_rotated = angle_steps;
 687
 688
 689                 if(!rotated_current)
 690                 {
 691                         rotated_current = new VFrame*[total_rotated];
 692                         bzero(rotated_current, sizeof(VFrame*) * total_rotated);
 693                 }
 694
 695 // printf("MotionScan::pixel_search %d total_rotated=%d w=%d h=%d block_w=%d block_h=%d\n",
 696 // __LINE__,
 697 // total_rotated,
 698 // downsampled_current_w,
 699 // downsampled_current_h,
 700 // (block_x2 - block_x1) / current_downsample,
 701 // (block_y2 - block_y1) / current_downsample);
 702                 for(int i = 0; i < angle_steps; i++)
 703                 {
 704
 705 // printf("MotionScan::pixel_search %d w=%d h=%d x=%d y=%d angle=%f\n",
 706 // __LINE__,
 707 // downsampled_current_w,
 708 // downsampled_current_h,
 709 // (block_x1 + block_x2) / 2 / current_downsample,
 710 // (block_y1 + block_y2) / 2 / current_downsample,
 711 // step_to_angle(i, r_result));
 712
 713 // printf("MotionScan::pixel_search %d i=%d rotated_current[i]=%p\n",
 714 // __LINE__,
 715 // i,
 716 // rotated_current[i]);
 717                         if(!rotated_current[i])
 718                         {
 719                                 rotated_current[i] = new VFrame();
 720                                 rotated_current[i]->set_use_shm(0);
 721                                 rotated_current[i]->reallocate(0,
 722                                         -1,
 723                                         0,
 724                                         0,
 725                                         0,
 726                                         downsampled_current_w + 1,
 727                                         downsampled_current_h + 1,
 728                                         current_frame_arg->get_color_model(),
 729                                         -1);
 730 //printf("MotionScan::pixel_search %d\n", __LINE__);
 731                         }
 732
 733
 734                         if(!rotater)
 735                         {
 736                                 rotater = new AffineEngine(get_total_clients(),
 737                                         get_total_clients());
 738                         }
 739
 740 // get smallest viewport size required for the angle
 741                         double diag = hypot((block_x2 - block_x1) / current_downsample,
 742                                 (block_y2 - block_y1) / current_downsample);
 743                         double angle1 = atan2(block_y2 - block_y1, block_x2 - block_x1) +
 744                                 TO_RAD(step_to_angle(i, r_result));
 745                         double angle2 = -atan2(block_y2 - block_y1, block_x2 - block_x1) +
 746                                 TO_RAD(step_to_angle(i, r_result));
 747                         double max_horiz = MAX(abs(diag * cos(angle1)), abs(diag * cos(angle2)));
 748                         double max_vert = MAX(abs(diag * sin(angle1)), abs(diag * sin(angle2)));
 749                         int center_x = (block_x1 + block_x2) / 2 / current_downsample;
 750                         int center_y = (block_y1 + block_y2) / 2 / current_downsample;
 751                         int x1 = center_x - max_horiz / 2;
 752                         int y1 = center_y - max_vert / 2;
 753                         int x2 = x1 + max_horiz;
 754                         int y2 = y1 + max_vert;
 755                         CLAMP(x1, 0, downsampled_current_w - 1);
 756                         CLAMP(y1, 0, downsampled_current_h - 1);
 757                         CLAMP(x2, 0, downsampled_current_w - 1);
 758                         CLAMP(y2, 0, downsampled_current_h - 1);
 759
 760 //printf("MotionScan::pixel_search %d %f %f %d %d\n",
 761 //__LINE__, TO_DEG(angle1), TO_DEG(angle2), (int)max_horiz, (int)max_vert);
 762                         rotater->set_in_viewport(x1,
 763                                 y1,
 764                                 x2 - x1,
 765                                 y2 - y1);
 766                         rotater->set_out_viewport(x1,
 767                                 y1,
 768                                 x2 - x1,
 769                                 y2 - y1);
 770
 771 //                      rotater->set_in_viewport(0,
 772 //                              0,
 773 //                              downsampled_current_w,
 774 //                              downsampled_current_h);
 775 //                      rotater->set_out_viewport(0,
 776 //                              0,
 777 //                              downsampled_current_w,
 778 //                              downsampled_current_h);
 779
 780                         rotater->set_in_pivot(center_x, center_y);
 781                         rotater->set_out_pivot(center_x, center_y);
 782
 783                         rotater->rotate(rotated_current[i],
 784                                 current_frame,
 785                                 step_to_angle(i, r_result));
 786
 787 // rotated_current[i]->draw_rect(block_x1 / current_downsample,
 788 // block_y1 / current_downsample,
 789 // block_x2 / current_downsample,
 790 // block_y2 / current_downsample);
 791 // char string[BCTEXTLEN];
 792 // sprintf(string, "/tmp/rotated%d", i);
 793 // rotated_current[i]->write_png(string);
 794 //downsampled_previous->write_png("/tmp/previous");
 795 //printf("MotionScan::pixel_search %d\n", __LINE__);
 796                 }
 797         }
 798
 799
 800
 801
 802
 803
 804 // printf("MotionScan::pixel_search %d block x=%d y=%d w=%d h=%d\n",
 805 // __LINE__,
 806 // block_x1 / current_downsample,
 807 // block_y1 / current_downsample,
 808 // block_w / current_downsample,
 809 // block_h / current_downsample);
 810
 811
 812
 813
 814
 815
 816
 817 //exit(1);
 818 // Test only translation of the middle rotated frame
 819         rotation_pass = 0;
 820         total_steps = x_steps * y_steps;
 821         set_package_count(total_steps);
 822         process_packages();
 823
 824
 825
 826
 827
 828
 829 // Get least difference
 830         int64_t min_difference = -1;
 831 #ifdef STDDEV_TEST
 832         double stddev_table[get_total_packages()];
 833 #endif
 834         for(int i = 0; i < get_total_packages(); i++)
 835         {
 836                 MotionScanPackage *pkg = (MotionScanPackage*)get_package(i);
 837
 838 #ifdef STDDEV_TEST
 839                 double stddev = sqrt(pkg->difference1) /
 840                         (block_w / current_downsample) /
 841                         (block_h / current_downsample) /
 842                         3;
 843 // printf("MotionScan::pixel_search %d current_downsample=%d search_x=%d search_y=%d diff1=%f\n",
 844 // __LINE__,
 845 // current_downsample,
 846 // pkg->search_x,
 847 // pkg->search_y,
 848 // sqrt(pkg->difference1) / block_w / current_downsample / block_h / 3 /* / variance */);
 849
 850 // printf("MotionScan::pixel_search %d range1=%f stddev=%f\n",
 851 // __LINE__,
 852 // range1,
 853 // stddev);
 854
 855                 stddev_table[i] = stddev;
 856 #endif // STDDEV_TEST
 857
 858                 if(pkg->difference1 < min_difference || i == 0)
 859                 {
 860                         min_difference = pkg->difference1;
 861                         x_result = pkg->search_x * current_downsample * OVERSAMPLE;
 862                         y_result = pkg->search_y * current_downsample * OVERSAMPLE;
 863
 864 // printf("MotionScan::pixel_search %d x_result=%d y_result=%d angle_step=%d diff=%lld\n",
 865 // __LINE__,
 866 // block_x1 * OVERSAMPLE - x_result,
 867 // block_y1 * OVERSAMPLE - y_result,
 868 // pkg->angle_step,
 869 // pkg->difference1);
 870
 871                 }
 872         }
 873
 874
 875 #ifdef STDDEV_TEST
 876         qsort(stddev_table, get_total_packages(), sizeof(double), compare);
 877
 878
 879 // reject motion vector if not similar enough
 880 //      if(stddev_table[0] > 0.2)
 881 //      {
 882 // if(debug)
 883 // {
 884 // printf("MotionScan::pixel_search %d stddev fail min_stddev=%f\n",
 885 // __LINE__,
 886 // stddev_table[0]);
 887 // }
 888 //              failed = 1;
 889 //              return;
 890 //      }
 891
 892 if(debug)
 893 {
 894         printf("MotionScan::pixel_search %d\n", __LINE__);
 895         for(int i = 0; i < get_total_packages(); i++)
 896         {
 897                 printf("%f\n", stddev_table[i]);
 898         }
 899 }
 900
 901 // reject motion vector if not a sigmoid curve
 902 // TODO: use linear interpolation
 903         int steps = 2;
 904         int step = get_total_packages() / steps;
 905         double curve[steps];
 906         for(int i = 0; i < steps; i++)
 907         {
 908                 int start = get_total_packages() * i / steps;
 909                 int end = get_total_packages() * (i + 1) / steps;
 910                 end = MIN(end, get_total_packages() - 1);
 911                 curve[i] = stddev_table[end] - stddev_table[start];
 912         }
 913
 914
 915 //      if(curve[0] < (curve[1] * 1.01) ||
 916 //              curve[2] < (curve[1] * 1.01) ||
 917 //              curve[0] < (curve[2] * 0.75))
 918 //      if(curve[0] < curve[1])
 919 //      {
 920 // if(debug)
 921 // {
 922 // printf("MotionScan::pixel_search %d curve fail %f %f\n",
 923 // __LINE__,
 924 // curve[0],
 925 // curve[1]);
 926 // }
 927 //              failed = 1;
 928 //              return;
 929 //      }
 930
 931 if(debug)
 932 {
 933 printf("MotionScan::pixel_search %d curve=%f %f ranges=%f %f min_stddev=%f\n",
 934 __LINE__,
 935 curve[0],
 936 curve[1],
 937 range1,
 938 range2,
 939 stddev_table[0]);
 940 }
 941 #endif // STDDEV_TEST
 942
 943
 944
 945
 946
 947         if(do_rotate)
 948         {
 949                 rotation_pass = 1;;
 950                 total_steps = angle_steps;
 951                 scan_x1 = x_result / OVERSAMPLE;
 952                 scan_y1 = y_result / OVERSAMPLE;
 953                 set_package_count(total_steps);
 954                 process_packages();
 955
 956
 957
 958                 min_difference = -1;
 959                 double prev_r_result = r_result;
 960                 for(int i = 0; i < get_total_packages(); i++)
 961                 {
 962                         MotionScanPackage *pkg = (MotionScanPackage*)get_package(i);
 963
 964 // printf("MotionScan::pixel_search %d search_x=%d search_y=%d angle_step=%d sub_x=%d sub_y=%d diff1=%lld diff2=%lld\n",
 965 // __LINE__,
 966 // pkg->search_x,
 967 // pkg->search_y,
 968 // pkg->search_angle_step,
 969 // pkg->sub_x,
 970 // pkg->sub_y,
 971 // pkg->difference1,
 972 // pkg->difference2);
 973                         if(pkg->difference1 < min_difference || i == 0)
 974                         {
 975                                 min_difference = pkg->difference1;
 976                                 r_result = step_to_angle(i, prev_r_result);
 977
 978         // printf("MotionScan::pixel_search %d x_result=%d y_result=%d angle_step=%d diff=%lld\n",
 979         // __LINE__,
 980         // block_x1 * OVERSAMPLE - x_result,
 981         // block_y1 * OVERSAMPLE - y_result,
 982         // pkg->angle_step,
 983         // pkg->difference1);
 984                         }
 985                 }
 986         }
 987
 988
 989 // printf("MotionScan::scan_frame %d current_downsample=%d x_result=%f y_result=%f r_result=%f\n",
 990 // __LINE__,
 991 // current_downsample,
 992 // (float)x_result / OVERSAMPLE,
 993 // (float)y_result / OVERSAMPLE,
 994 // r_result);
 995
 996 }
 997
 998
 999 // subpixel motion search
1000 void MotionScan::subpixel_search(int &x_result, int &y_result)
1001 {
1002         rotation_pass = 0;
1003         previous_frame = previous_frame_arg;
1004         current_frame = current_frame_arg;
1005
1006 //printf("MotionScan::scan_frame %d %d %d\n", __LINE__, x_result, y_result);
1007 // Scan every subpixel in a SUBPIXEL_RANGE * SUBPIXEL_RANGE square
1008         total_steps = (SUBPIXEL_RANGE * OVERSAMPLE) * (SUBPIXEL_RANGE * OVERSAMPLE);
1009
1010 // These aren't used in subpixel
1011         x_steps = OVERSAMPLE * SUBPIXEL_RANGE;
1012         y_steps = OVERSAMPLE * SUBPIXEL_RANGE;
1013         angle_steps = 1;
1014
1015         set_package_count(this->total_steps);
1016         process_packages();
1017
1018 // Get least difference
1019         int64_t min_difference = -1;
1020         for(int i = 0; i < get_total_packages(); i++)
1021         {
1022                 MotionScanPackage *pkg = (MotionScanPackage*)get_package(i);
1023 //printf("MotionScan::scan_frame %d search_x=%d search_y=%d sub_x=%d sub_y=%d diff1=%lld diff2=%lld\n",
1024 //__LINE__, pkg->search_x, pkg->search_y, pkg->sub_x, pkg->sub_y, pkg->difference1, pkg->difference2);
1025                 if(pkg->difference1 < min_difference || min_difference == -1)
1026                 {
1027                         min_difference = pkg->difference1;
1028
1029 // The sub coords are 1 pixel up & left of the block coords
1030                         x_result = pkg->search_x * OVERSAMPLE + pkg->sub_x;
1031                         y_result = pkg->search_y * OVERSAMPLE + pkg->sub_y;
1032
1033
1034 // Fill in results
1035                         dx_result = block_x1 * OVERSAMPLE - x_result;
1036                         dy_result = block_y1 * OVERSAMPLE - y_result;
1037 //printf("MotionScan::scan_frame %d dx_result=%d dy_result=%d diff=%lld\n",
1038 //__LINE__, dx_result, dy_result, min_difference);
1039                 }
1040
1041                 if(pkg->difference2 < min_difference)
1042                 {
1043                         min_difference = pkg->difference2;
1044
1045                         x_result = pkg->search_x * OVERSAMPLE - pkg->sub_x;
1046                         y_result = pkg->search_y * OVERSAMPLE - pkg->sub_y;
1047
1048                         dx_result = block_x1 * OVERSAMPLE - x_result;
1049                         dy_result = block_y1 * OVERSAMPLE - y_result;
1050 //printf("MotionScan::scan_frame %d dx_result=%d dy_result=%d diff=%lld\n",
1051 //__LINE__, dx_result, dy_result, min_difference);
1052                 }
1053         }
1054 }
1055
1056
1057 void MotionScan::scan_frame(VFrame *previous_frame,
1058         VFrame *current_frame,
1059         int global_range_w,
1060         int global_range_h,
1061         int global_block_w,
1062         int global_block_h,
1063         int block_x,
1064         int block_y,
1065         int frame_type,
1066         int tracking_type,
1067         int action_type,
1068         int horizontal_only,
1069         int vertical_only,
1070         int source_position,
1071         int total_dx,
1072         int total_dy,
1073         int global_origin_x,
1074         int global_origin_y,
1075         int do_motion,
1076         int do_rotate,
1077         double rotation_center,
1078         double rotation_range)
1079 {
1080         this->previous_frame_arg = previous_frame;
1081         this->current_frame_arg = current_frame;
1082         this->horizontal_only = horizontal_only;
1083         this->vertical_only = vertical_only;
1084         this->previous_frame = previous_frame_arg;
1085         this->current_frame = current_frame_arg;
1086         this->global_origin_x = global_origin_x;
1087         this->global_origin_y = global_origin_y;
1088         this->action_type = action_type;
1089         this->do_motion = do_motion;
1090         this->do_rotate = do_rotate;
1091         this->rotation_center = rotation_center;
1092         this->rotation_range = rotation_range;
1093
1094 //printf("MotionScan::scan_frame %d\n", __LINE__);
1095         dx_result = 0;
1096         dy_result = 0;
1097         dr_result = 0;
1098         failed = 0;
1099
1100         subpixel = 0;
1101 // starting level of detail
1102 // TODO: base it on a table of resolutions
1103         current_downsample = STARTING_DOWNSAMPLE;
1104         angle_step = 0;
1105
1106 // Single macroblock
1107         int w = current_frame->get_w();
1108         int h = current_frame->get_h();
1109
1110 // Initial search parameters
1111         scan_w = global_range_w;
1112         scan_h = global_range_h;
1113
1114         int block_w = global_block_w;
1115         int block_h = global_block_h;
1116
1117 // printf("MotionScan::scan_frame %d %d %d %d %d %d %d %d %d\n",
1118 // __LINE__,
1119 // global_range_w,
1120 // global_range_h,
1121 // global_block_w,
1122 // global_block_h,
1123 // scan_w,
1124 // scan_h,
1125 // block_w,
1126 // block_h);
1127
1128 // Location of block in previous frame
1129         block_x1 = (int)(block_x - block_w / 2);
1130         block_y1 = (int)(block_y - block_h / 2);
1131         block_x2 = (int)(block_x + block_w / 2);
1132         block_y2 = (int)(block_y + block_h / 2);
1133
1134 // Offset to location of previous block.  This offset needn't be very accurate
1135 // since it's the offset of the previous image and current image we want.
1136         if(frame_type == MotionScan::TRACK_PREVIOUS)
1137         {
1138                 block_x1 += total_dx / OVERSAMPLE;
1139                 block_y1 += total_dy / OVERSAMPLE;
1140                 block_x2 += total_dx / OVERSAMPLE;
1141                 block_y2 += total_dy / OVERSAMPLE;
1142         }
1143
1144         skip = 0;
1145
1146         switch(tracking_type)
1147         {
1148 // Don't calculate
1149                 case MotionScan::NO_CALCULATE:
1150                         dx_result = 0;
1151                         dy_result = 0;
1152                         dr_result = rotation_center;
1153                         skip = 1;
1154                         break;
1155
1156                 case MotionScan::LOAD:
1157                 {
1158 // Load result from disk
1159                         char string[BCTEXTLEN];
1160
1161                         skip = 1;
1162                         if(do_motion)
1163                         {
1164                                 sprintf(string, "%s%06d",
1165                                         MOTION_FILE,
1166                                         source_position);
1167 //printf("MotionScan::scan_frame %d %s\n", __LINE__, string);
1168                                 FILE *input = fopen(string, "r");
1169                                 if(input)
1170                                 {
1171                                         int temp = fscanf(input,
1172                                                 "%d %d",
1173                                                 &dx_result,
1174                                                 &dy_result);
1175                                         if( temp != 2 )
1176                                                 printf("MotionScan::scan_frame %d %s\n", __LINE__, string);
1177 // HACK
1178 //dx_result *= 2;
1179 //dy_result *= 2;
1180 //printf("MotionScan::scan_frame %d %d %d\n", __LINE__, dx_result, dy_result);
1181                                         fclose(input);
1182                                 }
1183                                 else
1184                                 {
1185                                         skip = 0;
1186                                 }
1187                         }
1188
1189                         if(do_rotate)
1190                         {
1191                                 sprintf(string,
1192                                         "%s%06d",
1193                                         ROTATION_FILE,
1194                                         source_position);
1195                                 FILE *input = fopen(string, "r");
1196                                 if(input)
1197                                 {
1198                                         int temp = fscanf(input, "%f", &dr_result);
1199                                         if( temp != 1 )
1200                                                 printf("MotionScan::scan_frame %d %s\n", __LINE__, string);
1201 // DEBUG
1202 //dr_result += 0.25;
1203                                         fclose(input);
1204                                 }
1205                                 else
1206                                 {
1207                                         skip = 0;
1208                                 }
1209                         }
1210                         break;
1211                 }
1212
1213 // Scan from scratch
1214                 default:
1215                         skip = 0;
1216                         break;
1217         }
1218
1219
1220
1221         if(!skip && test_match)
1222         {
1223                 if(previous_frame->data_matches(current_frame))
1224                 {
1225 printf("MotionScan::scan_frame: data matches. skipping.\n");
1226                         dx_result = 0;
1227                         dy_result = 0;
1228                         dr_result = rotation_center;
1229                         skip = 1;
1230                 }
1231         }
1232
1233
1234 // Perform scan
1235         if(!skip)
1236         {
1237 // Location of block in current frame
1238                 int origin_offset_x = this->global_origin_x;
1239                 int origin_offset_y = this->global_origin_y;
1240                 int x_result = block_x1 + origin_offset_x;
1241                 int y_result = block_y1 + origin_offset_y;
1242                 double r_result = rotation_center;
1243
1244 // printf("MotionScan::scan_frame 1 %d %d %d %d %d %d %d %d\n",
1245 // block_x1 + block_w / 2,
1246 // block_y1 + block_h / 2,
1247 // block_w,
1248 // block_h,
1249 // block_x1,
1250 // block_y1,
1251 // block_x2,
1252 // block_y2);
1253
1254                 while(!failed)
1255                 {
1256                         scan_x1 = x_result - scan_w / 2;
1257                         scan_y1 = y_result - scan_h / 2;
1258                         scan_x2 = x_result + scan_w / 2;
1259                         scan_y2 = y_result + scan_h / 2;
1260                         scan_angle1 = r_result - rotation_range;
1261                         scan_angle2 = r_result + rotation_range;
1262
1263
1264
1265 // Zero out requested values
1266 //                      if(horizontal_only)
1267 //                      {
1268 //                              scan_y1 = block_y1;
1269 //                              scan_y2 = block_y1 + 1;
1270 //                      }
1271 //                      if(vertical_only)
1272 //                      {
1273 //                              scan_x1 = block_x1;
1274 //                              scan_x2 = block_x1 + 1;
1275 //                      }
1276
1277 // printf("MotionScan::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",
1278 // __LINE__,
1279 // block_x1,
1280 // block_y1,
1281 // block_x2,
1282 // block_y2,
1283 // scan_x1,
1284 // scan_y1,
1285 // scan_x2,
1286 // scan_y2);
1287
1288
1289 // Clamp the block coords before the scan so we get useful scan coords.
1290                         clamp_scan(w,
1291                                 h,
1292                                 &block_x1,
1293                                 &block_y1,
1294                                 &block_x2,
1295                                 &block_y2,
1296                                 &scan_x1,
1297                                 &scan_y1,
1298                                 &scan_x2,
1299                                 &scan_y2,
1300                                 0);
1301
1302
1303 // printf("MotionScan::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",
1304 // __LINE__,
1305 // block_x1,
1306 // block_y1,
1307 // block_x2,
1308 // block_y2,
1309 // scan_x1,
1310 // scan_y1,
1311 // scan_x2,
1312 // scan_y2,
1313 // x_result,
1314 // y_result);
1315 //if(y_result == 88) exit(0);
1316
1317
1318 // Give up if invalid coords.
1319                         if(scan_y2 <= scan_y1 ||
1320                                 scan_x2 <= scan_x1 ||
1321                                 block_x2 <= block_x1 ||
1322                                 block_y2 <= block_y1)
1323                         {
1324                                 break;
1325                         }
1326
1327 // For subpixel, the top row and left column are skipped
1328                         if(subpixel)
1329                         {
1330
1331                                 subpixel_search(x_result, y_result);
1332 // printf("MotionScan::scan_frame %d x_result=%d y_result=%d\n",
1333 // __LINE__,
1334 // x_result / OVERSAMPLE,
1335 // y_result / OVERSAMPLE);
1336
1337                                 break;
1338                         }
1339                         else
1340 // Single pixel
1341                         {
1342                                 pixel_search(x_result, y_result, r_result);
1343 //printf("MotionScan::scan_frame %d x_result=%d y_result=%d\n", __LINE__, x_result / OVERSAMPLE, y_result / OVERSAMPLE);
1344
1345                                 if(failed)
1346                                 {
1347                                         dr_result = 0;
1348                                         dx_result = 0;
1349                                         dy_result = 0;
1350                                 }
1351                                 else
1352                                 if(current_downsample <= 1)
1353                                 {
1354                         // Single pixel accuracy reached.  Now do exhaustive subpixel search.
1355                                         if(action_type == MotionScan::STABILIZE ||
1356                                                 action_type == MotionScan::TRACK ||
1357                                                 action_type == MotionScan::NOTHING)
1358                                         {
1359                                                 x_result /= OVERSAMPLE;
1360                                                 y_result /= OVERSAMPLE;
1361 //printf("MotionScan::scan_frame %d x_result=%d y_result=%d\n", __LINE__, x_result, y_result);
1362                                                 scan_w = SUBPIXEL_RANGE;
1363                                                 scan_h = SUBPIXEL_RANGE;
1364 // Final R result
1365                                                 dr_result = rotation_center - r_result;
1366                                                 subpixel = 1;
1367                                         }
1368                                         else
1369                                         {
1370 // Fill in results and quit
1371                                                 dx_result = block_x1 * OVERSAMPLE - x_result;
1372                                                 dy_result = block_y1 * OVERSAMPLE - y_result;
1373                                                 dr_result = rotation_center - r_result;
1374                                                 break;
1375                                         }
1376                                 }
1377                                 else
1378 // Reduce scan area and try again
1379                                 {
1380 //                                      scan_w = (scan_x2 - scan_x1) / 2;
1381 //                                      scan_h = (scan_y2 - scan_y1) / 2;
1382 // need slightly more than 2x downsampling factor
1383
1384                                         if(current_downsample * 3 < scan_w &&
1385                                                 current_downsample * 3 < scan_h)
1386                                         {
1387                                                 scan_w = current_downsample * 3;
1388                                                 scan_h = current_downsample * 3;
1389                                         }
1390
1391                                         if(angle_step * 1.5 < rotation_range)
1392                                         {
1393                                                 rotation_range = angle_step * 1.5;
1394                                         }
1395 //printf("MotionScan::scan_frame %d %f %f\n", __LINE__, angle_step, rotation_range);
1396
1397                                         current_downsample /= 2;
1398
1399 // convert back to pixels
1400                                         x_result /= OVERSAMPLE;
1401                                         y_result /= OVERSAMPLE;
1402 // debug
1403 //exit(1);
1404                                 }
1405
1406                         }
1407                 }
1408
1409                 dx_result *= -1;
1410                 dy_result *= -1;
1411                 dr_result *= -1;
1412         }
1413 // printf("MotionScan::scan_frame %d dx=%f dy=%f dr=%f\n",
1414 // __LINE__,
1415 // (float)dx_result / OVERSAMPLE,
1416 // (float)dy_result / OVERSAMPLE,
1417 // dr_result);
1418
1419
1420
1421
1422 // Write results
1423         if(!skip && tracking_type == MotionScan::SAVE)
1424         {
1425                 char string[BCTEXTLEN];
1426
1427
1428                 if(do_motion)
1429                 {
1430                         sprintf(string,
1431                                 "%s%06d",
1432                                 MOTION_FILE,
1433                                 source_position);
1434                         FILE *output = fopen(string, "w");
1435                         if(output)
1436                         {
1437                                 fprintf(output,
1438                                         "%d %d\n",
1439                                         dx_result,
1440                                         dy_result);
1441                                 fclose(output);
1442                         }
1443                         else
1444                         {
1445                                 printf("MotionScan::scan_frame %d: save motion failed\n", __LINE__);
1446                         }
1447                 }
1448
1449                 if(do_rotate)
1450                 {
1451                         sprintf(string,
1452                                 "%s%06d",
1453                                 ROTATION_FILE,
1454                                 source_position);
1455                         FILE *output = fopen(string, "w");
1456                         if(output)
1457                         {
1458                                 fprintf(output, "%f\n", dr_result);
1459                                 fclose(output);
1460                         }
1461                         else
1462                         {
1463                                 printf("MotionScan::scan_frame %d save rotation failed\n", __LINE__);
1464                         }
1465                 }
1466         }
1467
1468
1469         if(vertical_only) dx_result = 0;
1470         if(horizontal_only) dy_result = 0;
1471
1472 // printf("MotionScan::scan_frame %d dx=%d dy=%d\n",
1473 // __LINE__,
1474 // this->dx_result,
1475 // this->dy_result);
1476 }
1477
1478
1479
1480
1481
1482
1483
1484
1485
1486
1487
1488
1489
1490
1491
1492
1493
1494
1495
1496 #define ABS_DIFF(type, temp_type, multiplier, components) \
1497 { \
1498         temp_type result_temp = 0; \
1499         for(int i = 0; i < h; i++) \
1500         { \
1501                 type *prev_row = (type*)prev_ptr; \
1502                 type *current_row = (type*)current_ptr; \
1503                 for(int j = 0; j < w; j++) \
1504                 { \
1505                         for(int k = 0; k < 3; k++) \
1506                         { \
1507                                 temp_type difference; \
1508                                 difference = *prev_row++ - *current_row++; \
1509                                 difference *= difference; \
1510                                 result_temp += difference; \
1511                         } \
1512                         if(components == 4) \
1513                         { \
1514                                 prev_row++; \
1515                                 current_row++; \
1516                         } \
1517                 } \
1518                 prev_ptr += row_bytes; \
1519                 current_ptr += row_bytes; \
1520         } \
1521         result = (int64_t)(result_temp * multiplier); \
1522 }
1523
1524 int64_t MotionScan::abs_diff(unsigned char *prev_ptr,
1525         unsigned char *current_ptr,
1526         int row_bytes,
1527         int w,
1528         int h,
1529         int color_model)
1530 {
1531         int64_t result = 0;
1532         switch(color_model)
1533         {
1534                 case BC_RGB888:
1535                         ABS_DIFF(unsigned char, int64_t, 1, 3)
1536                         break;
1537                 case BC_RGBA8888:
1538                         ABS_DIFF(unsigned char, int64_t, 1, 4)
1539                         break;
1540                 case BC_RGB_FLOAT:
1541                         ABS_DIFF(float, double, 0x10000, 3)
1542                         break;
1543                 case BC_RGBA_FLOAT:
1544                         ABS_DIFF(float, double, 0x10000, 4)
1545                         break;
1546                 case BC_YUV888:
1547                         ABS_DIFF(unsigned char, int64_t, 1, 3)
1548                         break;
1549                 case BC_YUVA8888:
1550                         ABS_DIFF(unsigned char, int64_t, 1, 4)
1551                         break;
1552         }
1553         return result;
1554 }
1555
1556
1557
1558 #define ABS_DIFF_SUB(type, temp_type, multiplier, components) \
1559 { \
1560         temp_type result_temp = 0; \
1561         temp_type y2_fraction = sub_y * 0x100 / OVERSAMPLE; \
1562         temp_type y1_fraction = 0x100 - y2_fraction; \
1563         temp_type x2_fraction = sub_x * 0x100 / OVERSAMPLE; \
1564         temp_type x1_fraction = 0x100 - x2_fraction; \
1565         for(int i = 0; i < h_sub; i++) \
1566         { \
1567                 type *prev_row1 = (type*)prev_ptr; \
1568                 type *prev_row2 = (type*)prev_ptr + components; \
1569                 type *prev_row3 = (type*)(prev_ptr + row_bytes); \
1570                 type *prev_row4 = (type*)(prev_ptr + row_bytes) + components; \
1571                 type *current_row = (type*)current_ptr; \
1572                 for(int j = 0; j < w_sub; j++) \
1573                 { \
1574 /* Scan each component */ \
1575                         for(int k = 0; k < 3; k++) \
1576                         { \
1577                                 temp_type difference; \
1578                                 temp_type prev_value = \
1579                                         (*prev_row1++ * x1_fraction * y1_fraction + \
1580                                         *prev_row2++ * x2_fraction * y1_fraction + \
1581                                         *prev_row3++ * x1_fraction * y2_fraction + \
1582                                         *prev_row4++ * x2_fraction * y2_fraction) / \
1583                                         0x100 / 0x100; \
1584                                 temp_type current_value = *current_row++; \
1585                                 difference = prev_value - current_value; \
1586                                 difference *= difference; \
1587                                 result_temp += difference; \
1588                         } \
1589  \
1590 /* skip alpha */ \
1591                         if(components == 4) \
1592                         { \
1593                                 prev_row1++; \
1594                                 prev_row2++; \
1595                                 prev_row3++; \
1596                                 prev_row4++; \
1597                                 current_row++; \
1598                         } \
1599                 } \
1600                 prev_ptr += row_bytes; \
1601                 current_ptr += row_bytes; \
1602         } \
1603         result = (int64_t)(result_temp * multiplier); \
1604 }
1605
1606
1607
1608
1609 int64_t MotionScan::abs_diff_sub(unsigned char *prev_ptr,
1610         unsigned char *current_ptr,
1611         int row_bytes,
1612         int w,
1613         int h,
1614         int color_model,
1615         int sub_x,
1616         int sub_y)
1617 {
1618         int h_sub = h - 1;
1619         int w_sub = w - 1;
1620         int64_t result = 0;
1621
1622         switch(color_model)
1623         {
1624                 case BC_RGB888:
1625                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 3)
1626                         break;
1627                 case BC_RGBA8888:
1628                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 4)
1629                         break;
1630                 case BC_RGB_FLOAT:
1631                         ABS_DIFF_SUB(float, double, 0x10000, 3)
1632                         break;
1633                 case BC_RGBA_FLOAT:
1634                         ABS_DIFF_SUB(float, double, 0x10000, 4)
1635                         break;
1636                 case BC_YUV888:
1637                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 3)
1638                         break;
1639                 case BC_YUVA8888:
1640                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 4)
1641                         break;
1642         }
1643         return result;
1644 }
1645
1646
1647 #if 0
1648 #define VARIANCE(type, temp_type, multiplier, components) \
1649 { \
1650         temp_type average[3] = { 0 }; \
1651         temp_type variance[3] = { 0 }; \
1652  \
1653         for(int i = 0; i < h; i++) \
1654         { \
1655                 type *row = (type*)current_ptr + i * row_bytes; \
1656                 for(int j = 0; j < w; j++) \
1657                 { \
1658                         for(int k = 0; k < 3; k++) \
1659                         { \
1660                                 average[k] += row[k]; \
1661                         } \
1662                         row += components; \
1663                 } \
1664         } \
1665         for(int k = 0; k < 3; k++) \
1666         { \
1667                 average[k] /= w * h; \
1668         } \
1669  \
1670         for(int i = 0; i < h; i++) \
1671         { \
1672                 type *row = (type*)current_ptr + i * row_bytes; \
1673                 for(int j = 0; j < w; j++) \
1674                 { \
1675                         for(int k = 0; k < 3; k++) \
1676                         { \
1677                                 variance[k] += SQR(row[k] - average[k]); \
1678                         } \
1679                         row += components; \
1680                 } \
1681         } \
1682         result = (double)multiplier * \
1683                 sqrt((variance[0] + variance[1] + variance[2]) / w / h / 3); \
1684 }
1685
1686 double MotionScan::calculate_variance(unsigned char *current_ptr,
1687         int row_bytes,
1688         int w,
1689         int h,
1690         int color_model)
1691 {
1692         double result = 0;
1693
1694         switch(color_model)
1695         {
1696                 case BC_RGB888:
1697                         VARIANCE(unsigned char, int, 1, 3)
1698                         break;
1699                 case BC_RGBA8888:
1700                         VARIANCE(unsigned char, int, 1, 4)
1701                         break;
1702                 case BC_RGB_FLOAT:
1703                         VARIANCE(float, double, 255, 3)
1704                         break;
1705                 case BC_RGBA_FLOAT:
1706                         VARIANCE(float, double, 255, 4)
1707                         break;
1708                 case BC_YUV888:
1709                         VARIANCE(unsigned char, int, 1, 3)
1710                         break;
1711                 case BC_YUVA8888:
1712                         VARIANCE(unsigned char, int, 1, 4)
1713                         break;
1714         }
1715
1716
1717         return result;
1718 }
1719 #endif // 0
1720
1721
1722
1723
1724 #define RANGE(type, temp_type, multiplier, components) \
1725 { \
1726         temp_type min[3]; \
1727         temp_type max[3]; \
1728         min[0] = 0x7fff; \
1729         min[1] = 0x7fff; \
1730         min[2] = 0x7fff; \
1731         max[0] = 0; \
1732         max[1] = 0; \
1733         max[2] = 0; \
1734  \
1735         for(int i = 0; i < h; i++) \
1736         { \
1737                 type *row = (type*)current_ptr + i * row_bytes; \
1738                 for(int j = 0; j < w; j++) \
1739                 { \
1740                         for(int k = 0; k < 3; k++) \
1741                         { \
1742                                 if(row[k] > max[k]) max[k] = row[k]; \
1743                                 if(row[k] < min[k]) min[k] = row[k]; \
1744                         } \
1745                         row += components; \
1746                 } \
1747         } \
1748  \
1749         for(int k = 0; k < 3; k++) \
1750         { \
1751                 /* printf("MotionScan::calculate_range %d k=%d max=%d min=%d\n", __LINE__, k, max[k], min[k]); */ \
1752                 if(max[k] - min[k] > result) result = max[k] - min[k]; \
1753         } \
1754  \
1755 }
1756
1757 double MotionScan::calculate_range(unsigned char *current_ptr,
1758         int row_bytes,
1759         int w,
1760         int h,
1761         int color_model)
1762 {
1763         double result = 0;
1764
1765         switch(color_model)
1766         {
1767                 case BC_RGB888:
1768                         RANGE(unsigned char, int, 1, 3)
1769                         break;
1770                 case BC_RGBA8888:
1771                         RANGE(unsigned char, int, 1, 4)
1772                         break;
1773                 case BC_RGB_FLOAT:
1774                         RANGE(float, float, 255, 3)
1775                         break;
1776                 case BC_RGBA_FLOAT:
1777                         RANGE(float, float, 255, 4)
1778                         break;
1779                 case BC_YUV888:
1780                         RANGE(unsigned char, int, 1, 3)
1781                         break;
1782                 case BC_YUVA8888:
1783                         RANGE(unsigned char, int, 1, 4)
1784                         break;
1785         }
1786
1787
1788         return result;
1789 }
1790
1791
1792 //#define CLAMP_BLOCK
1793
1794 // this truncates the scan area but not the macroblock unless the macro is defined
1795 void MotionScan::clamp_scan(int w,
1796         int h,
1797         int *block_x1,
1798         int *block_y1,
1799         int *block_x2,
1800         int *block_y2,
1801         int *scan_x1,
1802         int *scan_y1,
1803         int *scan_x2,
1804         int *scan_y2,
1805         int use_absolute)
1806 {
1807 // printf("MotionMain::clamp_scan 1 w=%d h=%d block=%d %d %d %d scan=%d %d %d %d absolute=%d\n",
1808 // w,
1809 // h,
1810 // *block_x1,
1811 // *block_y1,
1812 // *block_x2,
1813 // *block_y2,
1814 // *scan_x1,
1815 // *scan_y1,
1816 // *scan_x2,
1817 // *scan_y2,
1818 // use_absolute);
1819
1820         if(use_absolute)
1821         {
1822 // Limit size of scan area
1823 // Used for drawing vectors
1824 // scan is always out of range before block.
1825                 if(*scan_x1 < 0)
1826                 {
1827 #ifdef CLAMP_BLOCK
1828                         int difference = -*scan_x1;
1829                         *block_x1 += difference;
1830 #endif
1831                         *scan_x1 = 0;
1832                 }
1833
1834                 if(*scan_y1 < 0)
1835                 {
1836 #ifdef CLAMP_BLOCK
1837                         int difference = -*scan_y1;
1838                         *block_y1 += difference;
1839 #endif
1840                         *scan_y1 = 0;
1841                 }
1842
1843                 if(*scan_x2 > w)
1844                 {
1845                         int difference = *scan_x2 - w;
1846 #ifdef CLAMP_BLOCK
1847                         *block_x2 -= difference;
1848 #endif
1849                         *scan_x2 -= difference;
1850                 }
1851
1852                 if(*scan_y2 > h)
1853                 {
1854                         int difference = *scan_y2 - h;
1855 #ifdef CLAMP_BLOCK
1856                         *block_y2 -= difference;
1857 #endif
1858                         *scan_y2 -= difference;
1859                 }
1860
1861                 CLAMP(*scan_x1, 0, w);
1862                 CLAMP(*scan_y1, 0, h);
1863                 CLAMP(*scan_x2, 0, w);
1864                 CLAMP(*scan_y2, 0, h);
1865         }
1866         else
1867         {
1868 // Limit range of upper left block coordinates
1869 // Used for motion tracking
1870                 if(*scan_x1 < 0)
1871                 {
1872                         int difference = -*scan_x1;
1873 #ifdef CLAMP_BLOCK
1874                         *block_x1 += difference;
1875 #endif
1876                         *scan_x2 += difference;
1877                         *scan_x1 = 0;
1878                 }
1879
1880                 if(*scan_y1 < 0)
1881                 {
1882                         int difference = -*scan_y1;
1883 #ifdef CLAMP_BLOCK
1884                         *block_y1 += difference;
1885 #endif
1886                         *scan_y2 += difference;
1887                         *scan_y1 = 0;
1888                 }
1889
1890                 if(*scan_x2 - *block_x1 + *block_x2 > w)
1891                 {
1892                         int difference = *scan_x2 - *block_x1 + *block_x2 - w;
1893                         *scan_x2 -= difference;
1894 #ifdef CLAMP_BLOCK
1895                         *block_x2 -= difference;
1896 #endif
1897                 }
1898
1899                 if(*scan_y2 - *block_y1 + *block_y2 > h)
1900                 {
1901                         int difference = *scan_y2 - *block_y1 + *block_y2 - h;
1902                         *scan_y2 -= difference;
1903 #ifdef CLAMP_BLOCK
1904                         *block_y2 -= difference;
1905 #endif
1906                 }
1907
1908 //              CLAMP(*scan_x1, 0, w - (*block_x2 - *block_x1));
1909 //              CLAMP(*scan_y1, 0, h - (*block_y2 - *block_y1));
1910 //              CLAMP(*scan_x2, 0, w - (*block_x2 - *block_x1));
1911 //              CLAMP(*scan_y2, 0, h - (*block_y2 - *block_y1));
1912         }
1913
1914 // Sanity checks which break the calculation but should never happen if the
1915 // center of the block is inside the frame.
1916         CLAMP(*block_x1, 0, w);
1917         CLAMP(*block_x2, 0, w);
1918         CLAMP(*block_y1, 0, h);
1919         CLAMP(*block_y2, 0, h);
1920
1921 // printf("MotionMain::clamp_scan 2 w=%d h=%d block=%d %d %d %d scan=%d %d %d %d absolute=%d\n",
1922 // w,
1923 // h,
1924 // *block_x1,
1925 // *block_y1,
1926 // *block_x2,
1927 // *block_y2,
1928 // *scan_x1,
1929 // *scan_y1,
1930 // *scan_x2,
1931 // *scan_y2,
1932 // use_absolute);
1933 }
1934
1935
1936