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