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) / float(destSampleCount); 48 register float current = 0.0f; 49 50 #define SRC(n) *(const float*)(src + n * srcSampleOffset) 51 52 if (delta < 1.0) { 53 // upsample 54 while (count--) { 55 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current) ; 56 dest += destSampleOffset; 57 current += delta; 58 if (current >= 1.0f) { 59 current -= 1.0f; 60 src += srcSampleOffset; 61 } 62 } 63 } else { 64 // downsample 65 while (count--) { 66 double skipcount; 67 register float offset = modf(current, &skipcount); 68 *(float*)dest = gain 69 * (SRC(0) + (SRC((int)skipcount) - SRC(0)) * offset); 70 dest += destSampleOffset; 71 current += delta - skipcount; 72 src += (int)skipcount * srcSampleOffset; 73 } 74 } 75 } 76 77 78 void 79 Interpolate::int32_to_float(const void *_src, int32 srcSampleOffset, 80 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 81 int32 destSampleCount, float _gain) 82 { 83 register const char * src = (const char *)_src; 84 register char * dest = (char *)_dest; 85 register int32 count = destSampleCount; 86 register float gain = _gain / 2147483647.0; 87 88 if (srcSampleCount == destSampleCount) { 89 // optimized case for no resampling 90 while (count--) { 91 *(float *)dest = *(const int32 *)src * gain; 92 src += srcSampleOffset; 93 dest += destSampleOffset; 94 } 95 return; 96 } 97 98 register float delta = float(srcSampleCount) / float(destSampleCount); 99 register float current = 0.0f; 100 101 #undef SRC 102 #define SRC(n) *(const int32*)(src + n * srcSampleOffset) 103 104 if (delta < 1.0) { 105 // upsample 106 while (count--) { 107 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current) ; 108 dest += destSampleOffset; 109 current += delta; 110 if (current >= 1.0f) { 111 current -= 1.0f; 112 src += srcSampleOffset; 113 } 114 } 115 } else { 116 // downsample 117 while (count--) { 118 double skipcount; 119 register float offset = modf(current, &skipcount); 120 *(float*)dest = gain * (SRC(0) + (SRC((int)skipcount) - SRC(0)) 121 * offset); 122 dest += destSampleOffset; 123 current += delta - skipcount; 124 src += (int)skipcount * srcSampleOffset; 125 } 126 } 127 } 128 129 130 void 131 Interpolate::int16_to_float(const void *_src, int32 srcSampleOffset, 132 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 133 int32 destSampleCount, float _gain) 134 { 135 register const char * src = (const char *)_src; 136 register char * dest = (char *)_dest; 137 register int32 count = destSampleCount; 138 register float gain = _gain / 32767.0; 139 140 if (srcSampleCount == destSampleCount) { 141 // optimized case for no resampling 142 while (count--) { 143 *(float *)dest = *(const int16 *)src * gain; 144 src += srcSampleOffset; 145 dest += destSampleOffset; 146 } 147 return; 148 } 149 150 register float delta = float(srcSampleCount) / float(destSampleCount); 151 register float current = 0.0f; 152 153 #undef SRC 154 #define SRC(n) *(const int16*)(src + n * srcSampleOffset) 155 156 if (delta < 1.0) { 157 // upsample 158 while (count--) { 159 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current); 160 dest += destSampleOffset; 161 current += delta; 162 if (current >= 1.0f) { 163 current -= 1.0f; 164 src += srcSampleOffset; 165 } 166 } 167 } else { 168 // downsample 169 while (count--) { 170 double skipcount; 171 register float offset = modf(current, &skipcount); 172 *(float*)dest = gain * (SRC(0) + (SRC((int)skipcount) - SRC(0)) 173 * offset); 174 dest += destSampleOffset; 175 current += delta - skipcount; 176 src += (int)skipcount * srcSampleOffset; 177 } 178 } 179 } 180 181 182 void 183 Interpolate::int8_to_float(const void *_src, int32 srcSampleOffset, 184 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 185 int32 destSampleCount, float _gain) 186 { 187 register const char * src = (const char *)_src; 188 register char * dest = (char *)_dest; 189 register int32 count = destSampleCount; 190 register float gain = _gain / 127.0; 191 192 if (srcSampleCount == destSampleCount) { 193 // optimized case for no resampling 194 while (count--) { 195 *(float *)dest = *(const int8 *)src * gain; 196 src += srcSampleOffset; 197 dest += destSampleOffset; 198 } 199 return; 200 } 201 202 register float delta = float(srcSampleCount) / float(destSampleCount); 203 register float current = 0.0f; 204 205 #undef SRC 206 #define SRC(n) *(const int8*)(src + n * srcSampleOffset) 207 208 if (delta < 1.0) { 209 // upsample 210 while (count--) { 211 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current) ; 212 dest += destSampleOffset; 213 current += delta; 214 if (current >= 1.0f) { 215 current -= 1.0f; 216 src += srcSampleOffset; 217 } 218 } 219 } else { 220 // downsample 221 while (count--) { 222 double skipcount; 223 register float offset = modf(current, &skipcount); 224 *(float*)dest = gain * (SRC(0) + (SRC((int)skipcount) - SRC(0)) 225 * offset); 226 dest += destSampleOffset; 227 current += delta - skipcount; 228 src += (int)skipcount * srcSampleOffset; 229 } 230 } 231 } 232 233 234 void 235 Interpolate::uint8_to_float(const void *_src, int32 srcSampleOffset, 236 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 237 int32 destSampleCount, float _gain) 238 { 239 register const char * src = (const char *)_src; 240 register char * dest = (char *)_dest; 241 register int32 count = destSampleCount; 242 register float gain = _gain / 127.0; 243 244 if (srcSampleCount == destSampleCount) { 245 // optimized case for no resampling 246 while (count--) { 247 *(float *)dest = (((int32) *(const uint8 *)src) - 128) * gain; 248 src += srcSampleOffset; 249 dest += destSampleOffset; 250 } 251 return; 252 } 253 254 register float delta = float(srcSampleCount) / float(destSampleCount); 255 register float current = 0.0f; 256 257 #undef SRC 258 #define SRC(n) ( *(const uint8*)(src + n * srcSampleOffset) - 128) 259 260 if (delta < 1.0) { 261 // upsample 262 while (count--) { 263 *(float*)dest = gain * (SRC(0) + (SRC(1) - SRC(0)) * current); 264 dest += destSampleOffset; 265 current += delta; 266 if (current >= 1.0f) { 267 current -= 1.0f; 268 src += srcSampleOffset; 269 } 270 } 271 } else { 272 // downsample 273 while (count--) { 274 double skipcount; 275 register float offset = modf(current, &skipcount); 276 *(float*)dest = gain * (SRC(0) + (SRC((int)skipcount) - SRC(0)) 277 * offset); 278 dest += destSampleOffset; 279 current += delta - skipcount; 280 src += (int)skipcount * srcSampleOffset; 281 } 282 } 283 } 284 285 286 void 287 Interpolate::float_to_int32(const void *_src, int32 srcSampleOffset, 288 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 289 int32 destSampleCount, float _gain) 290 { 291 register const char * src = (const char *)_src; 292 register char * dest = (char *)_dest; 293 register int32 count = destSampleCount; 294 register float gain = _gain * 2147483647.0; 295 296 if (srcSampleCount == destSampleCount) { 297 // optimized case for no resampling 298 while (count--) { 299 register float sample = *(const float *)src * gain; 300 if (sample > 2147483647.0f) 301 *(int32 *)dest = 2147483647L; 302 else if (sample < -2147483647.0f) 303 *(int32 *)dest = -2147483647L; 304 else 305 *(int32 *)dest = (int32)sample; 306 src += srcSampleOffset; 307 dest += destSampleOffset; 308 } 309 return; 310 } 311 312 register float delta = float(srcSampleCount) / float(destSampleCount); 313 register float current = 0.0f; 314 315 #undef SRC 316 #define SRC(n) *(const float*)(src + n * srcSampleOffset) 317 318 if (delta < 1.0) { 319 // upsample 320 while (count--) { 321 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 322 * current); 323 if (sample > 2147483647.0f) 324 *(int32 *)dest = 2147483647L; 325 else if (sample < -2147483647.0f) 326 *(int32 *)dest = -2147483647L; 327 else 328 *(int32 *)dest = (int32)sample; 329 dest += destSampleOffset; 330 current += delta; 331 if (current >= 1.0f) { 332 current -= 1.0f; 333 src += srcSampleOffset; 334 } 335 } 336 } else { 337 // downsample 338 while (count--) { 339 double skipcount; 340 register float offset = modf(current, &skipcount); 341 register float sample = gain * (SRC(0) + (SRC((int)skipcount) 342 - SRC(0)) * offset); 343 if (sample > 2147483647.0f) 344 *(int32 *)dest = 2147483647L; 345 else if (sample < -2147483647.0f) 346 *(int32 *)dest = -2147483647L; 347 else 348 *(int32 *)dest = (int32)sample; 349 dest += destSampleOffset; 350 current += delta - skipcount; 351 src += (int)skipcount * srcSampleOffset; 352 } 353 } 354 } 355 356 357 void 358 Interpolate::float_to_int16(const void *_src, int32 srcSampleOffset, 359 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 360 int32 destSampleCount, float _gain) 361 { 362 register const char * src = (const char *)_src; 363 register char * dest = (char *)_dest; 364 register int32 count = destSampleCount; 365 register float gain = _gain * 32767.0; 366 367 if (srcSampleCount == destSampleCount) { 368 // optimized case for no resampling 369 while (count--) { 370 register float sample = *(const float *)src * gain; 371 if (sample > 32767.0f) 372 *(int16 *)dest = 32767; 373 else if (sample < -32767.0f) 374 *(int16 *)dest = -32767; 375 else 376 *(int16 *)dest = (int16)sample; 377 src += srcSampleOffset; 378 dest += destSampleOffset; 379 } 380 return; 381 } 382 383 register float delta = float(srcSampleCount) / float(destSampleCount); 384 register float current = 0.0f; 385 386 if (delta < 1.0) { 387 // upsample 388 while (count--) { 389 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 390 * current); 391 if (sample > 32767.0f) 392 *(int16 *)dest = 32767; 393 else if (sample < -32767.0f) 394 *(int16 *)dest = -32767; 395 else 396 *(int16 *)dest = (int16)sample; 397 dest += destSampleOffset; 398 current += delta; 399 if (current >= 1.0f) { 400 current -= 1.0f; 401 src += srcSampleOffset; 402 } 403 } 404 } else { 405 // downsample 406 while (count--) { 407 double skipcount; 408 register float offset = modf(current, &skipcount); 409 register float sample = gain * (SRC(0) + (SRC((int)skipcount) 410 - SRC(0)) * offset); 411 if (sample > 32767.0f) 412 *(int16 *)dest = 32767; 413 else if (sample < -32767.0f) 414 *(int16 *)dest = -32767; 415 else 416 *(int16 *)dest = (int16)sample; 417 dest += destSampleOffset; 418 current += delta - skipcount; 419 src += (int)skipcount * srcSampleOffset; 420 } 421 } 422 } 423 424 425 void 426 Interpolate::float_to_int8(const void *_src, int32 srcSampleOffset, 427 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 428 int32 destSampleCount, float _gain) 429 { 430 register const char * src = (const char *)_src; 431 register char * dest = (char *)_dest; 432 register int32 count = destSampleCount; 433 register float gain = _gain * 127.0; 434 435 if (srcSampleCount == destSampleCount) { 436 // optimized case for no resampling 437 while (count--) { 438 register float sample = *(const float *)src * gain; 439 if (sample > 127.0f) 440 *(int8 *)dest = 127; 441 else if (sample < -127.0f) 442 *(int8 *)dest = -127; 443 else 444 *(int8 *)dest = (int8)sample; 445 src += srcSampleOffset; 446 dest += destSampleOffset; 447 } 448 return; 449 } 450 451 register float delta = float(srcSampleCount) / float(destSampleCount); 452 register float current = 0.0f; 453 454 if (delta < 1.0) { 455 // upsample 456 while (count--) { 457 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 458 * current); 459 if (sample > 127.0f) 460 *(int8 *)dest = 127; 461 else if (sample < -127.0f) 462 *(int8 *)dest = -127; 463 else 464 *(int8 *)dest = (int8)sample; 465 dest += destSampleOffset; 466 current += delta; 467 if (current >= 1.0f) { 468 current -= 1.0f; 469 src += srcSampleOffset; 470 } 471 } 472 } else { 473 // downsample 474 while (count--) { 475 double skipcount; 476 register float offset = modf(current, &skipcount); 477 register float sample = gain * (SRC(0) + (SRC((int)skipcount) 478 - SRC(0)) * offset); 479 if (sample > 127.0f) 480 *(int8 *)dest = 127; 481 else if (sample < -127.0f) 482 *(int8 *)dest = -127; 483 else 484 *(int8 *)dest = (int8)sample; 485 dest += destSampleOffset; 486 current += delta - skipcount; 487 src += (int)skipcount * srcSampleOffset; 488 } 489 } 490 } 491 492 493 void 494 Interpolate::float_to_uint8(const void *_src, int32 srcSampleOffset, 495 int32 srcSampleCount, void *_dest, int32 destSampleOffset, 496 int32 destSampleCount, float _gain) 497 { 498 register const char * src = (const char *)_src; 499 register char * dest = (char *)_dest; 500 register int32 count = destSampleCount; 501 register float gain = _gain * 127.0; 502 503 if (srcSampleCount == destSampleCount) { 504 // optimized case for no resampling 505 while (count--) { 506 register float sample = 128.0f + *(const float *)src * gain; 507 if (sample > 255.0f) 508 *(uint8 *)dest = 255; 509 else if (sample < 1.0f) 510 *(uint8 *)dest = 1; 511 else 512 *(uint8 *)dest = (uint8)sample; 513 src += srcSampleOffset; 514 dest += destSampleOffset; 515 } 516 return; 517 } 518 519 register float delta = float(srcSampleCount) / float(destSampleCount); 520 register float current = 0.0f; 521 522 if (delta < 1.0) { 523 // upsample 524 while (count--) { 525 register float sample = gain * (SRC(0) + (SRC(1) - SRC(0)) 526 * current); 527 if (sample > 255.0f) 528 *(uint8 *)dest = 255; 529 else if (sample < 1.0f) 530 *(uint8 *)dest = 1; 531 else 532 *(uint8 *)dest = (uint8)sample; 533 dest += destSampleOffset; 534 current += delta; 535 if (current >= 1.0f) { 536 current -= 1.0f; 537 src += srcSampleOffset; 538 } 539 } 540 } else { 541 // downsample 542 while (count--) { 543 double skipcount; 544 register float offset = modf(current, &skipcount); 545 register float sample = gain * (SRC(0) + (SRC((int)skipcount) 546 - SRC(0)) * offset) + 128.0f; 547 if (sample > 255.0f) 548 *(uint8 *)dest = 255; 549 else if (sample < 1.0f) 550 *(uint8 *)dest = 1; 551 else 552 *(uint8 *)dest = (uint8)sample; 553 dest += destSampleOffset; 554 current += delta - skipcount; 555 src += (int)skipcount * srcSampleOffset; 556 } 557 } 558 } 559 560