Credit Andrew for build mods and cineform format
[goodguy/cinelerra.git] / 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