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] = new VFrame();
 606                                 rotated_current[i]->set_use_shm(0);
 607                                 rotated_current[i]->reallocate(0,
 608                                         -1,
 609                                         0,
 610                                         0,
 611                                         0,
 612                                         downsampled_current_w + 1,
 613                                         downsampled_current_h + 1,
 614                                         current_frame_arg->get_color_model(),
 615                                         -1);
 616 //printf("MotionHVScan::pixel_search %d\n", __LINE__);
 617                         }
 618
 619
 620                         if(!rotater)
 621                         {
 622                                 rotater = new AffineEngine(get_total_clients(),
 623                                         get_total_clients());
 624                         }
 625
 626 // get smallest viewport size required for the angle
 627                         double diag = hypot((block_x2 - block_x1) / current_downsample,
 628                                 (block_y2 - block_y1) / current_downsample);
 629                         double angle1 = atan2(block_y2 - block_y1, block_x2 - block_x1) +
 630                                 TO_RAD(step_to_angle(i, r_result));
 631                         double angle2 = -atan2(block_y2 - block_y1, block_x2 - block_x1) +
 632                                 TO_RAD(step_to_angle(i, r_result));
 633                         double max_horiz = MAX(abs(diag * cos(angle1)), abs(diag * cos(angle2)));
 634                         double max_vert = MAX(abs(diag * sin(angle1)), abs(diag * sin(angle2)));
 635                         int center_x = (block_x1 + block_x2) / 2 / current_downsample;
 636                         int center_y = (block_y1 + block_y2) / 2 / current_downsample;
 637                         int x1 = center_x - max_horiz / 2;
 638                         int y1 = center_y - max_vert / 2;
 639                         int x2 = x1 + max_horiz;
 640                         int y2 = y1 + max_vert;
 641                         CLAMP(x1, 0, downsampled_current_w - 1);
 642                         CLAMP(y1, 0, downsampled_current_h - 1);
 643                         CLAMP(x2, 0, downsampled_current_w - 1);
 644                         CLAMP(y2, 0, downsampled_current_h - 1);
 645
 646 //printf("MotionHVScan::pixel_search %d %f %f %d %d\n",
 647 //__LINE__, TO_DEG(angle1), TO_DEG(angle2), (int)max_horiz, (int)max_vert);
 648                         rotater->set_in_viewport(x1,
 649                                 y1,
 650                                 x2 - x1,
 651                                 y2 - y1);
 652                         rotater->set_out_viewport(x1,
 653                                 y1,
 654                                 x2 - x1,
 655                                 y2 - y1);
 656
 657 //                      rotater->set_in_viewport(0,
 658 //                              0,
 659 //                              downsampled_current_w,
 660 //                              downsampled_current_h);
 661 //                      rotater->set_out_viewport(0,
 662 //                              0,
 663 //                              downsampled_current_w,
 664 //                              downsampled_current_h);
 665
 666                         rotater->set_in_pivot(center_x, center_y);
 667                         rotater->set_out_pivot(center_x, center_y);
 668
 669                         rotater->rotate(rotated_current[i],
 670                                 current_frame,
 671                                 step_to_angle(i, r_result));
 672
 673 // rotated_current[i]->draw_rect(block_x1 / current_downsample,
 674 // block_y1 / current_downsample,
 675 // block_x2 / current_downsample,
 676 // block_y2 / current_downsample);
 677 // char string[BCTEXTLEN];
 678 // sprintf(string, "/tmp/rotated%d", i);
 679 // rotated_current[i]->write_png(string);
 680 //downsampled_previous->write_png("/tmp/previous");
 681 //printf("MotionHVScan::pixel_search %d\n", __LINE__);
 682                 }
 683         }
 684
 685
 686
 687
 688
 689
 690 // printf("MotionHVScan::pixel_search %d block x=%d y=%d w=%d h=%d\n",
 691 // __LINE__,
 692 // block_x1 / current_downsample,
 693 // block_y1 / current_downsample,
 694 // block_w / current_downsample,
 695 // block_h / current_downsample);
 696
 697
 698
 699
 700
 701
 702
 703 //exit(1);
 704 // Test only translation of the middle rotated frame
 705         rotation_pass = 0;
 706         total_steps = x_steps * y_steps;
 707         set_package_count(total_steps);
 708         process_packages();
 709
 710
 711
 712
 713
 714
 715 // Get least difference
 716         int64_t min_difference = -1;
 717 #ifdef STDDEV_TEST
 718         double stddev_table[get_total_packages()];
 719 #endif
 720         for(int i = 0; i < get_total_packages(); i++)
 721         {
 722                 MotionHVScanPackage *pkg = (MotionHVScanPackage*)get_package(i);
 723
 724 #ifdef STDDEV_TEST
 725                 double stddev = sqrt(pkg->difference1) /
 726                         (block_w / current_downsample) /
 727                         (block_h / current_downsample) /
 728                         3;
 729 // printf("MotionHVScan::pixel_search %d current_downsample=%d search_x=%d search_y=%d diff1=%f\n",
 730 // __LINE__,
 731 // current_downsample,
 732 // pkg->search_x,
 733 // pkg->search_y,
 734 // sqrt(pkg->difference1) / block_w / current_downsample / block_h / 3 /* / variance */);
 735
 736 // printf("MotionHVScan::pixel_search %d range1=%f stddev=%f\n",
 737 // __LINE__,
 738 // range1,
 739 // stddev);
 740
 741                 stddev_table[i] = stddev;
 742 #endif // STDDEV_TEST
 743
 744                 if(pkg->difference1 < min_difference || i == 0)
 745                 {
 746                         min_difference = pkg->difference1;
 747                         x_result = pkg->search_x * current_downsample * OVERSAMPLE;
 748                         y_result = pkg->search_y * current_downsample * OVERSAMPLE;
 749
 750 // printf("MotionHVScan::pixel_search %d x_result=%d y_result=%d angle_step=%d diff=%lld\n",
 751 // __LINE__,
 752 // block_x1 * OVERSAMPLE - x_result,
 753 // block_y1 * OVERSAMPLE - y_result,
 754 // pkg->angle_step,
 755 // pkg->difference1);
 756
 757                 }
 758         }
 759
 760
 761 #ifdef STDDEV_TEST
 762         qsort(stddev_table, get_total_packages(), sizeof(double), compare);
 763
 764
 765 // reject motion vector if not similar enough
 766 //      if(stddev_table[0] > 0.2)
 767 //      {
 768 // if(debug)
 769 // {
 770 // printf("MotionHVScan::pixel_search %d stddev fail min_stddev=%f\n",
 771 // __LINE__,
 772 // stddev_table[0]);
 773 // }
 774 //              failed = 1;
 775 //              return;
 776 //      }
 777
 778 if(debug)
 779 {
 780         printf("MotionHVScan::pixel_search %d\n", __LINE__);
 781         for(int i = 0; i < get_total_packages(); i++)
 782         {
 783                 printf("%f\n", stddev_table[i]);
 784         }
 785 }
 786
 787 // reject motion vector if not a sigmoid curve
 788 // TODO: use linear interpolation
 789         int steps = 2;
 790         int step = get_total_packages() / steps;
 791         double curve[steps];
 792         for(int i = 0; i < steps; i++)
 793         {
 794                 int start = get_total_packages() * i / steps;
 795                 int end = get_total_packages() * (i + 1) / steps;
 796                 end = MIN(end, get_total_packages() - 1);
 797                 curve[i] = stddev_table[end] - stddev_table[start];
 798         }
 799
 800
 801 //      if(curve[0] < (curve[1] * 1.01) ||
 802 //              curve[2] < (curve[1] * 1.01) ||
 803 //              curve[0] < (curve[2] * 0.75))
 804 //      if(curve[0] < curve[1])
 805 //      {
 806 // if(debug)
 807 // {
 808 // printf("MotionHVScan::pixel_search %d curve fail %f %f\n",
 809 // __LINE__,
 810 // curve[0],
 811 // curve[1]);
 812 // }
 813 //              failed = 1;
 814 //              return;
 815 //      }
 816
 817 if(debug)
 818 {
 819 printf("MotionHVScan::pixel_search %d curve=%f %f ranges=%f %f min_stddev=%f\n",
 820 __LINE__,
 821 curve[0],
 822 curve[1],
 823 range1,
 824 range2,
 825 stddev_table[0]);
 826 }
 827 #endif // STDDEV_TEST
 828
 829
 830
 831
 832
 833         if(do_rotate)
 834         {
 835                 rotation_pass = 1;;
 836                 total_steps = angle_steps;
 837                 scan_x1 = x_result / OVERSAMPLE;
 838                 scan_y1 = y_result / OVERSAMPLE;
 839                 set_package_count(total_steps);
 840                 process_packages();
 841
 842
 843
 844                 min_difference = -1;
 845                 double prev_r_result = r_result;
 846                 for(int i = 0; i < get_total_packages(); i++)
 847                 {
 848                         MotionHVScanPackage *pkg = (MotionHVScanPackage*)get_package(i);
 849
 850 // 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",
 851 // __LINE__,
 852 // pkg->search_x,
 853 // pkg->search_y,
 854 // pkg->search_angle_step,
 855 // pkg->sub_x,
 856 // pkg->sub_y,
 857 // pkg->difference1,
 858 // pkg->difference2);
 859                         if(pkg->difference1 < min_difference || i == 0)
 860                         {
 861                                 min_difference = pkg->difference1;
 862                                 r_result = step_to_angle(i, prev_r_result);
 863
 864         // printf("MotionHVScan::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 // printf("MotionHVScan::scan_frame %d current_downsample=%d x_result=%f y_result=%f r_result=%f\n",
 876 // __LINE__,
 877 // current_downsample,
 878 // (float)x_result / OVERSAMPLE,
 879 // (float)y_result / OVERSAMPLE,
 880 // r_result);
 881
 882 }
 883
 884
 885 // subpixel motion search
 886 void MotionHVScan::subpixel_search(int &x_result, int &y_result)
 887 {
 888         rotation_pass = 0;
 889         previous_frame = previous_frame_arg;
 890         current_frame = current_frame_arg;
 891
 892 //printf("MotionHVScan::scan_frame %d %d %d\n", __LINE__, x_result, y_result);
 893 // Scan every subpixel in a SUBPIXEL_RANGE * SUBPIXEL_RANGE square
 894         total_steps = (SUBPIXEL_RANGE * OVERSAMPLE) * (SUBPIXEL_RANGE * OVERSAMPLE);
 895
 896 // These aren't used in subpixel
 897         x_steps = OVERSAMPLE * SUBPIXEL_RANGE;
 898         y_steps = OVERSAMPLE * SUBPIXEL_RANGE;
 899         angle_steps = 1;
 900
 901         set_package_count(this->total_steps);
 902         process_packages();
 903
 904 // Get least difference
 905         int64_t min_difference = -1;
 906         for(int i = 0; i < get_total_packages(); i++)
 907         {
 908                 MotionHVScanPackage *pkg = (MotionHVScanPackage*)get_package(i);
 909 //printf("MotionHVScan::scan_frame %d search_x=%d search_y=%d sub_x=%d sub_y=%d diff1=%lld diff2=%lld\n",
 910 //__LINE__, pkg->search_x, pkg->search_y, pkg->sub_x, pkg->sub_y, pkg->difference1, pkg->difference2);
 911                 if(pkg->difference1 < min_difference || min_difference == -1)
 912                 {
 913                         min_difference = pkg->difference1;
 914
 915 // The sub coords are 1 pixel up & left of the block coords
 916                         x_result = pkg->search_x * OVERSAMPLE + pkg->sub_x;
 917                         y_result = pkg->search_y * OVERSAMPLE + pkg->sub_y;
 918
 919
 920 // Fill in results
 921                         dx_result = block_x1 * OVERSAMPLE - x_result;
 922                         dy_result = block_y1 * OVERSAMPLE - y_result;
 923 //printf("MotionHVScan::scan_frame %d dx_result=%d dy_result=%d diff=%lld\n",
 924 //__LINE__, dx_result, dy_result, min_difference);
 925                 }
 926
 927                 if(pkg->difference2 < min_difference)
 928                 {
 929                         min_difference = pkg->difference2;
 930
 931                         x_result = pkg->search_x * OVERSAMPLE - pkg->sub_x;
 932                         y_result = pkg->search_y * OVERSAMPLE - pkg->sub_y;
 933
 934                         dx_result = block_x1 * OVERSAMPLE - x_result;
 935                         dy_result = block_y1 * OVERSAMPLE - y_result;
 936 //printf("MotionHVScan::scan_frame %d dx_result=%d dy_result=%d diff=%lld\n",
 937 //__LINE__, dx_result, dy_result, min_difference);
 938                 }
 939         }
 940 }
 941
 942
 943 void MotionHVScan::scan_frame(VFrame *previous_frame,
 944         VFrame *current_frame,
 945         int global_range_w,
 946         int global_range_h,
 947         int global_block_w,
 948         int global_block_h,
 949         int block_x,
 950         int block_y,
 951         int frame_type,
 952         int tracking_type,
 953         int action_type,
 954         int horizontal_only,
 955         int vertical_only,
 956         int source_position,
 957         int total_dx,
 958         int total_dy,
 959         int global_origin_x,
 960         int global_origin_y,
 961         int do_motion,
 962         int do_rotate,
 963         double rotation_center,
 964         double rotation_range)
 965 {
 966         this->previous_frame_arg = previous_frame;
 967         this->current_frame_arg = current_frame;
 968         this->horizontal_only = horizontal_only;
 969         this->vertical_only = vertical_only;
 970         this->previous_frame = previous_frame_arg;
 971         this->current_frame = current_frame_arg;
 972         this->global_origin_x = global_origin_x;
 973         this->global_origin_y = global_origin_y;
 974         this->action_type = action_type;
 975         this->do_motion = do_motion;
 976         this->do_rotate = do_rotate;
 977         this->rotation_center = rotation_center;
 978         this->rotation_range = rotation_range;
 979
 980 //printf("MotionHVScan::scan_frame %d\n", __LINE__);
 981         dx_result = 0;
 982         dy_result = 0;
 983         dr_result = 0;
 984         failed = 0;
 985
 986         subpixel = 0;
 987 // starting level of detail
 988 // TODO: base it on a table of resolutions
 989         current_downsample = STARTING_DOWNSAMPLE;
 990         angle_step = 0;
 991
 992 // Single macroblock
 993         int w = current_frame->get_w();
 994         int h = current_frame->get_h();
 995
 996 // Initial search parameters
 997         scan_w = global_range_w;
 998         scan_h = global_range_h;
 999
1000         int block_w = global_block_w;
1001         int block_h = global_block_h;
1002
1003 // printf("MotionHVScan::scan_frame %d %d %d %d %d %d %d %d %d\n",
1004 // __LINE__,
1005 // global_range_w,
1006 // global_range_h,
1007 // global_block_w,
1008 // global_block_h,
1009 // scan_w,
1010 // scan_h,
1011 // block_w,
1012 // block_h);
1013
1014 // Location of block in previous frame
1015         block_x1 = (int)(block_x - block_w / 2);
1016         block_y1 = (int)(block_y - block_h / 2);
1017         block_x2 = (int)(block_x + block_w / 2);
1018         block_y2 = (int)(block_y + block_h / 2);
1019
1020 // Offset to location of previous block.  This offset needn't be very accurate
1021 // since it's the offset of the previous image and current image we want.
1022         if(frame_type == MotionHVScan::TRACK_PREVIOUS)
1023         {
1024                 block_x1 += total_dx / OVERSAMPLE;
1025                 block_y1 += total_dy / OVERSAMPLE;
1026                 block_x2 += total_dx / OVERSAMPLE;
1027                 block_y2 += total_dy / OVERSAMPLE;
1028         }
1029
1030         skip = 0;
1031
1032         switch(tracking_type)
1033         {
1034 // Don't calculate
1035                 case MotionHVScan::NO_CALCULATE:
1036                         dx_result = 0;
1037                         dy_result = 0;
1038                         dr_result = rotation_center;
1039                         skip = 1;
1040                         break;
1041
1042                 case MotionHVScan::LOAD:
1043                 {
1044 // Load result from disk
1045                         char string[BCTEXTLEN];
1046
1047                         skip = 1;
1048                         if(do_motion)
1049                         {
1050                                 sprintf(string, "%s%06d",
1051                                         MOTION_FILE,
1052                                         source_position);
1053 //printf("MotionHVScan::scan_frame %d %s\n", __LINE__, string);
1054                                 FILE *input = fopen(string, "r");
1055                                 if(input)
1056                                 {
1057                                         int temp = fscanf(input,
1058                                                 "%d %d",
1059                                                 &dx_result,
1060                                                 &dy_result);
1061                                         if( temp != 2 )
1062                                                 printf("MotionHVScan::scan_frame %d %s\n", __LINE__, string);
1063 // HACK
1064 //dx_result *= 2;
1065 //dy_result *= 2;
1066 //printf("MotionHVScan::scan_frame %d %d %d\n", __LINE__, dx_result, dy_result);
1067                                         fclose(input);
1068                                 }
1069                                 else
1070                                 {
1071                                         skip = 0;
1072                                 }
1073                         }
1074
1075                         if(do_rotate)
1076                         {
1077                                 sprintf(string,
1078                                         "%s%06d",
1079                                         ROTATION_FILE,
1080                                         source_position);
1081                                 FILE *input = fopen(string, "r");
1082                                 if(input)
1083                                 {
1084                                         int temp = fscanf(input, "%f", &dr_result);
1085                                         if( temp != 1 )
1086                                                 printf("MotionHVScan::scan_frame %d %s\n", __LINE__, string);
1087 // DEBUG
1088 //dr_result += 0.25;
1089                                         fclose(input);
1090                                 }
1091                                 else
1092                                 {
1093                                         skip = 0;
1094                                 }
1095                         }
1096                         break;
1097                 }
1098
1099 // Scan from scratch
1100                 default:
1101                         skip = 0;
1102                         break;
1103         }
1104
1105
1106
1107         if(!skip && test_match)
1108         {
1109                 if(previous_frame->data_matches(current_frame))
1110                 {
1111 printf("MotionHVScan::scan_frame: data matches. skipping.\n");
1112                         dx_result = 0;
1113                         dy_result = 0;
1114                         dr_result = rotation_center;
1115                         skip = 1;
1116                 }
1117         }
1118
1119
1120 // Perform scan
1121         if(!skip)
1122         {
1123 // Location of block in current frame
1124                 int origin_offset_x = this->global_origin_x;
1125                 int origin_offset_y = this->global_origin_y;
1126                 int x_result = block_x1 + origin_offset_x;
1127                 int y_result = block_y1 + origin_offset_y;
1128                 double r_result = rotation_center;
1129
1130 // printf("MotionHVScan::scan_frame 1 %d %d %d %d %d %d %d %d\n",
1131 // block_x1 + block_w / 2,
1132 // block_y1 + block_h / 2,
1133 // block_w,
1134 // block_h,
1135 // block_x1,
1136 // block_y1,
1137 // block_x2,
1138 // block_y2);
1139
1140                 while(!failed)
1141                 {
1142                         scan_x1 = x_result - scan_w / 2;
1143                         scan_y1 = y_result - scan_h / 2;
1144                         scan_x2 = x_result + scan_w / 2;
1145                         scan_y2 = y_result + scan_h / 2;
1146                         scan_angle1 = r_result - rotation_range;
1147                         scan_angle2 = r_result + rotation_range;
1148
1149
1150
1151 // Zero out requested values
1152 //                      if(horizontal_only)
1153 //                      {
1154 //                              scan_y1 = block_y1;
1155 //                              scan_y2 = block_y1 + 1;
1156 //                      }
1157 //                      if(vertical_only)
1158 //                      {
1159 //                              scan_x1 = block_x1;
1160 //                              scan_x2 = block_x1 + 1;
1161 //                      }
1162
1163 // 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",
1164 // __LINE__,
1165 // block_x1,
1166 // block_y1,
1167 // block_x2,
1168 // block_y2,
1169 // scan_x1,
1170 // scan_y1,
1171 // scan_x2,
1172 // scan_y2);
1173
1174
1175 // Clamp the block coords before the scan so we get useful scan coords.
1176                         clamp_scan(w,
1177                                 h,
1178                                 &block_x1,
1179                                 &block_y1,
1180                                 &block_x2,
1181                                 &block_y2,
1182                                 &scan_x1,
1183                                 &scan_y1,
1184                                 &scan_x2,
1185                                 &scan_y2,
1186                                 0);
1187
1188
1189 // 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",
1190 // __LINE__,
1191 // block_x1,
1192 // block_y1,
1193 // block_x2,
1194 // block_y2,
1195 // scan_x1,
1196 // scan_y1,
1197 // scan_x2,
1198 // scan_y2,
1199 // x_result,
1200 // y_result);
1201 //if(y_result == 88) exit(0);
1202
1203
1204 // Give up if invalid coords.
1205                         if(scan_y2 <= scan_y1 ||
1206                                 scan_x2 <= scan_x1 ||
1207                                 block_x2 <= block_x1 ||
1208                                 block_y2 <= block_y1)
1209                         {
1210                                 break;
1211                         }
1212
1213 // For subpixel, the top row and left column are skipped
1214                         if(subpixel)
1215                         {
1216
1217                                 subpixel_search(x_result, y_result);
1218 // printf("MotionHVScan::scan_frame %d x_result=%d y_result=%d\n",
1219 // __LINE__,
1220 // x_result / OVERSAMPLE,
1221 // y_result / OVERSAMPLE);
1222
1223                                 break;
1224                         }
1225                         else
1226 // Single pixel
1227                         {
1228                                 pixel_search(x_result, y_result, r_result);
1229 //printf("MotionHVScan::scan_frame %d x_result=%d y_result=%d\n", __LINE__, x_result / OVERSAMPLE, y_result / OVERSAMPLE);
1230
1231                                 if(failed)
1232                                 {
1233                                         dr_result = 0;
1234                                         dx_result = 0;
1235                                         dy_result = 0;
1236                                 }
1237                                 else
1238                                 if(current_downsample <= 1)
1239                                 {
1240                         // Single pixel accuracy reached.  Now do exhaustive subpixel search.
1241                                         if(action_type == MotionHVScan::STABILIZE ||
1242                                                 action_type == MotionHVScan::TRACK ||
1243                                                 action_type == MotionHVScan::NOTHING)
1244                                         {
1245                                                 x_result /= OVERSAMPLE;
1246                                                 y_result /= OVERSAMPLE;
1247 //printf("MotionHVScan::scan_frame %d x_result=%d y_result=%d\n", __LINE__, x_result, y_result);
1248                                                 scan_w = SUBPIXEL_RANGE;
1249                                                 scan_h = SUBPIXEL_RANGE;
1250 // Final R result
1251                                                 dr_result = rotation_center - r_result;
1252                                                 subpixel = 1;
1253                                         }
1254                                         else
1255                                         {
1256 // Fill in results and quit
1257                                                 dx_result = block_x1 * OVERSAMPLE - x_result;
1258                                                 dy_result = block_y1 * OVERSAMPLE - y_result;
1259                                                 dr_result = rotation_center - r_result;
1260                                                 break;
1261                                         }
1262                                 }
1263                                 else
1264 // Reduce scan area and try again
1265                                 {
1266 //                                      scan_w = (scan_x2 - scan_x1) / 2;
1267 //                                      scan_h = (scan_y2 - scan_y1) / 2;
1268 // need slightly more than 2x downsampling factor
1269
1270                                         if(current_downsample * 3 < scan_w &&
1271                                                 current_downsample * 3 < scan_h)
1272                                         {
1273                                                 scan_w = current_downsample * 3;
1274                                                 scan_h = current_downsample * 3;
1275                                         }
1276
1277                                         if(angle_step * 1.5 < rotation_range)
1278                                         {
1279                                                 rotation_range = angle_step * 1.5;
1280                                         }
1281 //printf("MotionHVScan::scan_frame %d %f %f\n", __LINE__, angle_step, rotation_range);
1282
1283                                         current_downsample /= 2;
1284
1285 // convert back to pixels
1286                                         x_result /= OVERSAMPLE;
1287                                         y_result /= OVERSAMPLE;
1288 // debug
1289 //exit(1);
1290                                 }
1291
1292                         }
1293                 }
1294
1295                 dx_result *= -1;
1296                 dy_result *= -1;
1297                 dr_result *= -1;
1298         }
1299 // printf("MotionHVScan::scan_frame %d dx=%f dy=%f dr=%f\n",
1300 // __LINE__,
1301 // (float)dx_result / OVERSAMPLE,
1302 // (float)dy_result / OVERSAMPLE,
1303 // dr_result);
1304
1305
1306
1307
1308 // Write results
1309         if(!skip && tracking_type == MotionHVScan::SAVE)
1310         {
1311                 char string[BCTEXTLEN];
1312
1313
1314                 if(do_motion)
1315                 {
1316                         sprintf(string,
1317                                 "%s%06d",
1318                                 MOTION_FILE,
1319                                 source_position);
1320                         FILE *output = fopen(string, "w");
1321                         if(output)
1322                         {
1323                                 fprintf(output,
1324                                         "%d %d\n",
1325                                         dx_result,
1326                                         dy_result);
1327                                 fclose(output);
1328                         }
1329                         else
1330                         {
1331                                 printf("MotionHVScan::scan_frame %d: save motion failed\n", __LINE__);
1332                         }
1333                 }
1334
1335                 if(do_rotate)
1336                 {
1337                         sprintf(string,
1338                                 "%s%06d",
1339                                 ROTATION_FILE,
1340                                 source_position);
1341                         FILE *output = fopen(string, "w");
1342                         if(output)
1343                         {
1344                                 fprintf(output, "%f\n", dr_result);
1345                                 fclose(output);
1346                         }
1347                         else
1348                         {
1349                                 printf("MotionHVScan::scan_frame %d save rotation failed\n", __LINE__);
1350                         }
1351                 }
1352         }
1353
1354
1355         if(vertical_only) dx_result = 0;
1356         if(horizontal_only) dy_result = 0;
1357
1358 // printf("MotionHVScan::scan_frame %d dx=%d dy=%d\n",
1359 // __LINE__,
1360 // this->dx_result,
1361 // this->dy_result);
1362 }
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382 #define ABS_DIFF(type, temp_type, multiplier, components) \
1383 { \
1384         temp_type result_temp = 0; \
1385         for(int i = 0; i < h; i++) \
1386         { \
1387                 type *prev_row = (type*)prev_ptr; \
1388                 type *current_row = (type*)current_ptr; \
1389                 for(int j = 0; j < w; j++) \
1390                 { \
1391                         for(int k = 0; k < 3; k++) \
1392                         { \
1393                                 temp_type difference; \
1394                                 difference = *prev_row++ - *current_row++; \
1395                                 difference *= difference; \
1396                                 result_temp += difference; \
1397                         } \
1398                         if(components == 4) \
1399                         { \
1400                                 prev_row++; \
1401                                 current_row++; \
1402                         } \
1403                 } \
1404                 prev_ptr += row_bytes; \
1405                 current_ptr += row_bytes; \
1406         } \
1407         result = (int64_t)(result_temp * multiplier); \
1408 }
1409
1410 int64_t MotionHVScan::abs_diff(unsigned char *prev_ptr,
1411         unsigned char *current_ptr,
1412         int row_bytes,
1413         int w,
1414         int h,
1415         int color_model)
1416 {
1417         int64_t result = 0;
1418         switch(color_model)
1419         {
1420                 case BC_RGB888:
1421                         ABS_DIFF(unsigned char, int64_t, 1, 3)
1422                         break;
1423                 case BC_RGBA8888:
1424                         ABS_DIFF(unsigned char, int64_t, 1, 4)
1425                         break;
1426                 case BC_RGB_FLOAT:
1427                         ABS_DIFF(float, double, 0x10000, 3)
1428                         break;
1429                 case BC_RGBA_FLOAT:
1430                         ABS_DIFF(float, double, 0x10000, 4)
1431                         break;
1432                 case BC_YUV888:
1433                         ABS_DIFF(unsigned char, int64_t, 1, 3)
1434                         break;
1435                 case BC_YUVA8888:
1436                         ABS_DIFF(unsigned char, int64_t, 1, 4)
1437                         break;
1438         }
1439         return result;
1440 }
1441
1442
1443
1444 #define ABS_DIFF_SUB(type, temp_type, multiplier, components) \
1445 { \
1446         temp_type result_temp = 0; \
1447         temp_type y2_fraction = sub_y * 0x100 / OVERSAMPLE; \
1448         temp_type y1_fraction = 0x100 - y2_fraction; \
1449         temp_type x2_fraction = sub_x * 0x100 / OVERSAMPLE; \
1450         temp_type x1_fraction = 0x100 - x2_fraction; \
1451         for(int i = 0; i < h_sub; i++) \
1452         { \
1453                 type *prev_row1 = (type*)prev_ptr; \
1454                 type *prev_row2 = (type*)prev_ptr + components; \
1455                 type *prev_row3 = (type*)(prev_ptr + row_bytes); \
1456                 type *prev_row4 = (type*)(prev_ptr + row_bytes) + components; \
1457                 type *current_row = (type*)current_ptr; \
1458                 for(int j = 0; j < w_sub; j++) \
1459                 { \
1460 /* Scan each component */ \
1461                         for(int k = 0; k < 3; k++) \
1462                         { \
1463                                 temp_type difference; \
1464                                 temp_type prev_value = \
1465                                         (*prev_row1++ * x1_fraction * y1_fraction + \
1466                                         *prev_row2++ * x2_fraction * y1_fraction + \
1467                                         *prev_row3++ * x1_fraction * y2_fraction + \
1468                                         *prev_row4++ * x2_fraction * y2_fraction) / \
1469                                         0x100 / 0x100; \
1470                                 temp_type current_value = *current_row++; \
1471                                 difference = prev_value - current_value; \
1472                                 difference *= difference; \
1473                                 result_temp += difference; \
1474                         } \
1475  \
1476 /* skip alpha */ \
1477                         if(components == 4) \
1478                         { \
1479                                 prev_row1++; \
1480                                 prev_row2++; \
1481                                 prev_row3++; \
1482                                 prev_row4++; \
1483                                 current_row++; \
1484                         } \
1485                 } \
1486                 prev_ptr += row_bytes; \
1487                 current_ptr += row_bytes; \
1488         } \
1489         result = (int64_t)(result_temp * multiplier); \
1490 }
1491
1492
1493
1494
1495 int64_t MotionHVScan::abs_diff_sub(unsigned char *prev_ptr,
1496         unsigned char *current_ptr,
1497         int row_bytes,
1498         int w,
1499         int h,
1500         int color_model,
1501         int sub_x,
1502         int sub_y)
1503 {
1504         int h_sub = h - 1;
1505         int w_sub = w - 1;
1506         int64_t result = 0;
1507
1508         switch(color_model)
1509         {
1510                 case BC_RGB888:
1511                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 3)
1512                         break;
1513                 case BC_RGBA8888:
1514                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 4)
1515                         break;
1516                 case BC_RGB_FLOAT:
1517                         ABS_DIFF_SUB(float, double, 0x10000, 3)
1518                         break;
1519                 case BC_RGBA_FLOAT:
1520                         ABS_DIFF_SUB(float, double, 0x10000, 4)
1521                         break;
1522                 case BC_YUV888:
1523                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 3)
1524                         break;
1525                 case BC_YUVA8888:
1526                         ABS_DIFF_SUB(unsigned char, int64_t, 1, 4)
1527                         break;
1528         }
1529         return result;
1530 }
1531
1532
1533 #if 0
1534 #define VARIANCE(type, temp_type, multiplier, components) \
1535 { \
1536         temp_type average[3] = { 0 }; \
1537         temp_type variance[3] = { 0 }; \
1538  \
1539         for(int i = 0; i < h; i++) \
1540         { \
1541                 type *row = (type*)current_ptr + i * row_bytes; \
1542                 for(int j = 0; j < w; j++) \
1543                 { \
1544                         for(int k = 0; k < 3; k++) \
1545                         { \
1546                                 average[k] += row[k]; \
1547                         } \
1548                         row += components; \
1549                 } \
1550         } \
1551         for(int k = 0; k < 3; k++) \
1552         { \
1553                 average[k] /= w * h; \
1554         } \
1555  \
1556         for(int i = 0; i < h; i++) \
1557         { \
1558                 type *row = (type*)current_ptr + i * row_bytes; \
1559                 for(int j = 0; j < w; j++) \
1560                 { \
1561                         for(int k = 0; k < 3; k++) \
1562                         { \
1563                                 variance[k] += SQR(row[k] - average[k]); \
1564                         } \
1565                         row += components; \
1566                 } \
1567         } \
1568         result = (double)multiplier * \
1569                 sqrt((variance[0] + variance[1] + variance[2]) / w / h / 3); \
1570 }
1571
1572 double MotionHVScan::calculate_variance(unsigned char *current_ptr,
1573         int row_bytes,
1574         int w,
1575         int h,
1576         int color_model)
1577 {
1578         double result = 0;
1579
1580         switch(color_model)
1581         {
1582                 case BC_RGB888:
1583                         VARIANCE(unsigned char, int, 1, 3)
1584                         break;
1585                 case BC_RGBA8888:
1586                         VARIANCE(unsigned char, int, 1, 4)
1587                         break;
1588                 case BC_RGB_FLOAT:
1589                         VARIANCE(float, double, 255, 3)
1590                         break;
1591                 case BC_RGBA_FLOAT:
1592                         VARIANCE(float, double, 255, 4)
1593                         break;
1594                 case BC_YUV888:
1595                         VARIANCE(unsigned char, int, 1, 3)
1596                         break;
1597                 case BC_YUVA8888:
1598                         VARIANCE(unsigned char, int, 1, 4)
1599                         break;
1600         }
1601
1602
1603         return result;
1604 }
1605 #endif // 0
1606
1607
1608
1609
1610 #define RANGE(type, temp_type, multiplier, components) \
1611 { \
1612         temp_type min[3]; \
1613         temp_type max[3]; \
1614         min[0] = 0x7fff; \
1615         min[1] = 0x7fff; \
1616         min[2] = 0x7fff; \
1617         max[0] = 0; \
1618         max[1] = 0; \
1619         max[2] = 0; \
1620  \
1621         for(int i = 0; i < h; i++) \
1622         { \
1623                 type *row = (type*)current_ptr + i * row_bytes; \
1624                 for(int j = 0; j < w; j++) \
1625                 { \
1626                         for(int k = 0; k < 3; k++) \
1627                         { \
1628                                 if(row[k] > max[k]) max[k] = row[k]; \
1629                                 if(row[k] < min[k]) min[k] = row[k]; \
1630                         } \
1631                         row += components; \
1632                 } \
1633         } \
1634  \
1635         for(int k = 0; k < 3; k++) \
1636         { \
1637                 /* printf("MotionHVScan::calculate_range %d k=%d max=%d min=%d\n", __LINE__, k, max[k], min[k]); */ \
1638                 if(max[k] - min[k] > result) result = max[k] - min[k]; \
1639         } \
1640  \
1641 }
1642
1643 double MotionHVScan::calculate_range(unsigned char *current_ptr,
1644         int row_bytes,
1645         int w,
1646         int h,
1647         int color_model)
1648 {
1649         double result = 0;
1650
1651         switch(color_model)
1652         {
1653                 case BC_RGB888:
1654                         RANGE(unsigned char, int, 1, 3)
1655                         break;
1656                 case BC_RGBA8888:
1657                         RANGE(unsigned char, int, 1, 4)
1658                         break;
1659                 case BC_RGB_FLOAT:
1660                         RANGE(float, float, 255, 3)
1661                         break;
1662                 case BC_RGBA_FLOAT:
1663                         RANGE(float, float, 255, 4)
1664                         break;
1665                 case BC_YUV888:
1666                         RANGE(unsigned char, int, 1, 3)
1667                         break;
1668                 case BC_YUVA8888:
1669                         RANGE(unsigned char, int, 1, 4)
1670                         break;
1671         }
1672
1673
1674         return result;
1675 }
1676
1677
1678 //#define CLAMP_BLOCK
1679
1680 // this truncates the scan area but not the macroblock unless the macro is defined
1681 void MotionHVScan::clamp_scan(int w,
1682         int h,
1683         int *block_x1,
1684         int *block_y1,
1685         int *block_x2,
1686         int *block_y2,
1687         int *scan_x1,
1688         int *scan_y1,
1689         int *scan_x2,
1690         int *scan_y2,
1691         int use_absolute)
1692 {
1693 // printf("MotionHVMain::clamp_scan 1 w=%d h=%d block=%d %d %d %d scan=%d %d %d %d absolute=%d\n",
1694 // w,
1695 // h,
1696 // *block_x1,
1697 // *block_y1,
1698 // *block_x2,
1699 // *block_y2,
1700 // *scan_x1,
1701 // *scan_y1,
1702 // *scan_x2,
1703 // *scan_y2,
1704 // use_absolute);
1705
1706         if(use_absolute)
1707         {
1708 // Limit size of scan area
1709 // Used for drawing vectors
1710 // scan is always out of range before block.
1711                 if(*scan_x1 < 0)
1712                 {
1713 #ifdef CLAMP_BLOCK
1714                         int difference = -*scan_x1;
1715                         *block_x1 += difference;
1716 #endif
1717                         *scan_x1 = 0;
1718                 }
1719
1720                 if(*scan_y1 < 0)
1721                 {
1722 #ifdef CLAMP_BLOCK
1723                         int difference = -*scan_y1;
1724                         *block_y1 += difference;
1725 #endif
1726                         *scan_y1 = 0;
1727                 }
1728
1729                 if(*scan_x2 > w)
1730                 {
1731                         int difference = *scan_x2 - w;
1732 #ifdef CLAMP_BLOCK
1733                         *block_x2 -= difference;
1734 #endif
1735                         *scan_x2 -= difference;
1736                 }
1737
1738                 if(*scan_y2 > h)
1739                 {
1740                         int difference = *scan_y2 - h;
1741 #ifdef CLAMP_BLOCK
1742                         *block_y2 -= difference;
1743 #endif
1744                         *scan_y2 -= difference;
1745                 }
1746
1747                 CLAMP(*scan_x1, 0, w);
1748                 CLAMP(*scan_y1, 0, h);
1749                 CLAMP(*scan_x2, 0, w);
1750                 CLAMP(*scan_y2, 0, h);
1751         }
1752         else
1753         {
1754 // Limit range of upper left block coordinates
1755 // Used for motion tracking
1756                 if(*scan_x1 < 0)
1757                 {
1758                         int difference = -*scan_x1;
1759 #ifdef CLAMP_BLOCK
1760                         *block_x1 += difference;
1761 #endif
1762                         *scan_x2 += difference;
1763                         *scan_x1 = 0;
1764                 }
1765
1766                 if(*scan_y1 < 0)
1767                 {
1768                         int difference = -*scan_y1;
1769 #ifdef CLAMP_BLOCK
1770                         *block_y1 += difference;
1771 #endif
1772                         *scan_y2 += difference;
1773                         *scan_y1 = 0;
1774                 }
1775
1776                 if(*scan_x2 - *block_x1 + *block_x2 > w)
1777                 {
1778                         int difference = *scan_x2 - *block_x1 + *block_x2 - w;
1779                         *scan_x2 -= difference;
1780 #ifdef CLAMP_BLOCK
1781                         *block_x2 -= difference;
1782 #endif
1783                 }
1784
1785                 if(*scan_y2 - *block_y1 + *block_y2 > h)
1786                 {
1787                         int difference = *scan_y2 - *block_y1 + *block_y2 - h;
1788                         *scan_y2 -= difference;
1789 #ifdef CLAMP_BLOCK
1790                         *block_y2 -= difference;
1791 #endif
1792                 }
1793
1794 //              CLAMP(*scan_x1, 0, w - (*block_x2 - *block_x1));
1795 //              CLAMP(*scan_y1, 0, h - (*block_y2 - *block_y1));
1796 //              CLAMP(*scan_x2, 0, w - (*block_x2 - *block_x1));
1797 //              CLAMP(*scan_y2, 0, h - (*block_y2 - *block_y1));
1798         }
1799
1800 // Sanity checks which break the calculation but should never happen if the
1801 // center of the block is inside the frame.
1802         CLAMP(*block_x1, 0, w);
1803         CLAMP(*block_x2, 0, w);
1804         CLAMP(*block_y1, 0, h);
1805         CLAMP(*block_y2, 0, h);
1806
1807 // printf("MotionHVMain::clamp_scan 2 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
1821
1822