3779afe0f8079a925ff57fc0bb26d8d2fa728b8f
[ffmpeg.git] / libavcodec / motion_est.c
1 /*
2  * Motion estimation 
3  * Copyright (c) 2000,2001 Gerard Lantau.
4  * 
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., 675 Mass Ave, Cambridge, MA 02139, USA.
19  *
20  * new Motion Estimation (X1/EPZS) by Michael Niedermayer <michaelni@gmx.at>
21  */
22 #include <stdlib.h>
23 #include <stdio.h>
24 #include "avcodec.h"
25 #include "dsputil.h"
26 #include "mpegvideo.h"
27
28 #define ABS(a) ((a)>0 ? (a) : -(a))
29 #define MAX(a,b) ((a) > (b) ? (a) : (b))
30 #define INTER_BIAS      257
31
32 static void halfpel_motion_search(MpegEncContext * s,
33                                   int *mx_ptr, int *my_ptr, int dmin,
34                                   int xmin, int ymin, int xmax, int ymax,
35                                   int pred_x, int pred_y);
36
37 /* config it to test motion vector encoding (send random vectors) */
38 //#define CONFIG_TEST_MV_ENCODE
39
40 static int pix_sum(UINT8 * pix, int line_size)
41 {
42     int s, i, j;
43
44     s = 0;
45     for (i = 0; i < 16; i++) {
46         for (j = 0; j < 16; j += 8) {
47             s += pix[0];
48             s += pix[1];
49             s += pix[2];
50             s += pix[3];
51             s += pix[4];
52             s += pix[5];
53             s += pix[6];
54             s += pix[7];
55             pix += 8;
56         }
57         pix += line_size - 16;
58     }
59     return s;
60 }
61
62 static int pix_dev(UINT8 * pix, int line_size, int mean)
63 {
64     int s, i, j;
65
66     s = 0;
67     for (i = 0; i < 16; i++) {
68         for (j = 0; j < 16; j += 8) {
69             s += ABS(pix[0]-mean);
70             s += ABS(pix[1]-mean);
71             s += ABS(pix[2]-mean);
72             s += ABS(pix[3]-mean);
73             s += ABS(pix[4]-mean);
74             s += ABS(pix[5]-mean);
75             s += ABS(pix[6]-mean);
76             s += ABS(pix[7]-mean);
77             pix += 8;
78         }
79         pix += line_size - 16;
80     }
81     return s;
82 }
83
84 static int pix_norm1(UINT8 * pix, int line_size)
85 {
86     int s, i, j;
87     UINT32 *sq = squareTbl + 256;
88
89     s = 0;
90     for (i = 0; i < 16; i++) {
91         for (j = 0; j < 16; j += 8) {
92             s += sq[pix[0]];
93             s += sq[pix[1]];
94             s += sq[pix[2]];
95             s += sq[pix[3]];
96             s += sq[pix[4]];
97             s += sq[pix[5]];
98             s += sq[pix[6]];
99             s += sq[pix[7]];
100             pix += 8;
101         }
102         pix += line_size - 16;
103     }
104     return s;
105 }
106
107 static int pix_norm(UINT8 * pix1, UINT8 * pix2, int line_size)
108 {
109     int s, i, j;
110     UINT32 *sq = squareTbl + 256;
111
112     s = 0;
113     for (i = 0; i < 16; i++) {
114         for (j = 0; j < 16; j += 8) {
115             s += sq[pix1[0] - pix2[0]];
116             s += sq[pix1[1] - pix2[1]];
117             s += sq[pix1[2] - pix2[2]];
118             s += sq[pix1[3] - pix2[3]];
119             s += sq[pix1[4] - pix2[4]];
120             s += sq[pix1[5] - pix2[5]];
121             s += sq[pix1[6] - pix2[6]];
122             s += sq[pix1[7] - pix2[7]];
123             pix1 += 8;
124             pix2 += 8;
125         }
126         pix1 += line_size - 16;
127         pix2 += line_size - 16;
128     }
129     return s;
130 }
131
132 static void no_motion_search(MpegEncContext * s,
133                              int *mx_ptr, int *my_ptr)
134 {
135     *mx_ptr = 16 * s->mb_x;
136     *my_ptr = 16 * s->mb_y;
137 }
138
139 static int full_motion_search(MpegEncContext * s,
140                               int *mx_ptr, int *my_ptr, int range,
141                               int xmin, int ymin, int xmax, int ymax)
142 {
143     int x1, y1, x2, y2, xx, yy, x, y;
144     int mx, my, dmin, d;
145     UINT8 *pix;
146
147     xx = 16 * s->mb_x;
148     yy = 16 * s->mb_y;
149     x1 = xx - range + 1;        /* we loose one pixel to avoid boundary pb with half pixel pred */
150     if (x1 < xmin)
151         x1 = xmin;
152     x2 = xx + range - 1;
153     if (x2 > xmax)
154         x2 = xmax;
155     y1 = yy - range + 1;
156     if (y1 < ymin)
157         y1 = ymin;
158     y2 = yy + range - 1;
159     if (y2 > ymax)
160         y2 = ymax;
161     pix = s->new_picture[0] + (yy * s->linesize) + xx;
162     dmin = 0x7fffffff;
163     mx = 0;
164     my = 0;
165     for (y = y1; y <= y2; y++) {
166         for (x = x1; x <= x2; x++) {
167             d = pix_abs16x16(pix, s->last_picture[0] + (y * s->linesize) + x,
168                              s->linesize);
169             if (d < dmin ||
170                 (d == dmin &&
171                  (abs(x - xx) + abs(y - yy)) <
172                  (abs(mx - xx) + abs(my - yy)))) {
173                 dmin = d;
174                 mx = x;
175                 my = y;
176             }
177         }
178     }
179
180     *mx_ptr = mx;
181     *my_ptr = my;
182
183 #if 0
184     if (*mx_ptr < -(2 * range) || *mx_ptr >= (2 * range) ||
185         *my_ptr < -(2 * range) || *my_ptr >= (2 * range)) {
186         fprintf(stderr, "error %d %d\n", *mx_ptr, *my_ptr);
187     }
188 #endif
189     return dmin;
190 }
191
192
193 static int log_motion_search(MpegEncContext * s,
194                              int *mx_ptr, int *my_ptr, int range,
195                              int xmin, int ymin, int xmax, int ymax)
196 {
197     int x1, y1, x2, y2, xx, yy, x, y;
198     int mx, my, dmin, d;
199     UINT8 *pix;
200
201     xx = s->mb_x << 4;
202     yy = s->mb_y << 4;
203
204     /* Left limit */
205     x1 = xx - range;
206     if (x1 < xmin)
207         x1 = xmin;
208
209     /* Right limit */
210     x2 = xx + range;
211     if (x2 > xmax)
212         x2 = xmax;
213
214     /* Upper limit */
215     y1 = yy - range;
216     if (y1 < ymin)
217         y1 = ymin;
218
219     /* Lower limit */
220     y2 = yy + range;
221     if (y2 > ymax)
222         y2 = ymax;
223
224     pix = s->new_picture[0] + (yy * s->linesize) + xx;
225     dmin = 0x7fffffff;
226     mx = 0;
227     my = 0;
228
229     do {
230         for (y = y1; y <= y2; y += range) {
231             for (x = x1; x <= x2; x += range) {
232                 d = pix_abs16x16(pix, s->last_picture[0] + (y * s->linesize) + x, s->linesize);
233                 if (d < dmin || (d == dmin && (abs(x - xx) + abs(y - yy)) < (abs(mx - xx) + abs(my - yy)))) {
234                     dmin = d;
235                     mx = x;
236                     my = y;
237                 }
238             }
239         }
240
241         range = range >> 1;
242
243         x1 = mx - range;
244         if (x1 < xmin)
245             x1 = xmin;
246
247         x2 = mx + range;
248         if (x2 > xmax)
249             x2 = xmax;
250
251         y1 = my - range;
252         if (y1 < ymin)
253             y1 = ymin;
254
255         y2 = my + range;
256         if (y2 > ymax)
257             y2 = ymax;
258
259     } while (range >= 1);
260
261 #ifdef DEBUG
262     fprintf(stderr, "log       - MX: %d\tMY: %d\n", mx, my);
263 #endif
264     *mx_ptr = mx;
265     *my_ptr = my;
266     return dmin;
267 }
268
269 static int phods_motion_search(MpegEncContext * s,
270                                int *mx_ptr, int *my_ptr, int range,
271                                int xmin, int ymin, int xmax, int ymax)
272 {
273     int x1, y1, x2, y2, xx, yy, x, y, lastx, d;
274     int mx, my, dminx, dminy;
275     UINT8 *pix;
276
277     xx = s->mb_x << 4;
278     yy = s->mb_y << 4;
279
280     /* Left limit */
281     x1 = xx - range;
282     if (x1 < xmin)
283         x1 = xmin;
284
285     /* Right limit */
286     x2 = xx + range;
287     if (x2 > xmax)
288         x2 = xmax;
289
290     /* Upper limit */
291     y1 = yy - range;
292     if (y1 < ymin)
293         y1 = ymin;
294
295     /* Lower limit */
296     y2 = yy + range;
297     if (y2 > ymax)
298         y2 = ymax;
299
300     pix = s->new_picture[0] + (yy * s->linesize) + xx;
301     mx = 0;
302     my = 0;
303
304     x = xx;
305     y = yy;
306     do {
307         dminx = 0x7fffffff;
308         dminy = 0x7fffffff;
309
310         lastx = x;
311         for (x = x1; x <= x2; x += range) {
312             d = pix_abs16x16(pix, s->last_picture[0] + (y * s->linesize) + x, s->linesize);
313             if (d < dminx || (d == dminx && (abs(x - xx) + abs(y - yy)) < (abs(mx - xx) + abs(my - yy)))) {
314                 dminx = d;
315                 mx = x;
316             }
317         }
318
319         x = lastx;
320         for (y = y1; y <= y2; y += range) {
321             d = pix_abs16x16(pix, s->last_picture[0] + (y * s->linesize) + x, s->linesize);
322             if (d < dminy || (d == dminy && (abs(x - xx) + abs(y - yy)) < (abs(mx - xx) + abs(my - yy)))) {
323                 dminy = d;
324                 my = y;
325             }
326         }
327
328         range = range >> 1;
329
330         x = mx;
331         y = my;
332         x1 = mx - range;
333         if (x1 < xmin)
334             x1 = xmin;
335
336         x2 = mx + range;
337         if (x2 > xmax)
338             x2 = xmax;
339
340         y1 = my - range;
341         if (y1 < ymin)
342             y1 = ymin;
343
344         y2 = my + range;
345         if (y2 > ymax)
346             y2 = ymax;
347
348     } while (range >= 1);
349
350 #ifdef DEBUG
351     fprintf(stderr, "phods     - MX: %d\tMY: %d\n", mx, my);
352 #endif
353
354     /* half pixel search */
355     *mx_ptr = mx;
356     *my_ptr = my;
357     return dminy;
358 }
359
360
361 #define Z_THRESHOLD 256
362
363 #define CHECK_MV(x,y)\
364 {\
365     d = pix_abs16x16(new_pic, old_pic + (x) + (y)*pic_stride, pic_stride);\
366     d += (mv_penalty[((x)<<shift)-pred_x] + mv_penalty[((y)<<shift)-pred_y])*quant;\
367     if(d<dmin){\
368         best[0]=x;\
369         best[1]=y;\
370         dmin=d;\
371     }\
372 }
373
374 #define CHECK_MV_DIR(x,y,new_dir)\
375 {\
376     d = pix_abs16x16(new_pic, old_pic + (x) + (y)*pic_stride, pic_stride);\
377     d += (mv_penalty[((x)<<shift)-pred_x] + mv_penalty[((y)<<shift)-pred_y])*quant;\
378     if(d<dmin){\
379         best[0]=x;\
380         best[1]=y;\
381         dmin=d;\
382         next_dir= new_dir;\
383     }\
384 }
385
386 #define CHECK_MV4(x,y)\
387 {\
388     d = pix_abs8x8(new_pic, old_pic + (x) + (y)*pic_stride, pic_stride);\
389     d += (mv_penalty[((x)<<shift)-pred_x] + mv_penalty[((y)<<shift)-pred_y])*quant;\
390     if(d<dmin){\
391         best[0]=x;\
392         best[1]=y;\
393         dmin=d;\
394     }\
395 }
396
397 #define CHECK_MV4_DIR(x,y,new_dir)\
398 {\
399     d = pix_abs8x8(new_pic, old_pic + (x) + (y)*pic_stride, pic_stride);\
400     d += (mv_penalty[((x)<<shift)-pred_x] + mv_penalty[((y)<<shift)-pred_y])*quant;\
401     if(d<dmin){\
402         best[0]=x;\
403         best[1]=y;\
404         dmin=d;\
405         next_dir= new_dir;\
406     }\
407 }
408
409
410 #define check(x,y,S,v)\
411 if( (x)<(xmin<<(S)) ) printf("%d %d %d %d xmin" #v, (x), (y), s->mb_x, s->mb_y);\
412 if( (x)>(xmax<<(S)) ) printf("%d %d %d %d xmax" #v, (x), (y), s->mb_x, s->mb_y);\
413 if( (y)<(ymin<<(S)) ) printf("%d %d %d %d ymin" #v, (x), (y), s->mb_x, s->mb_y);\
414 if( (y)>(ymax<<(S)) ) printf("%d %d %d %d ymax" #v, (x), (y), s->mb_x, s->mb_y);\
415
416
417 static inline int small_diamond_search(MpegEncContext * s, int *best, int dmin,
418                                        UINT8 *new_pic, UINT8 *old_pic, int pic_stride,
419                                        int pred_x, int pred_y, UINT16 *mv_penalty, int quant,
420                                        int xmin, int ymin, int xmax, int ymax, int shift)
421 {
422     int next_dir=-1;
423
424     for(;;){
425         int d;
426         const int dir= next_dir;
427         const int x= best[0];
428         const int y= best[1];
429         next_dir=-1;
430
431 //printf("%d", dir);
432         if(dir!=2 && x>xmin) CHECK_MV_DIR(x-1, y  , 0)
433         if(dir!=3 && y>ymin) CHECK_MV_DIR(x  , y-1, 1)
434         if(dir!=0 && x<xmax) CHECK_MV_DIR(x+1, y  , 2)
435         if(dir!=1 && y<ymax) CHECK_MV_DIR(x  , y+1, 3)
436
437         if(next_dir==-1){
438             return dmin;
439         }
440     }
441
442 /*    for(;;){
443         int d;
444         const int x= best[0];
445         const int y= best[1];
446         const int last_min=dmin;
447         if(x>xmin) CHECK_MV(x-1, y  )
448         if(y>xmin) CHECK_MV(x  , y-1)
449         if(x<xmax) CHECK_MV(x+1, y  )
450         if(y<xmax) CHECK_MV(x  , y+1)
451         if(x>xmin && y>ymin) CHECK_MV(x-1, y-1)
452         if(x>xmin && y<ymax) CHECK_MV(x-1, y+1)
453         if(x<xmax && y>ymin) CHECK_MV(x+1, y-1)
454         if(x<xmax && y<ymax) CHECK_MV(x+1, y+1)
455         if(x-1>xmin) CHECK_MV(x-2, y  )
456         if(y-1>xmin) CHECK_MV(x  , y-2)
457         if(x+1<xmax) CHECK_MV(x+2, y  )
458         if(y+1<xmax) CHECK_MV(x  , y+2)
459         if(x-1>xmin && y-1>ymin) CHECK_MV(x-2, y-2)
460         if(x-1>xmin && y+1<ymax) CHECK_MV(x-2, y+2)
461         if(x+1<xmax && y-1>ymin) CHECK_MV(x+2, y-2)
462         if(x+1<xmax && y+1<ymax) CHECK_MV(x+2, y+2)
463         if(dmin==last_min) return dmin;
464     }
465     */
466 }
467
468 static inline int small_diamond_search4MV(MpegEncContext * s, int *best, int dmin,
469                                        UINT8 *new_pic, UINT8 *old_pic, int pic_stride,
470                                        int pred_x, int pred_y, UINT16 *mv_penalty, int quant,
471                                        int xmin, int ymin, int xmax, int ymax, int shift)
472 {
473     int next_dir=-1;
474
475     for(;;){
476         int d;
477         const int dir= next_dir;
478         const int x= best[0];
479         const int y= best[1];
480         next_dir=-1;
481
482 //printf("%d", dir);
483         if(dir!=2 && x>xmin) CHECK_MV4_DIR(x-1, y  , 0)
484         if(dir!=3 && y>ymin) CHECK_MV4_DIR(x  , y-1, 1)
485         if(dir!=0 && x<xmax) CHECK_MV4_DIR(x+1, y  , 2)
486         if(dir!=1 && y<ymax) CHECK_MV4_DIR(x  , y+1, 3)
487
488         if(next_dir==-1){
489             return dmin;
490         }
491     }
492 }
493
494 static inline int snake_search(MpegEncContext * s, int *best, int dmin,
495                                        UINT8 *new_pic, UINT8 *old_pic, int pic_stride,
496                                        int pred_x, int pred_y, UINT16 *mv_penalty, int quant,
497                                        int xmin, int ymin, int xmax, int ymax, int shift)
498 {
499     int dir=0;
500     int c=1;
501     static int x_dir[8]= {1,1,0,-1,-1,-1, 0, 1};
502     static int y_dir[8]= {0,1,1, 1, 0,-1,-1,-1};
503     int fails=0;
504     int last_d[2]={dmin, dmin};
505
506 /*static int good=0;
507 static int bad=0;
508 static int point=0;
509
510 point++;
511 if(256*256*256*64%point==0)
512 {
513     printf("%d %d %d\n", good, bad, point);
514 }*/
515
516     for(;;){
517         int x= best[0];
518         int y= best[1];
519         int d;
520         x+=x_dir[dir];
521         y+=y_dir[dir];
522         if(x>=xmin && x<=xmax && y>=ymin && y<=ymax){
523             d = pix_abs16x16(new_pic, old_pic + (x) + (y)*pic_stride, pic_stride);
524             d += (mv_penalty[((x)<<shift)-pred_x] + mv_penalty[((y)<<shift)-pred_y])*quant;
525         }else{
526             d = dmin + 10000; //FIXME smarter boundary handling
527         }
528         if(d<dmin){
529             best[0]=x;
530             best[1]=y;
531             dmin=d;
532
533             if(last_d[1] - last_d[0] > last_d[0] - d) c= -c;
534             dir+=c;
535
536             fails=0;
537 //good++;
538             last_d[1]=last_d[0];
539             last_d[0]=d;
540         }else{
541 //bad++;
542             if(fails){
543                 if(fails>=3) return dmin;
544             }else{
545                 c= -c;
546             }
547             dir+=c*2;
548             fails++;
549         }
550         dir&=7;
551     }
552 }
553
554 static int epzs_motion_search(MpegEncContext * s,
555                              int *mx_ptr, int *my_ptr,
556                              int P[5][2], int pred_x, int pred_y,
557                              int xmin, int ymin, int xmax, int ymax)
558 {
559     int best[2]={0, 0};
560     int d, dmin; 
561     UINT8 *new_pic, *old_pic;
562     const int pic_stride= s->linesize;
563     const int pic_xy= (s->mb_y*pic_stride + s->mb_x)*16;
564     UINT16 *mv_penalty= s->mv_penalty[s->f_code] + MAX_MV; // f_code of the prev frame
565     int quant= s->qscale; // qscale of the prev frame
566     const int shift= 1+s->quarter_sample;
567
568     new_pic = s->new_picture[0] + pic_xy;
569     old_pic = s->last_picture[0] + pic_xy;
570    
571     dmin = pix_abs16x16(new_pic, old_pic, pic_stride);
572     if(dmin<Z_THRESHOLD){
573         *mx_ptr= 0;
574         *my_ptr= 0;
575 //printf("Z");
576         return dmin;
577     }
578
579     /* first line */
580     if ((s->mb_y == 0 || s->first_slice_line || s->first_gob_line)) {
581         CHECK_MV(P[1][0]>>shift, P[1][1]>>shift)
582     }else{
583         CHECK_MV(P[4][0]>>shift, P[4][1]>>shift)
584         if(dmin<Z_THRESHOLD){
585             *mx_ptr= P[4][0]>>shift;
586             *my_ptr= P[4][1]>>shift;
587 //printf("M\n");
588             return dmin;
589         }
590         CHECK_MV(P[1][0]>>shift, P[1][1]>>shift)
591         CHECK_MV(P[2][0]>>shift, P[2][1]>>shift)
592         CHECK_MV(P[3][0]>>shift, P[3][1]>>shift)
593     }
594     CHECK_MV(P[0][0]>>shift, P[0][1]>>shift)
595
596 //check(best[0],best[1],0, b0)
597     if(s->full_search==ME_EPZS)
598         dmin= small_diamond_search(s, best, dmin, new_pic, old_pic, pic_stride, 
599                                    pred_x, pred_y, mv_penalty, quant, xmin, ymin, xmax, ymax, shift);
600     else
601         dmin=         snake_search(s, best, dmin, new_pic, old_pic, pic_stride, 
602                                    pred_x, pred_y, mv_penalty, quant, xmin, ymin, xmax, ymax, shift);
603 //check(best[0],best[1],0, b1)
604     *mx_ptr= best[0];
605     *my_ptr= best[1];    
606
607 //    printf("%d %d %d \n", best[0], best[1], dmin);
608     return dmin;
609 }
610
611 static int epzs_motion_search4(MpegEncContext * s, int block,
612                              int *mx_ptr, int *my_ptr,
613                              int P[6][2], int pred_x, int pred_y,
614                              int xmin, int ymin, int xmax, int ymax)
615 {
616     int best[2]={0, 0};
617     int d, dmin; 
618     UINT8 *new_pic, *old_pic;
619     const int pic_stride= s->linesize;
620     const int pic_xy= ((s->mb_y*2 + (block>>1))*pic_stride + s->mb_x*2 + (block&1))*8;
621     UINT16 *mv_penalty= s->mv_penalty[s->f_code] + MAX_MV; // f_code of the prev frame
622     int quant= s->qscale; // qscale of the prev frame
623     const int shift= 1+s->quarter_sample;
624
625     new_pic = s->new_picture[0] + pic_xy;
626     old_pic = s->last_picture[0] + pic_xy;
627    
628     dmin = pix_abs8x8(new_pic, old_pic, pic_stride);
629
630     /* first line */
631     if ((s->mb_y == 0 || s->first_slice_line || s->first_gob_line) && block<2) {
632         CHECK_MV4(P[1][0]>>shift, P[1][1]>>shift)
633     }else{
634         CHECK_MV4(P[4][0]>>shift, P[4][1]>>shift)
635         if(dmin<Z_THRESHOLD){
636             *mx_ptr= P[4][0]>>shift;
637             *my_ptr= P[4][1]>>shift;
638 //printf("M\n");
639             return dmin;
640         }
641         CHECK_MV4(P[1][0]>>shift, P[1][1]>>shift)
642         CHECK_MV4(P[2][0]>>shift, P[2][1]>>shift)
643         CHECK_MV4(P[3][0]>>shift, P[3][1]>>shift)
644     }
645     CHECK_MV4(P[0][0]>>shift, P[0][1]>>shift)
646     CHECK_MV4(P[5][0]>>shift, P[5][1]>>shift)
647
648 //check(best[0],best[1],0, b0)
649     dmin= small_diamond_search4MV(s, best, dmin, new_pic, old_pic, pic_stride, 
650                                    pred_x, pred_y, mv_penalty, quant, xmin, ymin, xmax, ymax, shift);
651 //check(best[0],best[1],0, b1)
652     *mx_ptr= best[0];
653     *my_ptr= best[1];    
654
655 //    printf("%d %d %d \n", best[0], best[1], dmin);
656     return dmin;
657 }
658
659 #define CHECK_HALF_MV(suffix, x, y) \
660     d= pix_abs16x16_ ## suffix(pix, ptr+((x)>>1), s->linesize);\
661     d += (mv_penalty[pen_x + x] + mv_penalty[pen_y + y])*quant;\
662     if(d<dminh){\
663         dminh= d;\
664         mx= mx1 + x;\
665         my= my1 + y;\
666     }
667
668 #define CHECK_HALF_MV4(suffix, x, y) \
669     d= pix_abs8x8_ ## suffix(pix, ptr+((x)>>1), s->linesize);\
670     d += (mv_penalty[pen_x + x] + mv_penalty[pen_y + y])*quant;\
671     if(d<dminh){\
672         dminh= d;\
673         mx= mx1 + x;\
674         my= my1 + y;\
675     }
676     
677 /* The idea would be to make half pel ME after Inter/Intra decision to 
678    save time. */
679 static inline void halfpel_motion_search(MpegEncContext * s,
680                                   int *mx_ptr, int *my_ptr, int dmin,
681                                   int xmin, int ymin, int xmax, int ymax,
682                                   int pred_x, int pred_y)
683 {
684     UINT16 *mv_penalty= s->mv_penalty[s->f_code] + MAX_MV; // f_code of the prev frame
685     const int quant= s->qscale;
686     int pen_x, pen_y;
687     int mx, my, mx1, my1, d, xx, yy, dminh;
688     UINT8 *pix, *ptr;
689
690     mx = *mx_ptr;
691     my = *my_ptr;
692     ptr = s->last_picture[0] + (my * s->linesize) + mx;
693
694     xx = 16 * s->mb_x;
695     yy = 16 * s->mb_y;
696     pix =  s->new_picture[0] + (yy * s->linesize) + xx;
697     
698     dminh = dmin;
699
700     if (mx > xmin && mx < xmax && 
701         my > ymin && my < ymax) {
702
703         mx= mx1= 2*(mx - xx);
704         my= my1= 2*(my - yy);
705         if(dmin < Z_THRESHOLD && mx==0 && my==0){
706             *mx_ptr = 0;
707             *my_ptr = 0;
708             return;
709         }
710         
711         pen_x= pred_x + mx;
712         pen_y= pred_y + my;
713
714         ptr-= s->linesize;
715         CHECK_HALF_MV(xy2, -1, -1)
716         CHECK_HALF_MV(y2 ,  0, -1)
717         CHECK_HALF_MV(xy2, +1, -1)
718         
719         ptr+= s->linesize;
720         CHECK_HALF_MV(x2 , -1,  0)
721         CHECK_HALF_MV(x2 , +1,  0)
722         CHECK_HALF_MV(xy2, -1, +1)
723         CHECK_HALF_MV(y2 ,  0, +1)
724         CHECK_HALF_MV(xy2, +1, +1)
725
726     }else{
727         mx= 2*(mx - xx);
728         my= 2*(my - yy);
729     }
730
731     *mx_ptr = mx;
732     *my_ptr = my;
733 }
734
735 static inline void halfpel_motion_search4(MpegEncContext * s,
736                                   int *mx_ptr, int *my_ptr, int dmin,
737                                   int xmin, int ymin, int xmax, int ymax,
738                                   int pred_x, int pred_y, int block_x, int block_y)
739 {
740     UINT16 *mv_penalty= s->mv_penalty[s->f_code] + MAX_MV; // f_code of the prev frame
741     const int quant= s->qscale;
742     int pen_x, pen_y;
743     int mx, my, mx1, my1, d, xx, yy, dminh;
744     UINT8 *pix, *ptr;
745
746     xx = 8 * block_x;
747     yy = 8 * block_y;
748     pix =  s->new_picture[0] + (yy * s->linesize) + xx;
749     
750     mx = *mx_ptr;
751     my = *my_ptr;
752     ptr = s->last_picture[0] + ((yy+my) * s->linesize) + xx + mx;
753
754     dminh = dmin;
755
756     if (mx > xmin && mx < xmax && 
757         my > ymin && my < ymax) {
758
759         mx= mx1= 2*mx;
760         my= my1= 2*my;
761         if(dmin < Z_THRESHOLD && mx==0 && my==0){
762             *mx_ptr = 0;
763             *my_ptr = 0;
764             return;
765         }
766         
767         pen_x= pred_x + mx;
768         pen_y= pred_y + my;
769
770         ptr-= s->linesize;
771         CHECK_HALF_MV4(xy2, -1, -1)
772         CHECK_HALF_MV4(y2 ,  0, -1)
773         CHECK_HALF_MV4(xy2, +1, -1)
774         
775         ptr+= s->linesize;
776         CHECK_HALF_MV4(x2 , -1,  0)
777         CHECK_HALF_MV4(x2 , +1,  0)
778         CHECK_HALF_MV4(xy2, -1, +1)
779         CHECK_HALF_MV4(y2 ,  0, +1)
780         CHECK_HALF_MV4(xy2, +1, +1)
781
782     }else{
783         mx*=2;
784         my*=2;
785     }
786
787     *mx_ptr = mx;
788     *my_ptr = my;
789 }
790
791 static inline void set_mv_tables(MpegEncContext * s, int mx, int my)
792 {
793     const int xy= s->mb_x + s->mb_y*s->mb_width;
794     
795     s->mv_table[0][xy] = mx;
796     s->mv_table[1][xy] = my;
797
798     /* has allready been set to the 4 MV if 4MV is done */
799     if(!(s->flags&CODEC_FLAG_4MV)){
800         int mot_xy= s->block_index[0];
801
802         s->motion_val[mot_xy  ][0]= mx;
803         s->motion_val[mot_xy  ][1]= my;
804         s->motion_val[mot_xy+1][0]= mx;
805         s->motion_val[mot_xy+1][1]= my;
806
807         mot_xy += s->block_wrap[0];
808         s->motion_val[mot_xy  ][0]= mx;
809         s->motion_val[mot_xy  ][1]= my;
810         s->motion_val[mot_xy+1][0]= mx;
811         s->motion_val[mot_xy+1][1]= my;
812     }
813 }
814
815 #ifndef CONFIG_TEST_MV_ENCODE
816
817 void estimate_motion(MpegEncContext * s,
818                     int mb_x, int mb_y)
819 {
820     UINT8 *pix, *ppix;
821     int sum, varc, vard, mx, my, range, dmin, xx, yy;
822     int xmin, ymin, xmax, ymax;
823     int rel_xmin, rel_ymin, rel_xmax, rel_ymax;
824     int pred_x=0, pred_y=0;
825     int P[6][2];
826     const int shift= 1+s->quarter_sample;
827     int mb_type=0;
828     
829     range = 8 * (1 << (s->f_code - 1));
830     /* XXX: temporary kludge to avoid overflow for msmpeg4 */
831     if (s->out_format == FMT_H263 && !s->h263_msmpeg4)
832         range = range * 2;
833
834     if (s->unrestricted_mv) {
835         xmin = -16;
836         ymin = -16;
837         if (s->h263_plus)
838             range *= 2;
839         if(s->avctx==NULL || s->avctx->codec->id!=CODEC_ID_MPEG4){
840             xmax = s->mb_width*16;
841             ymax = s->mb_height*16;
842         }else {
843             /* XXX: dunno if this is correct but ffmpeg4 decoder wont like it otherwise 
844                     (cuz the drawn edge isnt large enough))*/
845             xmax = s->width;
846             ymax = s->height;
847         }
848     } else {
849         xmin = 0;
850         ymin = 0;
851         xmax = s->mb_width*16 - 16;
852         ymax = s->mb_height*16 - 16;
853     }
854     switch(s->full_search) {
855     case ME_ZERO:
856     default:
857         no_motion_search(s, &mx, &my);
858         dmin = 0;
859         break;
860     case ME_FULL:
861         dmin = full_motion_search(s, &mx, &my, range, xmin, ymin, xmax, ymax);
862         break;
863     case ME_LOG:
864         dmin = log_motion_search(s, &mx, &my, range / 2, xmin, ymin, xmax, ymax);
865         break;
866     case ME_PHODS:
867         dmin = phods_motion_search(s, &mx, &my, range / 2, xmin, ymin, xmax, ymax);
868         break;
869     case ME_X1:
870     case ME_EPZS:
871        {
872             const int mot_stride = s->block_wrap[0];
873             const int mot_xy = s->block_index[0];
874
875             rel_xmin= xmin - mb_x*16;
876             rel_xmax= xmax - mb_x*16;
877             rel_ymin= ymin - mb_y*16;
878             rel_ymax= ymax - mb_y*16;
879
880             P[0][0] = s->motion_val[mot_xy    ][0];
881             P[0][1] = s->motion_val[mot_xy    ][1];
882             P[1][0] = s->motion_val[mot_xy - 1][0];
883             P[1][1] = s->motion_val[mot_xy - 1][1];
884             if(P[1][0] > (rel_xmax<<shift)) P[1][0]= (rel_xmax<<shift);
885
886             /* special case for first line */
887             if ((mb_y == 0 || s->first_slice_line || s->first_gob_line)) {
888                 P[4][0] = P[1][0];
889                 P[4][1] = P[1][1];
890             } else {
891                 P[2][0] = s->motion_val[mot_xy - mot_stride             ][0];
892                 P[2][1] = s->motion_val[mot_xy - mot_stride             ][1];
893                 P[3][0] = s->motion_val[mot_xy - mot_stride + 2         ][0];
894                 P[3][1] = s->motion_val[mot_xy - mot_stride + 2         ][1];
895                 if(P[2][1] > (rel_ymax<<shift)) P[2][1]= (rel_ymax<<shift);
896                 if(P[3][0] < (rel_xmin<<shift)) P[3][0]= (rel_xmin<<shift);
897                 if(P[3][1] > (rel_ymax<<shift)) P[3][1]= (rel_ymax<<shift);
898         
899                 P[4][0]= mid_pred(P[1][0], P[2][0], P[3][0]);
900                 P[4][1]= mid_pred(P[1][1], P[2][1], P[3][1]);
901             }
902             if(s->out_format == FMT_H263){
903                 pred_x = P[4][0];
904                 pred_y = P[4][1];
905             }else { /* mpeg1 at least */
906                 pred_x= P[1][0];
907                 pred_y= P[1][1];
908             }
909         }
910         dmin = epzs_motion_search(s, &mx, &my, P, pred_x, pred_y, rel_xmin, rel_ymin, rel_xmax, rel_ymax);
911  
912         mx+= mb_x*16;
913         my+= mb_y*16;
914         break;
915     }
916     
917     if(s->flags&CODEC_FLAG_4MV){
918         int block;
919
920         mb_type|= MB_TYPE_INTER4V;
921
922         for(block=0; block<4; block++){
923             int mx4, my4;
924             int pred_x4, pred_y4;
925             int dmin4;
926             static const int off[4]= {2, 1, 1, -1};
927             const int mot_stride = s->block_wrap[0];
928             const int mot_xy = s->block_index[block];
929             const int block_x= mb_x*2 + (block&1);
930             const int block_y= mb_y*2 + (block>>1);
931
932             const int rel_xmin4= xmin - block_x*8;
933             const int rel_xmax4= xmax - block_x*8 + 8;
934             const int rel_ymin4= ymin - block_y*8;
935             const int rel_ymax4= ymax - block_y*8 + 8;
936
937             P[0][0] = s->motion_val[mot_xy    ][0];
938             P[0][1] = s->motion_val[mot_xy    ][1];
939             P[1][0] = s->motion_val[mot_xy - 1][0];
940             P[1][1] = s->motion_val[mot_xy - 1][1];
941             if(P[1][0] > (rel_xmax4<<shift)) P[1][0]= (rel_xmax4<<shift);
942
943             /* special case for first line */
944             if ((mb_y == 0 || s->first_slice_line || s->first_gob_line) && block<2) {
945                 P[4][0] = P[1][0];
946                 P[4][1] = P[1][1];
947             } else {
948                 P[2][0] = s->motion_val[mot_xy - mot_stride             ][0];
949                 P[2][1] = s->motion_val[mot_xy - mot_stride             ][1];
950                 P[3][0] = s->motion_val[mot_xy - mot_stride + off[block]][0];
951                 P[3][1] = s->motion_val[mot_xy - mot_stride + off[block]][1];
952                 if(P[2][1] > (rel_ymax4<<shift)) P[2][1]= (rel_ymax4<<shift);
953                 if(P[3][0] < (rel_xmin4<<shift)) P[3][0]= (rel_xmin4<<shift);
954                 if(P[3][0] > (rel_xmax4<<shift)) P[3][0]= (rel_xmax4<<shift);
955                 if(P[3][1] > (rel_ymax4<<shift)) P[3][1]= (rel_ymax4<<shift);
956         
957                 P[4][0]= mid_pred(P[1][0], P[2][0], P[3][0]);
958                 P[4][1]= mid_pred(P[1][1], P[2][1], P[3][1]);
959             }
960             if(s->out_format == FMT_H263){
961                 pred_x4 = P[4][0];
962                 pred_y4 = P[4][1];
963             }else { /* mpeg1 at least */
964                 pred_x4= P[1][0];
965                 pred_y4= P[1][1];
966             }
967             P[5][0]= mx - mb_x*16;
968             P[5][1]= my - mb_y*16;
969
970             dmin4 = epzs_motion_search4(s, block, &mx4, &my4, P, pred_x4, pred_y4, rel_xmin4, rel_ymin4, rel_xmax4, rel_ymax4);
971
972             halfpel_motion_search4(s, &mx4, &my4, dmin4, rel_xmin4, rel_ymin4, rel_xmax4, rel_ymax4, 
973                                    pred_x4, pred_y4, block_x, block_y);
974      
975             s->motion_val[ s->block_index[block] ][0]= mx4;
976             s->motion_val[ s->block_index[block] ][1]= my4;
977         }
978     }
979
980     /* intra / predictive decision */
981     xx = mb_x * 16;
982     yy = mb_y * 16;
983
984     pix = s->new_picture[0] + (yy * s->linesize) + xx;
985     /* At this point (mx,my) are full-pell and the absolute displacement */
986     ppix = s->last_picture[0] + (my * s->linesize) + mx;
987     
988     sum = pix_sum(pix, s->linesize);
989 #if 0
990     varc = pix_dev(pix, s->linesize, (sum+128)>>8) + INTER_BIAS;
991     vard = pix_abs16x16(pix, ppix, s->linesize);
992 #else
993     sum= (sum+8)>>4;
994     varc = ((pix_norm1(pix, s->linesize) - sum*sum + 128 + 500)>>8);
995     vard = (pix_norm(pix, ppix, s->linesize)+128)>>8;
996 #endif
997
998     s->mb_var[s->mb_width * mb_y + mb_x] = varc;
999     s->avg_mb_var+= varc;
1000     s->mc_mb_var += vard;
1001
1002 #if 0
1003     printf("varc=%4d avg_var=%4d (sum=%4d) vard=%4d mx=%2d my=%2d\n",
1004            varc, s->avg_mb_var, sum, vard, mx - xx, my - yy);
1005 #endif
1006     if(s->flags&CODEC_FLAG_HQ){
1007         if (vard*2 + 200 > varc)
1008             mb_type|= MB_TYPE_INTRA;
1009         if (varc*2 + 200 > vard){
1010             mb_type|= MB_TYPE_INTER;
1011             halfpel_motion_search(s, &mx, &my, dmin, xmin, ymin, xmax, ymax, pred_x, pred_y);
1012         }else{
1013             mx = mx*2 - mb_x*32;
1014             my = my*2 - mb_y*32;
1015         }
1016     }else{
1017         if (vard <= 64 || vard < varc) {
1018             mb_type|= MB_TYPE_INTER;
1019             if (s->full_search != ME_ZERO) {
1020                 halfpel_motion_search(s, &mx, &my, dmin, xmin, ymin, xmax, ymax, pred_x, pred_y);
1021             } else {
1022                 mx -= 16 * mb_x;
1023                 my -= 16 * mb_y;
1024             }
1025         }else{
1026             mb_type|= MB_TYPE_INTRA;
1027             mx = 0;//mx*2 - 32 * mb_x;
1028             my = 0;//my*2 - 32 * mb_y;
1029         }
1030     }
1031
1032     s->mb_type[mb_y*s->mb_width + mb_x]= mb_type;
1033     set_mv_tables(s, mx, my);
1034 }
1035
1036 #else
1037
1038 /* test version which generates valid random vectors */
1039 int estimate_motion(MpegEncContext * s,
1040                     int mb_x, int mb_y,
1041                     int *mx_ptr, int *my_ptr)
1042 {
1043     int xx, yy, x1, y1, x2, y2, range;
1044
1045     if ((random() % 10) >= 5) {
1046         range = 8 * (1 << (s->f_code - 1));
1047         if (s->out_format == FMT_H263 && !s->h263_msmpeg4)
1048             range = range * 2;
1049
1050         xx = 16 * s->mb_x;
1051         yy = 16 * s->mb_y;
1052         x1 = xx - range;
1053         if (x1 < 0)
1054             x1 = 0;
1055         x2 = xx + range - 1;
1056         if (x2 > (s->width - 16))
1057             x2 = s->width - 16;
1058         y1 = yy - range;
1059         if (y1 < 0)
1060             y1 = 0;
1061         y2 = yy + range - 1;
1062         if (y2 > (s->height - 16))
1063             y2 = s->height - 16;
1064
1065         *mx_ptr = (random() % (2 * (x2 - x1 + 1))) + 2 * (x1 - xx);
1066         *my_ptr = (random() % (2 * (y2 - y1 + 1))) + 2 * (y1 - yy);
1067         return 0;
1068     } else {
1069         *mx_ptr = 0;
1070         *my_ptr = 0;
1071         return 1;
1072     }
1073 }
1074
1075 #endif