git.sesse.net Git - ffmpeg/blob - libswresample/rematrix.c

   1 /*
   2  * Copyright (C) 2011 Michael Niedermayer (michaelni@gmx.at)
   3  *
   4  * This file is part of libswresample
   5  *
   6  * libswresample is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public
   8  * License as published by the Free Software Foundation; either
   9  * version 2.1 of the License, or (at your option) any later version.
  10  *
  11  * libswresample 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 GNU
  14  * Lesser General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General Public
  17  * License along with libswresample; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  19  */
  20
  21 #include "swresample_internal.h"
  22 #include "libavutil/audioconvert.h"
  23 #include "libavutil/avassert.h"
  24
  25 #define ONE (1.0)
  26 #define R(x) x
  27 #define SAMPLE float
  28 #define RENAME(x) x ## _float
  29 #include "rematrix_template.c"
  30 #undef SAMPLE
  31 #undef RENAME
  32 #undef R
  33 #undef ONE
  34
  35 #define ONE (-32768)
  36 #define R(x) (((x) + 16384)>>15)
  37 #define SAMPLE int16_t
  38 #define RENAME(x) x ## _s16
  39 #include "rematrix_template.c"
  40
  41
  42 #define FRONT_LEFT             0
  43 #define FRONT_RIGHT            1
  44 #define FRONT_CENTER           2
  45 #define LOW_FREQUENCY          3
  46 #define BACK_LEFT              4
  47 #define BACK_RIGHT             5
  48 #define FRONT_LEFT_OF_CENTER   6
  49 #define FRONT_RIGHT_OF_CENTER  7
  50 #define BACK_CENTER            8
  51 #define SIDE_LEFT              9
  52 #define SIDE_RIGHT             10
  53 #define TOP_CENTER             11
  54 #define TOP_FRONT_LEFT         12
  55 #define TOP_FRONT_CENTER       13
  56 #define TOP_FRONT_RIGHT        14
  57 #define TOP_BACK_LEFT          15
  58 #define TOP_BACK_CENTER        16
  59 #define TOP_BACK_RIGHT         17
  60
  61 static int even(int64_t layout){
  62     if(!layout) return 1;
  63     if(layout&(layout-1)) return 1;
  64     return 0;
  65 }
  66
  67 static int sane_layout(int64_t layout){
  68     if(!(layout & AV_CH_LAYOUT_SURROUND)) // at least 1 front speaker
  69         return 0;
  70     if(!even(layout & (AV_CH_FRONT_LEFT | AV_CH_FRONT_RIGHT))) // no asymetric front
  71         return 0;
  72     if(!even(layout & (AV_CH_SIDE_LEFT | AV_CH_SIDE_RIGHT)))   // no asymetric side
  73         return 0;
  74     if(!even(layout & (AV_CH_BACK_LEFT | AV_CH_BACK_RIGHT)))
  75         return 0;
  76     if(!even(layout & (AV_CH_FRONT_LEFT_OF_CENTER | AV_CH_FRONT_RIGHT_OF_CENTER)))
  77         return 0;
  78     if(av_get_channel_layout_nb_channels(layout) >= SWR_CH_MAX)
  79         return 0;
  80
  81     return 1;
  82 }
  83
  84 int swr_rematrix_init(SwrContext *s){
  85     int i, j, out_i;
  86     double matrix[64][64]={{0}};
  87     int64_t unaccounted= s->in_ch_layout & ~s->out_ch_layout;
  88     double maxcoef=0;
  89
  90     for(i=0; i<64; i++){
  91         if(s->in_ch_layout & s->out_ch_layout & (1LL<<i))
  92             matrix[i][i]= 1.0;
  93     }
  94
  95     if(!sane_layout(s->in_ch_layout)){
  96         av_log(s, AV_LOG_ERROR, "Input channel layout isnt supported\n");
  97         return AVERROR(EINVAL);
  98     }
  99     if(!sane_layout(s->out_ch_layout)){
 100         av_log(s, AV_LOG_ERROR, "Output channel layout isnt supported\n");
 101         return AVERROR(EINVAL);
 102     }
 103
 104 //FIXME implement dolby surround
 105 //FIXME implement full ac3
 106
 107
 108     if(unaccounted & AV_CH_FRONT_CENTER){
 109         if((s->out_ch_layout & AV_CH_LAYOUT_STEREO) == AV_CH_LAYOUT_STEREO){
 110             matrix[ FRONT_LEFT][FRONT_CENTER]+= M_SQRT1_2;
 111             matrix[FRONT_RIGHT][FRONT_CENTER]+= M_SQRT1_2;
 112         }else
 113             av_assert0(0);
 114     }
 115     if(unaccounted & AV_CH_LAYOUT_STEREO){
 116         if(s->out_ch_layout & AV_CH_FRONT_CENTER){
 117             matrix[FRONT_CENTER][ FRONT_LEFT]+= M_SQRT1_2;
 118             matrix[FRONT_CENTER][FRONT_RIGHT]+= M_SQRT1_2;
 119             if(s->in_ch_layout & AV_CH_FRONT_CENTER)
 120                 matrix[FRONT_CENTER][ FRONT_CENTER] = s->clev*sqrt(2);
 121         }else
 122             av_assert0(0);
 123     }
 124
 125     if(unaccounted & AV_CH_BACK_CENTER){
 126         if(s->out_ch_layout & AV_CH_BACK_LEFT){
 127             matrix[ BACK_LEFT][BACK_CENTER]+= M_SQRT1_2;
 128             matrix[BACK_RIGHT][BACK_CENTER]+= M_SQRT1_2;
 129         }else if(s->out_ch_layout & AV_CH_SIDE_LEFT){
 130             matrix[ SIDE_LEFT][BACK_CENTER]+= M_SQRT1_2;
 131             matrix[SIDE_RIGHT][BACK_CENTER]+= M_SQRT1_2;
 132         }else if(s->out_ch_layout & AV_CH_FRONT_LEFT){
 133             matrix[ FRONT_LEFT][BACK_CENTER]+= s->slev*M_SQRT1_2;
 134             matrix[FRONT_RIGHT][BACK_CENTER]+= s->slev*M_SQRT1_2;
 135         }else if(s->out_ch_layout & AV_CH_FRONT_CENTER){
 136             matrix[ FRONT_CENTER][BACK_CENTER]+= s->slev*M_SQRT1_2;
 137         }else
 138             av_assert0(0);
 139     }
 140     if(unaccounted & AV_CH_BACK_LEFT){
 141         if(s->out_ch_layout & AV_CH_BACK_CENTER){
 142             matrix[BACK_CENTER][ BACK_LEFT]+= M_SQRT1_2;
 143             matrix[BACK_CENTER][BACK_RIGHT]+= M_SQRT1_2;
 144         }else if(s->out_ch_layout & AV_CH_SIDE_LEFT){
 145             if(s->in_ch_layout & AV_CH_SIDE_LEFT){
 146                 matrix[ SIDE_LEFT][ BACK_LEFT]+= M_SQRT1_2;
 147                 matrix[SIDE_RIGHT][BACK_RIGHT]+= M_SQRT1_2;
 148             }else{
 149             matrix[ SIDE_LEFT][ BACK_LEFT]+= 1.0;
 150             matrix[SIDE_RIGHT][BACK_RIGHT]+= 1.0;
 151             }
 152         }else if(s->out_ch_layout & AV_CH_FRONT_LEFT){
 153             matrix[ FRONT_LEFT][ BACK_LEFT]+= s->slev;
 154             matrix[FRONT_RIGHT][BACK_RIGHT]+= s->slev;
 155         }else if(s->out_ch_layout & AV_CH_FRONT_CENTER){
 156             matrix[ FRONT_CENTER][BACK_LEFT ]+= s->slev*M_SQRT1_2;
 157             matrix[ FRONT_CENTER][BACK_RIGHT]+= s->slev*M_SQRT1_2;
 158         }else
 159             av_assert0(0);
 160     }
 161
 162     if(unaccounted & AV_CH_SIDE_LEFT){
 163         if(s->out_ch_layout & AV_CH_BACK_LEFT){
 164             matrix[ BACK_LEFT][ SIDE_LEFT]+= 1.0;
 165             matrix[BACK_RIGHT][SIDE_RIGHT]+= 1.0;
 166         }else if(s->out_ch_layout & AV_CH_BACK_CENTER){
 167             matrix[BACK_CENTER][ SIDE_LEFT]+= M_SQRT1_2;
 168             matrix[BACK_CENTER][SIDE_RIGHT]+= M_SQRT1_2;
 169         }else if(s->out_ch_layout & AV_CH_FRONT_LEFT){
 170             matrix[ FRONT_LEFT][ SIDE_LEFT]+= s->slev;
 171             matrix[FRONT_RIGHT][SIDE_RIGHT]+= s->slev;
 172         }else if(s->out_ch_layout & AV_CH_FRONT_CENTER){
 173             matrix[ FRONT_CENTER][SIDE_LEFT ]+= s->slev*M_SQRT1_2;
 174             matrix[ FRONT_CENTER][SIDE_RIGHT]+= s->slev*M_SQRT1_2;
 175         }else
 176             av_assert0(0);
 177     }
 178
 179     if(unaccounted & AV_CH_FRONT_LEFT_OF_CENTER){
 180         if(s->out_ch_layout & AV_CH_FRONT_LEFT){
 181             matrix[ FRONT_LEFT][ FRONT_LEFT_OF_CENTER]+= 1.0;
 182             matrix[FRONT_RIGHT][FRONT_RIGHT_OF_CENTER]+= 1.0;
 183         }else if(s->out_ch_layout & AV_CH_FRONT_CENTER){
 184             matrix[ FRONT_CENTER][ FRONT_LEFT_OF_CENTER]+= M_SQRT1_2;
 185             matrix[ FRONT_CENTER][FRONT_RIGHT_OF_CENTER]+= M_SQRT1_2;
 186         }else
 187             av_assert0(0);
 188     }
 189
 190     //FIXME quantize for integeres
 191     for(out_i=i=0; i<64; i++){
 192         double sum=0;
 193         int in_i=0;
 194         int ch_in=0;
 195         for(j=0; j<64; j++){
 196             s->matrix[out_i][in_i]= matrix[i][j];
 197             s->matrix16[out_i][in_i]= lrintf(matrix[i][j] * 32768);
 198             if(matrix[i][j]){
 199                 s->matrix_ch[out_i][++ch_in]= in_i;
 200                 sum += fabs(matrix[i][j]);
 201             }
 202             if(s->in_ch_layout & (1ULL<<j))
 203                 in_i++;
 204         }
 205         s->matrix_ch[out_i][0]= ch_in;
 206         maxcoef= FFMAX(maxcoef, sum);
 207         if(s->out_ch_layout & (1ULL<<i))
 208             out_i++;
 209     }
 210     if((   s->out_sample_fmt < AV_SAMPLE_FMT_FLT
 211         || s->int_sample_fmt < AV_SAMPLE_FMT_FLT) && maxcoef > 1.0){
 212         for(i=0; i<SWR_CH_MAX; i++)
 213             for(j=0; j<SWR_CH_MAX; j++){
 214                 s->matrix[i][j] /= maxcoef;
 215                 s->matrix16[i][j]= lrintf(s->matrix[i][j] * 32768);
 216             }
 217     }
 218         for(i=0; i<av_get_channel_layout_nb_channels(s->out_ch_layout); i++){
 219             for(j=0; j<av_get_channel_layout_nb_channels(s->in_ch_layout); j++){
 220                 av_log(NULL, AV_LOG_DEBUG, "%f ", s->matrix[i][j]);
 221             }
 222             av_log(NULL, AV_LOG_DEBUG, "\n");
 223         }
 224     return 0;
 225 }
 226
 227 int swr_rematrix(SwrContext *s, AudioData *out, AudioData *in, int len, int mustcopy){
 228     int out_i, in_i, i, j;
 229
 230 av_assert0(out->ch_count == av_get_channel_layout_nb_channels(s->out_ch_layout));
 231 av_assert0(in ->ch_count == av_get_channel_layout_nb_channels(s-> in_ch_layout));
 232
 233     for(out_i=0; out_i<out->ch_count; out_i++){
 234         switch(s->matrix_ch[out_i][0]){
 235         case 1:
 236             in_i= s->matrix_ch[out_i][1];
 237             if(mustcopy || s->matrix[out_i][in_i]!=1.0){
 238                 if(s->int_sample_fmt == AV_SAMPLE_FMT_FLT){
 239                     copy_float(out->ch[out_i], in->ch[in_i], s->matrix[out_i][in_i], len);
 240                 }else
 241                     copy_s16  (out->ch[out_i], in->ch[in_i], s->matrix16[out_i][in_i], len);
 242             }else{
 243                 out->ch[out_i]= in->ch[in_i];
 244             }
 245             break;
 246         case 2:
 247             if(s->int_sample_fmt == AV_SAMPLE_FMT_FLT){
 248                 sum2_float(out->ch[out_i], in->ch[ s->matrix_ch[out_i][1] ],           in->ch[ s->matrix_ch[out_i][2] ],
 249                                  s->matrix[out_i][ s->matrix_ch[out_i][1] ], s->matrix[out_i][ s->matrix_ch[out_i][2] ],
 250                            len);
 251             }else{
 252                 sum2_s16  (out->ch[out_i], in->ch[ s->matrix_ch[out_i][1] ],           in->ch[ s->matrix_ch[out_i][2] ],
 253                                  s->matrix16[out_i][ s->matrix_ch[out_i][1] ], s->matrix16[out_i][ s->matrix_ch[out_i][2] ],
 254                            len);
 255             }
 256             break;
 257         default:
 258             if(s->int_sample_fmt == AV_SAMPLE_FMT_FLT){
 259                 for(i=0; i<len; i++){
 260                     float v=0;
 261                     for(j=0; j<s->matrix_ch[out_i][0]; j++){
 262                         in_i= s->matrix_ch[out_i][1+j];
 263                         v+= ((float*)in->ch[in_i])[i] * s->matrix[out_i][in_i];
 264                     }
 265                     ((float*)out->ch[out_i])[i]= v;
 266                 }
 267             }else{
 268                 for(i=0; i<len; i++){
 269                     int v=0;
 270                     for(j=0; j<s->matrix_ch[out_i][0]; j++){
 271                         in_i= s->matrix_ch[out_i][1+j];
 272                         v+= ((int16_t*)in->ch[in_i])[i] * s->matrix16[out_i][in_i];
 273                     }
 274                     ((int16_t*)out->ch[out_i])[i]= (v + 16384)>>15;
 275                 }
 276             }
 277         }
 278     }
 279     return 0;
 280 }