home *** CD-ROM | disk | FTP | other *** search
/ OS/2 Shareware BBS: 10 Tools / 10-Tools.zip / wvis0626.zip / warpvision_20020626.zip / libavcodec / imgresample.c < prev    next >
C/C++ Source or Header  |  2002-06-19  |  18KB  |  616 lines
  1. /*
  2.  * High quality image resampling with polyphase filters 
  3.  * Copyright (c) 2001 Fabrice Bellard.
  4.  *
  5.  * This library is free software; you can redistribute it and/or
  6.  * modify it under the terms of the GNU Lesser General Public
  7.  * License as published by the Free Software Foundation; either
  8.  * version 2 of the License, or (at your option) any later version.
  9.  *
  10.  * This library is distributed in the hope that it will be useful,
  11.  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * Lesser General Public License for more details.
  14.  *
  15.  * You should have received a copy of the GNU Lesser General Public
  16.  * License along with this library; if not, write to the Free Software
  17.  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18.  */
  19. #include "avcodec.h"
  20. #include "dsputil.h"
  21.  
  22. #ifdef USE_FASTMEMCPY
  23. #include "fastmemcpy.h"
  24. #endif
  25.  
  26.  
  27. #define NB_COMPONENTS 3
  28.  
  29. #define PHASE_BITS 4
  30. #define NB_PHASES  (1 << PHASE_BITS)
  31. #define NB_TAPS    4
  32. #define FCENTER    1  /* index of the center of the filter */
  33.  
  34. #define POS_FRAC_BITS 16
  35. #define POS_FRAC      (1 << POS_FRAC_BITS)
  36. /* 6 bits precision is needed for MMX */
  37. #define FILTER_BITS   8
  38.  
  39. #define LINE_BUF_HEIGHT (NB_TAPS * 4)
  40.  
  41. struct ImgReSampleContext {
  42.     int iwidth, iheight, owidth, oheight;
  43.     int h_incr, v_incr;
  44.     INT16 h_filters[NB_PHASES][NB_TAPS] __align8; /* horizontal filters */
  45.     INT16 v_filters[NB_PHASES][NB_TAPS] __align8; /* vertical filters */
  46.     UINT8 *line_buf;
  47. };
  48.  
  49. static inline int get_phase(int pos)
  50. {
  51.     return ((pos) >> (POS_FRAC_BITS - PHASE_BITS)) & ((1 << PHASE_BITS) - 1);
  52. }
  53.  
  54. /* This function must be optimized */
  55. static void h_resample_fast(UINT8 *dst, int dst_width, UINT8 *src, int src_width,
  56.                             int src_start, int src_incr, INT16 *filters)
  57. {
  58.     int src_pos, phase, sum, i;
  59.     UINT8 *s;
  60.     INT16 *filter;
  61.  
  62.     src_pos = src_start;
  63.     for(i=0;i<dst_width;i++) {
  64. #ifdef TEST
  65.         /* test */
  66.         if ((src_pos >> POS_FRAC_BITS) < 0 ||
  67.             (src_pos >> POS_FRAC_BITS) > (src_width - NB_TAPS))
  68.             abort();
  69. #endif
  70.         s = src + (src_pos >> POS_FRAC_BITS);
  71.         phase = get_phase(src_pos);
  72.         filter = filters + phase * NB_TAPS;
  73. #if NB_TAPS == 4
  74.         sum = s[0] * filter[0] +
  75.             s[1] * filter[1] +
  76.             s[2] * filter[2] +
  77.             s[3] * filter[3];
  78. #else
  79.         {
  80.             int j;
  81.             sum = 0;
  82.             for(j=0;j<NB_TAPS;j++)
  83.                 sum += s[j] * filter[j];
  84.         }
  85. #endif
  86.         sum = sum >> FILTER_BITS;
  87.         if (sum < 0)
  88.             sum = 0;
  89.         else if (sum > 255)
  90.             sum = 255;
  91.         dst[0] = sum;
  92.         src_pos += src_incr;
  93.         dst++;
  94.     }
  95. }
  96.  
  97. /* This function must be optimized */
  98. static void v_resample(UINT8 *dst, int dst_width, UINT8 *src, int wrap, 
  99.                        INT16 *filter)
  100. {
  101.     int sum, i;
  102.     UINT8 *s;
  103.  
  104.     s = src;
  105.     for(i=0;i<dst_width;i++) {
  106. #if NB_TAPS == 4
  107.         sum = s[0 * wrap] * filter[0] +
  108.             s[1 * wrap] * filter[1] +
  109.             s[2 * wrap] * filter[2] +
  110.             s[3 * wrap] * filter[3];
  111. #else
  112.         {
  113.             int j;
  114.             UINT8 *s1 = s;
  115.  
  116.             sum = 0;
  117.             for(j=0;j<NB_TAPS;j++) {
  118.                 sum += s1[0] * filter[j];
  119.                 s1 += wrap;
  120.             }
  121.         }
  122. #endif
  123.         sum = sum >> FILTER_BITS;
  124.         if (sum < 0)
  125.             sum = 0;
  126.         else if (sum > 255)
  127.             sum = 255;
  128.         dst[0] = sum;
  129.         dst++;
  130.         s++;
  131.     }
  132. }
  133.  
  134. #ifdef HAVE_MMX
  135.  
  136. #include "i386/mmx.h"
  137.  
  138. #define FILTER4(reg) \
  139. {\
  140.         s = src + (src_pos >> POS_FRAC_BITS);\
  141.         phase = get_phase(src_pos);\
  142.         filter = filters + phase * NB_TAPS;\
  143.         movq_m2r(*s, reg);\
  144.         punpcklbw_r2r(mm7, reg);\
  145.         movq_m2r(*filter, mm6);\
  146.         pmaddwd_r2r(reg, mm6);\
  147.         movq_r2r(mm6, reg);\
  148.         psrlq_i2r(32, reg);\
  149.         paddd_r2r(mm6, reg);\
  150.         psrad_i2r(FILTER_BITS, reg);\
  151.         src_pos += src_incr;\
  152. }
  153.  
  154. #define DUMP(reg) movq_r2m(reg, tmp); printf(#reg "=%016Lx\n", tmp.uq);
  155.  
  156. /* XXX: do four pixels at a time */
  157. static void h_resample_fast4_mmx(UINT8 *dst, int dst_width, UINT8 *src, int src_width,
  158.                                  int src_start, int src_incr, INT16 *filters)
  159. {
  160.     int src_pos, phase;
  161.     UINT8 *s;
  162.     INT16 *filter;
  163.     mmx_t tmp;
  164.     
  165.     src_pos = src_start;
  166.     pxor_r2r(mm7, mm7);
  167.  
  168.     while (dst_width >= 4) {
  169.  
  170.         FILTER4(mm0);
  171.         FILTER4(mm1);
  172.         FILTER4(mm2);
  173.         FILTER4(mm3);
  174.  
  175.         packuswb_r2r(mm7, mm0);
  176.         packuswb_r2r(mm7, mm1);
  177.         packuswb_r2r(mm7, mm3);
  178.         packuswb_r2r(mm7, mm2);
  179.         movq_r2m(mm0, tmp);
  180.         dst[0] = tmp.ub[0];
  181.         movq_r2m(mm1, tmp);
  182.         dst[1] = tmp.ub[0];
  183.         movq_r2m(mm2, tmp);
  184.         dst[2] = tmp.ub[0];
  185.         movq_r2m(mm3, tmp);
  186.         dst[3] = tmp.ub[0];
  187.         dst += 4;
  188.         dst_width -= 4;
  189.     }
  190.     while (dst_width > 0) {
  191.         FILTER4(mm0);
  192.         packuswb_r2r(mm7, mm0);
  193.         movq_r2m(mm0, tmp);
  194.         dst[0] = tmp.ub[0];
  195.         dst++;
  196.         dst_width--;
  197.     }
  198.     emms();
  199. }
  200.  
  201. static void v_resample4_mmx(UINT8 *dst, int dst_width, UINT8 *src, int wrap, 
  202.                             INT16 *filter)
  203. {
  204.     int sum, i, v;
  205.     UINT8 *s;
  206.     mmx_t tmp;
  207.     mmx_t coefs[4];
  208.     
  209.     for(i=0;i<4;i++) {
  210.         v = filter[i];
  211.         coefs[i].uw[0] = v;
  212.         coefs[i].uw[1] = v;
  213.         coefs[i].uw[2] = v;
  214.         coefs[i].uw[3] = v;
  215.     }
  216.     
  217.     pxor_r2r(mm7, mm7);
  218.     s = src;
  219.     while (dst_width >= 4) {
  220.         movq_m2r(s[0 * wrap], mm0);
  221.         punpcklbw_r2r(mm7, mm0);
  222.         movq_m2r(s[1 * wrap], mm1);
  223.         punpcklbw_r2r(mm7, mm1);
  224.         movq_m2r(s[2 * wrap], mm2);
  225.         punpcklbw_r2r(mm7, mm2);
  226.         movq_m2r(s[3 * wrap], mm3);
  227.         punpcklbw_r2r(mm7, mm3);
  228.  
  229.         pmullw_m2r(coefs[0], mm0);
  230.         pmullw_m2r(coefs[1], mm1);
  231.         pmullw_m2r(coefs[2], mm2);
  232.         pmullw_m2r(coefs[3], mm3);
  233.  
  234.         paddw_r2r(mm1, mm0);
  235.         paddw_r2r(mm3, mm2);
  236.         paddw_r2r(mm2, mm0);
  237.         psraw_i2r(FILTER_BITS, mm0);
  238.         
  239.         packuswb_r2r(mm7, mm0);
  240.         movq_r2m(mm0, tmp);
  241.  
  242.         *(UINT32 *)dst = tmp.ud[0];
  243.         dst += 4;
  244.         s += 4;
  245.         dst_width -= 4;
  246.     }
  247.     while (dst_width > 0) {
  248.         sum = s[0 * wrap] * filter[0] +
  249.             s[1 * wrap] * filter[1] +
  250.             s[2 * wrap] * filter[2] +
  251.             s[3 * wrap] * filter[3];
  252.         sum = sum >> FILTER_BITS;
  253.         if (sum < 0)
  254.             sum = 0;
  255.         else if (sum > 255)
  256.             sum = 255;
  257.         dst[0] = sum;
  258.         dst++;
  259.         s++;
  260.         dst_width--;
  261.     }
  262.     emms();
  263. }
  264. #endif
  265.  
  266. /* slow version to handle limit cases. Does not need optimisation */
  267. static void h_resample_slow(UINT8 *dst, int dst_width, UINT8 *src, int src_width,
  268.                             int src_start, int src_incr, INT16 *filters)
  269. {
  270.     int src_pos, phase, sum, j, v, i;
  271.     UINT8 *s, *src_end;
  272.     INT16 *filter;
  273.  
  274.     src_end = src + src_width;
  275.     src_pos = src_start;
  276.     for(i=0;i<dst_width;i++) {
  277.         s = src + (src_pos >> POS_FRAC_BITS);
  278.         phase = get_phase(src_pos);
  279.         filter = filters + phase * NB_TAPS;
  280.         sum = 0;
  281.         for(j=0;j<NB_TAPS;j++) {
  282.             if (s < src)
  283.                 v = src[0];
  284.             else if (s >= src_end)
  285.                 v = src_end[-1];
  286.             else
  287.                 v = s[0];
  288.             sum += v * filter[j];
  289.             s++;
  290.         }
  291.         sum = sum >> FILTER_BITS;
  292.         if (sum < 0)
  293.             sum = 0;
  294.         else if (sum > 255)
  295.             sum = 255;
  296.         dst[0] = sum;
  297.         src_pos += src_incr;
  298.         dst++;
  299.     }
  300. }
  301.  
  302. static void h_resample(UINT8 *dst, int dst_width, UINT8 *src, int src_width,
  303.                        int src_start, int src_incr, INT16 *filters)
  304. {
  305.     int n, src_end;
  306.  
  307.     if (src_start < 0) {
  308.         n = (0 - src_start + src_incr - 1) / src_incr;
  309.         h_resample_slow(dst, n, src, src_width, src_start, src_incr, filters);
  310.         dst += n;
  311.         dst_width -= n;
  312.         src_start += n * src_incr;
  313.     }
  314.     src_end = src_start + dst_width * src_incr;
  315.     if (src_end > ((src_width - NB_TAPS) << POS_FRAC_BITS)) {
  316.         n = (((src_width - NB_TAPS + 1) << POS_FRAC_BITS) - 1 - src_start) / 
  317.             src_incr;
  318.     } else {
  319.         n = dst_width;
  320.     }
  321. #ifdef HAVE_MMX
  322.     if ((mm_flags & MM_MMX) && NB_TAPS == 4)
  323.         h_resample_fast4_mmx(dst, n, 
  324.                              src, src_width, src_start, src_incr, filters);
  325.     else
  326. #endif
  327.         h_resample_fast(dst, n, 
  328.                         src, src_width, src_start, src_incr, filters);
  329.     if (n < dst_width) {
  330.         dst += n;
  331.         dst_width -= n;
  332.         src_start += n * src_incr;
  333.         h_resample_slow(dst, dst_width, 
  334.                         src, src_width, src_start, src_incr, filters);
  335.     }
  336. }
  337.  
  338. static void component_resample(ImgReSampleContext *s, 
  339.                                UINT8 *output, int owrap, int owidth, int oheight,
  340.                                UINT8 *input, int iwrap, int iwidth, int iheight)
  341. {
  342.     int src_y, src_y1, last_src_y, ring_y, phase_y, y1, y;
  343.     UINT8 *new_line, *src_line;
  344.  
  345.     last_src_y = - FCENTER - 1;
  346.     /* position of the bottom of the filter in the source image */
  347.     src_y = (last_src_y + NB_TAPS) * POS_FRAC; 
  348.     ring_y = NB_TAPS; /* position in ring buffer */
  349.     for(y=0;y<oheight;y++) {
  350.         /* apply horizontal filter on new lines from input if needed */
  351.         src_y1 = src_y >> POS_FRAC_BITS;
  352.         while (last_src_y < src_y1) {
  353.             if (++ring_y >= LINE_BUF_HEIGHT + NB_TAPS)
  354.                 ring_y = NB_TAPS;
  355.             last_src_y++;
  356.             /* handle limit conditions : replicate line (slighly
  357.                inefficient because we filter multiple times */
  358.             y1 = last_src_y;
  359.             if (y1 < 0) {
  360.                 y1 = 0;
  361.             } else if (y1 >= iheight) {
  362.                 y1 = iheight - 1;
  363.             }
  364.             src_line = input + y1 * iwrap;
  365.             new_line = s->line_buf + ring_y * owidth;
  366.             /* apply filter and handle limit cases correctly */
  367.             h_resample(new_line, owidth, 
  368.                        src_line, iwidth, - FCENTER * POS_FRAC, s->h_incr, 
  369.                        &s->h_filters[0][0]);
  370.             /* handle ring buffer wraping */
  371.             if (ring_y >= LINE_BUF_HEIGHT) {
  372.                 memcpy(s->line_buf + (ring_y - LINE_BUF_HEIGHT) * owidth,
  373.                        new_line, owidth);
  374.             }
  375.         }
  376.         /* apply vertical filter */
  377.         phase_y = get_phase(src_y);
  378. #ifdef HAVE_MMX
  379.         /* desactivated MMX because loss of precision */
  380.         if ((mm_flags & MM_MMX) && NB_TAPS == 4 && 0)
  381.             v_resample4_mmx(output, owidth, 
  382.                             s->line_buf + (ring_y - NB_TAPS + 1) * owidth, owidth, 
  383.                             &s->v_filters[phase_y][0]);
  384.         else
  385. #endif
  386.             v_resample(output, owidth, 
  387.                        s->line_buf + (ring_y - NB_TAPS + 1) * owidth, owidth, 
  388.                        &s->v_filters[phase_y][0]);
  389.             
  390.         src_y += s->v_incr;
  391.         output += owrap;
  392.     }
  393. }
  394.  
  395. /* XXX: the following filter is quite naive, but it seems to suffice
  396.    for 4 taps */
  397. static void build_filter(INT16 *filter, float factor)
  398. {
  399.     int ph, i, v;
  400.     float x, y, tab[NB_TAPS], norm, mult;
  401.  
  402.     /* if upsampling, only need to interpolate, no filter */
  403.     if (factor > 1.0)
  404.         factor = 1.0;
  405.  
  406.     for(ph=0;ph<NB_PHASES;ph++) {
  407.         norm = 0;
  408.         for(i=0;i<NB_TAPS;i++) {
  409.             
  410.             x = M_PI * ((float)(i - FCENTER) - (float)ph / NB_PHASES) * factor;
  411.             if (x == 0)
  412.                 y = 1.0;
  413.             else
  414.                 y = sin(x) / x;
  415.             tab[i] = y;
  416.             norm += y;
  417.         }
  418.  
  419.         /* normalize so that an uniform color remains the same */
  420.         mult = (float)(1 << FILTER_BITS) / norm;
  421.         for(i=0;i<NB_TAPS;i++) {
  422.             v = (int)(tab[i] * mult);
  423.             filter[ph * NB_TAPS + i] = v;
  424.         }
  425.     }
  426. }
  427.  
  428. ImgReSampleContext *img_resample_init(int owidth, int oheight,
  429.                                       int iwidth, int iheight)
  430. {
  431.     ImgReSampleContext *s;
  432.  
  433.     s = av_mallocz(sizeof(ImgReSampleContext));
  434.     if (!s)
  435.         return NULL;
  436.     s->line_buf = av_mallocz(owidth * (LINE_BUF_HEIGHT + NB_TAPS));
  437.     if (!s->line_buf) 
  438.         goto fail;
  439.     
  440.     s->owidth = owidth;
  441.     s->oheight = oheight;
  442.     s->iwidth = iwidth;
  443.     s->iheight = iheight;
  444.     
  445.     s->h_incr = (iwidth * POS_FRAC) / owidth;
  446.     s->v_incr = (iheight * POS_FRAC) / oheight;
  447.     
  448.     build_filter(&s->h_filters[0][0], (float)owidth / (float)iwidth);
  449.     build_filter(&s->v_filters[0][0], (float)oheight / (float)iheight);
  450.  
  451.     return s;
  452.  fail:
  453.     av_free(s);
  454.     return NULL;
  455. }
  456.  
  457. void img_resample(ImgReSampleContext *s, 
  458.                   AVPicture *output, AVPicture *input)
  459. {
  460.     int i, shift;
  461.  
  462.     for(i=0;i<3;i++) {
  463.         shift = (i == 0) ? 0 : 1;
  464.         component_resample(s, output->data[i], output->linesize[i], 
  465.                            s->owidth >> shift, s->oheight >> shift,
  466.                            input->data[i], input->linesize[i], 
  467.                            s->iwidth >> shift, s->iheight >> shift);
  468.     }
  469. }
  470.  
  471. void img_resample_close(ImgReSampleContext *s)
  472. {
  473.     av_free(s->line_buf);
  474.     av_free(s);
  475. }
  476.  
  477. #ifdef TEST
  478.  
  479. void *av_mallocz(int size)
  480. {
  481.     void *ptr;
  482.     ptr = malloc(size);
  483.     memset(ptr, 0, size);
  484.     return ptr;
  485. }
  486.  
  487. /* input */
  488. #define XSIZE 256
  489. #define YSIZE 256
  490. UINT8 img[XSIZE * YSIZE];
  491.  
  492. /* output */
  493. #define XSIZE1 512
  494. #define YSIZE1 512
  495. UINT8 img1[XSIZE1 * YSIZE1];
  496. UINT8 img2[XSIZE1 * YSIZE1];
  497.  
  498. void save_pgm(const char *filename, UINT8 *img, int xsize, int ysize)
  499. {
  500.     FILE *f;
  501.     f=fopen(filename,"w");
  502.     fprintf(f,"P5\n%d %d\n%d\n", xsize, ysize, 255);
  503.     fwrite(img,1, xsize * ysize,f);
  504.     fclose(f);
  505. }
  506.  
  507. static void dump_filter(INT16 *filter)
  508. {
  509.     int i, ph;
  510.  
  511.     for(ph=0;ph<NB_PHASES;ph++) {
  512.         printf("%2d: ", ph);
  513.         for(i=0;i<NB_TAPS;i++) {
  514.             printf(" %5.2f", filter[ph * NB_TAPS + i] / 256.0);
  515.         }
  516.         printf("\n");
  517.     }
  518. }
  519.  
  520. #ifdef HAVE_MMX
  521. extern int mm_flags;
  522. #endif
  523.  
  524. int main(int argc, char **argv)
  525. {
  526.     int x, y, v, i, xsize, ysize;
  527.     ImgReSampleContext *s;
  528.     float fact, factors[] = { 1/2.0, 3.0/4.0, 1.0, 4.0/3.0, 16.0/9.0, 2.0 };
  529.     char buf[256];
  530.  
  531.     /* build test image */
  532.     for(y=0;y<YSIZE;y++) {
  533.         for(x=0;x<XSIZE;x++) {
  534.             if (x < XSIZE/2 && y < YSIZE/2) {
  535.                 if (x < XSIZE/4 && y < YSIZE/4) {
  536.                     if ((x % 10) <= 6 &&
  537.                         (y % 10) <= 6)
  538.                         v = 0xff;
  539.                     else
  540.                         v = 0x00;
  541.                 } else if (x < XSIZE/4) {
  542.                     if (x & 1) 
  543.                         v = 0xff;
  544.                     else 
  545.                         v = 0;
  546.                 } else if (y < XSIZE/4) {
  547.                     if (y & 1) 
  548.                         v = 0xff;
  549.                     else 
  550.                         v = 0;
  551.                 } else {
  552.                     if (y < YSIZE*3/8) {
  553.                         if ((y+x) & 1) 
  554.                             v = 0xff;
  555.                         else 
  556.                             v = 0;
  557.                     } else {
  558.                         if (((x+3) % 4) <= 1 &&
  559.                             ((y+3) % 4) <= 1)
  560.                             v = 0xff;
  561.                         else
  562.                             v = 0x00;
  563.                     }
  564.                 }
  565.             } else if (x < XSIZE/2) {
  566.                 v = ((x - (XSIZE/2)) * 255) / (XSIZE/2);
  567.             } else if (y < XSIZE/2) {
  568.                 v = ((y - (XSIZE/2)) * 255) / (XSIZE/2);
  569.             } else {
  570.                 v = ((x + y - XSIZE) * 255) / XSIZE;
  571.             }
  572.             img[y * XSIZE + x] = v;
  573.         }
  574.     }
  575.     save_pgm("/tmp/in.pgm", img, XSIZE, YSIZE);
  576.     for(i=0;i<sizeof(factors)/sizeof(float);i++) {
  577.         fact = factors[i];
  578.         xsize = (int)(XSIZE * fact);
  579.         ysize = (int)(YSIZE * fact);
  580.         s = img_resample_init(xsize, ysize, XSIZE, YSIZE);
  581.         printf("Factor=%0.2f\n", fact);
  582.         dump_filter(&s->h_filters[0][0]);
  583.         component_resample(s, img1, xsize, xsize, ysize,
  584.                            img, XSIZE, XSIZE, YSIZE);
  585.         img_resample_close(s);
  586.  
  587.         sprintf(buf, "/tmp/out%d.pgm", i);
  588.         save_pgm(buf, img1, xsize, ysize);
  589.     }
  590.  
  591.     /* mmx test */
  592. #ifdef HAVE_MMX
  593.     printf("MMX test\n");
  594.     fact = 0.72;
  595.     xsize = (int)(XSIZE * fact);
  596.     ysize = (int)(YSIZE * fact);
  597.     mm_flags = MM_MMX;
  598.     s = img_resample_init(xsize, ysize, XSIZE, YSIZE);
  599.     component_resample(s, img1, xsize, xsize, ysize,
  600.                        img, XSIZE, XSIZE, YSIZE);
  601.  
  602.     mm_flags = 0;
  603.     s = img_resample_init(xsize, ysize, XSIZE, YSIZE);
  604.     component_resample(s, img2, xsize, xsize, ysize,
  605.                        img, XSIZE, XSIZE, YSIZE);
  606.     if (memcmp(img1, img2, xsize * ysize) != 0) {
  607.         fprintf(stderr, "mmx error\n");
  608.         exit(1);
  609.     }
  610.     printf("MMX OK\n");
  611. #endif
  612.     return 0;
  613. }
  614.  
  615. #endif
  616.