1 /* 2 * Copyright 2010 Adrien Destugues <pulkomandy@pulkomandy.ath.cx> 3 * Distributed under the terms of the MIT Licence. 4 */ 5 6 7 #include "Interpolate.h" 8 9 #include <cmath> 10 11 12 /*! Resampling class doing linear interpolation. 13 */ 14 15 16 Interpolate::Interpolate(uint32 src_format, uint32 dst_format) 17 : 18 Resampler(src_format, dst_format) 19 { 20 } 21 22 Interpolate::~Interpolate() 23 { 24 } 25 26 27 void 28 Interpolate::float_to_float(const void *_src, int32 srcSampleOffset, 29 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 30 int32 destSampleCount, float _gain) 31 { 32 register const char * src = (const char *)_src; 33 register char * dest = (char *)_dest; 34 register int32 count = destSampleCount; 35 register float gain = _gain; 36 37 if (srcSampleCount == destSampleCount) { 38 // optimized case for no resampling 39 while (count--) { 40 *(float *)dest = *(const float *)src * gain; 41 src += srcSampleOffset; 42 dest += destSampleOffset; 43 } 44 return; 45 } 46 47 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 48 register float current = 0.0f; 49 50 #define SRC(n) *(const float*)(src + n * srcSampleOffset) 51 52 while (--count) { 53 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current) ; 54 dest += destSampleOffset; 55 current += delta; 56 if (current >= 1.0f) { 57 double ipart; 58 current = modf(current, &ipart); 59 src += srcSampleOffset * (int)ipart; 60 } 61 } 62 63 *(float*)dest = SRC(0) * gain; 64 } 65 66 67 void 68 Interpolate::int32_to_float(const void *_src, int32 srcSampleOffset, 69 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 70 int32 destSampleCount, float _gain) 71 { 72 register const char * src = (const char *)_src; 73 register char * dest = (char *)_dest; 74 register int32 count = destSampleCount; 75 register float gain = _gain / 2147483647.0; 76 77 if (srcSampleCount == destSampleCount) { 78 // optimized case for no resampling 79 while (count--) { 80 *(float *)dest = *(const int32 *)src * gain; 81 src += srcSampleOffset; 82 dest += destSampleOffset; 83 } 84 return; 85 } 86 87 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 88 register float current = 0.0f; 89 90 #undef SRC 91 #define SRC(n) *(const int32*)(src + n * srcSampleOffset) 92 93 while (--count) { 94 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current) ; 95 dest += destSampleOffset; 96 current += delta; 97 if (current >= 1.0f) { 98 double ipart; 99 current = modf(current, &ipart); 100 src += srcSampleOffset * (int)ipart; 101 } 102 } 103 104 *(float*)dest = SRC(0) * gain; 105 } 106 107 108 void 109 Interpolate::int16_to_float(const void *_src, int32 srcSampleOffset, 110 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 111 int32 destSampleCount, float _gain) 112 { 113 register const char * src = (const char *)_src; 114 register char * dest = (char *)_dest; 115 register int32 count = destSampleCount; 116 register float gain = _gain / 32767.0; 117 118 if (srcSampleCount == destSampleCount) { 119 // optimized case for no resampling 120 while (count--) { 121 *(float *)dest = *(const int16 *)src * gain; 122 src += srcSampleOffset; 123 dest += destSampleOffset; 124 } 125 return; 126 } 127 128 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 129 register float current = 0.0f; 130 131 #undef SRC 132 #define SRC(n) *(const int16*)(src + n * srcSampleOffset) 133 134 while (--count) { 135 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current); 136 dest += destSampleOffset; 137 current += delta; 138 if (current >= 1.0f) { 139 double ipart; 140 current = modf(current, &ipart); 141 src += srcSampleOffset * (int)ipart; 142 } 143 } 144 145 *(float*)dest = SRC(0) * gain; 146 } 147 148 149 void 150 Interpolate::int8_to_float(const void *_src, int32 srcSampleOffset, 151 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 152 int32 destSampleCount, float _gain) 153 { 154 register const char * src = (const char *)_src; 155 register char * dest = (char *)_dest; 156 register int32 count = destSampleCount; 157 register float gain = _gain / 127.0; 158 159 if (srcSampleCount == destSampleCount) { 160 // optimized case for no resampling 161 while (count--) { 162 *(float *)dest = *(const int8 *)src * gain; 163 src += srcSampleOffset; 164 dest += destSampleOffset; 165 } 166 return; 167 } 168 169 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 170 register float current = 0.0f; 171 172 #undef SRC 173 #define SRC(n) *(const int8*)(src + n * srcSampleOffset) 174 175 while (--count) { 176 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current) ; 177 dest += destSampleOffset; 178 current += delta; 179 if (current >= 1.0f) { 180 double ipart; 181 current = modf(current, &ipart); 182 src += srcSampleOffset * (int)ipart; 183 } 184 } 185 186 *(float*)dest = SRC(0) * gain; 187 } 188 189 190 void 191 Interpolate::uint8_to_float(const void *_src, int32 srcSampleOffset, 192 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 193 int32 destSampleCount, float _gain) 194 { 195 register const char * src = (const char *)_src; 196 register char * dest = (char *)_dest; 197 register int32 count = destSampleCount; 198 register float gain = _gain / 127.0; 199 200 if (srcSampleCount == destSampleCount) { 201 // optimized case for no resampling 202 while (count--) { 203 *(float *)dest = (((int32) *(const uint8 *)src) - 128) * gain; 204 src += srcSampleOffset; 205 dest += destSampleOffset; 206 } 207 return; 208 } 209 210 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 211 register float current = 0.0f; 212 213 #undef SRC 214 #define SRC(n) ( *(const uint8*)(src + n * srcSampleOffset) - 128) 215 216 while (--count) { 217 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current); 218 dest += destSampleOffset; 219 current += delta; 220 if (current >= 1.0f) { 221 double ipart; 222 current = modf(current, &ipart); 223 src += srcSampleOffset * (int)ipart; 224 } 225 } 226 227 *(float*)dest = SRC(0) * gain; 228 } 229 230 231 void 232 Interpolate::float_to_int32(const void *_src, int32 srcSampleOffset, 233 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 234 int32 destSampleCount, float _gain) 235 { 236 register const char * src = (const char *)_src; 237 register char * dest = (char *)_dest; 238 register int32 count = destSampleCount; 239 register float gain = _gain * 2147483647.0; 240 241 if (srcSampleCount == destSampleCount) { 242 // optimized case for no resampling 243 while (count--) { 244 register float sample = *(const float *)src * gain; 245 if (sample > 2147483647.0f) 246 *(int32 *)dest = 2147483647L; 247 else if (sample < -2147483647.0f) 248 *(int32 *)dest = -2147483647L; 249 else 250 *(int32 *)dest = (int32)sample; 251 src += srcSampleOffset; 252 dest += destSampleOffset; 253 } 254 return; 255 } 256 257 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 258 register float current = 0.0f; 259 260 #undef SRC 261 #define SRC(n) *(const float*)(src + n * srcSampleOffset) 262 263 while (--count) { 264 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 265 * current); 266 if (sample > 2147483647.0f) 267 *(int32 *)dest = 2147483647L; 268 else if (sample < -2147483647.0f) 269 *(int32 *)dest = -2147483647L; 270 else 271 *(int32 *)dest = (int32)sample; 272 dest += destSampleOffset; 273 current += delta; 274 if (current >= 1.0f) { 275 double ipart; 276 current = modf(current, &ipart); 277 src += srcSampleOffset * (int)ipart; 278 } 279 } 280 281 register float sample = SRC(0)*gain; 282 if (sample > 2147483647.0f) 283 *(int32 *)dest = 2147483647L; 284 else if (sample < -2147483647.0f) 285 *(int32 *)dest = -2147483647L; 286 else 287 *(int32 *)dest = (int32)sample; 288 } 289 290 291 void 292 Interpolate::float_to_int16(const void *_src, int32 srcSampleOffset, 293 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 294 int32 destSampleCount, float _gain) 295 { 296 register const char * src = (const char *)_src; 297 register char * dest = (char *)_dest; 298 register int32 count = destSampleCount; 299 register float gain = _gain * 32767.0; 300 301 if (srcSampleCount == destSampleCount) { 302 // optimized case for no resampling 303 while (count--) { 304 register float sample = *(const float *)src * gain; 305 if (sample > 32767.0f) 306 *(int16 *)dest = 32767; 307 else if (sample < -32767.0f) 308 *(int16 *)dest = -32767; 309 else 310 *(int16 *)dest = (int16)sample; 311 src += srcSampleOffset; 312 dest += destSampleOffset; 313 } 314 return; 315 } 316 317 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 318 register float current = 0.0f; 319 320 while (--count) { 321 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 322 * current); 323 if (sample > 32767.0f) 324 *(int16 *)dest = 32767; 325 else if (sample < -32767.0f) 326 *(int16 *)dest = -32767; 327 else 328 *(int16 *)dest = (int16)sample; 329 dest += destSampleOffset; 330 current += delta; 331 if (current >= 1.0f) { 332 double ipart; 333 current = modf(current, &ipart); 334 src += srcSampleOffset * (int)ipart; 335 } 336 } 337 338 register float sample = SRC(0) * gain; 339 if (sample > 32767.0f) 340 *(int16 *)dest = 32767; 341 else if (sample < -32767.0f) 342 *(int16 *)dest = -32767; 343 else 344 *(int16 *)dest = (int16)sample; 345 } 346 347 348 void 349 Interpolate::float_to_int8(const void *_src, int32 srcSampleOffset, 350 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 351 int32 destSampleCount, float _gain) 352 { 353 register const char * src = (const char *)_src; 354 register char * dest = (char *)_dest; 355 register int32 count = destSampleCount; 356 register float gain = _gain * 127.0; 357 358 if (srcSampleCount == destSampleCount) { 359 // optimized case for no resampling 360 while (count--) { 361 register float sample = *(const float *)src * gain; 362 if (sample > 127.0f) 363 *(int8 *)dest = 127; 364 else if (sample < -127.0f) 365 *(int8 *)dest = -127; 366 else 367 *(int8 *)dest = (int8)sample; 368 src += srcSampleOffset; 369 dest += destSampleOffset; 370 } 371 return; 372 } 373 374 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 375 register float current = 0.0f; 376 377 while (--count) { 378 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 379 * current); 380 if (sample > 127.0f) 381 *(int8 *)dest = 127; 382 else if (sample < -127.0f) 383 *(int8 *)dest = -127; 384 else 385 *(int8 *)dest = (int8)sample; 386 dest += destSampleOffset; 387 current += delta; 388 if (current >= 1.0f) { 389 double ipart; 390 current = modf(current, &ipart); 391 src += srcSampleOffset * (int)ipart; 392 } 393 } 394 395 register float sample = SRC(0) * gain; 396 if (sample > 127.0f) 397 *(int8 *)dest = 127; 398 else if (sample < -127.0f) 399 *(int8 *)dest = -127; 400 else 401 *(int8 *)dest = (int8)sample; 402 } 403 404 405 void 406 Interpolate::float_to_uint8(const void *_src, int32 srcSampleOffset, 407 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 408 int32 destSampleCount, float _gain) 409 { 410 register const char * src = (const char *)_src; 411 register char * dest = (char *)_dest; 412 register int32 count = destSampleCount; 413 register float gain = _gain * 127.0; 414 415 if (srcSampleCount == destSampleCount) { 416 // optimized case for no resampling 417 while (count--) { 418 register float sample = 128.0f + *(const float *)src * gain; 419 if (sample > 255.0f) 420 *(uint8 *)dest = 255; 421 else if (sample < 1.0f) 422 *(uint8 *)dest = 1; 423 else 424 *(uint8 *)dest = (uint8)sample; 425 src += srcSampleOffset; 426 dest += destSampleOffset; 427 } 428 return; 429 } 430 431 register float delta = float(srcSampleCount - 1) / float(destSampleCount - 1); 432 register float current = 0.0f; 433 434 while (--count) { 435 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 436 * current); 437 if (sample > 255.0f) 438 *(uint8 *)dest = 255; 439 else if (sample < 1.0f) 440 *(uint8 *)dest = 1; 441 else 442 *(uint8 *)dest = (uint8)sample; 443 dest += destSampleOffset; 444 current += delta; 445 if (current >= 1.0f) { 446 double ipart; 447 current = modf(current, &ipart); 448 src += srcSampleOffset * (int)ipart; 449 } 450 } 451 452 register float sample = SRC(0) * gain; 453 if (sample > 255.0f) 454 *(uint8 *)dest = 255; 455 else if (sample < 1.0f) 456 *(uint8 *)dest = 1; 457 else 458 *(uint8 *)dest = (uint8)sample; 459 } 460 461