1 /* 2 * Copyright (C) 2020 Linux Studio Plugins Project <https://lsp-plug.in/> 3 * (C) 2020 Vladimir Sadovnikov <sadko4u@gmail.com> 4 * 5 * This file is part of lsp-plugins 6 * Created on: 28 сент. 2015 г. 7 * 8 * lsp-plugins is free software: you can redistribute it and/or modify 9 * it under the terms of the GNU Lesser General Public License as published by 10 * the Free Software Foundation, either version 3 of the License, or 11 * any later version. 12 * 13 * lsp-plugins is distributed in the hope that it will be useful, 14 * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 * GNU Lesser General Public License for more details. 17 * 18 * You should have received a copy of the GNU Lesser General Public License 19 * along with lsp-plugins. If not, see <https://www.gnu.org/licenses/>. 20 */ 21 22 #include <dsp/dsp.h> 23 24 #include <plugins/phase_detector.h> 25 #include <core/debug.h> 26 #include <core/colors.h> 27 #include <core/util/Color.h> 28 29 #include <string.h> 30 31 namespace lsp 32 { phase_detector()33 phase_detector::phase_detector() : plugin_t(metadata) 34 { 35 fTimeInterval = DETECT_TIME_DFL; 36 fReactivity = REACT_TIME_DFL; 37 38 vFunction = NULL; 39 vAccumulated = NULL; 40 vNormalized = NULL; 41 42 nMaxVectorSize = 0; 43 nVectorSize = 0; 44 nFuncSize = 0; 45 nBest = 0; 46 nWorst = 0; 47 nSelected = 0; 48 49 nGapSize = 0; 50 nMaxGapSize = 0; 51 nGapOffset = 0; 52 53 vA.nSize = 0; 54 vA.pData = NULL; 55 vB.nSize = 0; 56 vB.pData = NULL; 57 58 fTau = 0.0f; 59 fSelector = SELECTOR_DFL; 60 bBypass = false; 61 62 pIDisplay = NULL; 63 } 64 ~phase_detector()65 phase_detector::~phase_detector() 66 { 67 dropBuffers(); 68 } 69 fillGap(const float * a,const float * b,size_t count)70 size_t phase_detector::fillGap(const float *a, const float *b, size_t count) 71 { 72 lsp_assert(a != NULL); 73 lsp_assert(b != NULL); 74 lsp_assert(vA.pData != NULL); 75 lsp_assert(vB.pData != NULL); 76 77 size_t fill = nMaxGapSize - nGapSize; 78 79 if (fill <= 0) 80 { 81 if (nGapOffset < nGapSize) 82 return 0; 83 84 lsp_assert((nGapSize + vA.nSize) <= (nMaxVectorSize * 3)); 85 lsp_assert((nGapSize + vB.nSize) <= (nMaxVectorSize * 4)); 86 87 dsp::copy(vA.pData, &vA.pData[nGapSize], vA.nSize); 88 dsp::copy(vB.pData, &vB.pData[nGapSize], vB.nSize); 89 nGapSize = 0; 90 nGapOffset = 0; 91 fill = nMaxGapSize; 92 } 93 94 if (count < fill) 95 fill = count; 96 97 lsp_assert((nGapSize + vA.nSize + fill) <= (nMaxVectorSize * 3)); 98 lsp_assert((nGapSize + vB.nSize + fill) <= (nMaxVectorSize * 4)); 99 100 dsp::copy(&vA.pData[vA.nSize + nGapSize], a, fill); 101 dsp::copy(&vB.pData[vB.nSize + nGapSize], b, fill); 102 nGapSize += fill; 103 104 return fill; 105 } 106 update_sample_rate(long sr)107 void phase_detector::update_sample_rate(long sr) 108 { 109 lsp_debug("sample_rate = %ld", sr); 110 /* 111 +---------+---------+---------+ 112 A: | Gap | A Data | Lookup | 113 +---------+---------+---------+ 114 / | 115 +---------+---------+---------+---------+ 116 B: | Gap | B Delay | B Data | Lookup | 117 +---------+---------+---------+---------+ 118 | / 119 +---------+---------+ 120 F: | Correlation funcs | 121 +---------+---------+ 122 */ 123 124 // Cleanup buffers 125 dropBuffers(); 126 127 nMaxVectorSize = millis_to_samples(fSampleRate, DETECT_TIME_MAX); 128 vA.pData = new float[nMaxVectorSize * 3]; 129 vB.pData = new float[nMaxVectorSize * 4]; 130 vFunction = new float[nMaxVectorSize * 2]; 131 vAccumulated = new float[nMaxVectorSize * 2]; 132 vNormalized = new float[nMaxVectorSize * 2]; 133 134 setTimeInterval(fTimeInterval, true); 135 setReactiveInterval(fReactivity); 136 137 clearBuffers(); 138 } 139 update_settings()140 void phase_detector::update_settings() 141 { 142 lsp_debug("update settings sample_rate = %ld", get_sample_rate()); 143 144 bool clear = false; 145 bool old_bypass = bBypass; 146 147 // Read parameters 148 float bypass = vPorts[BYPASS] -> getValue(); 149 float reset = vPorts[RESET] -> getValue(); 150 fSelector = vPorts[SELECTOR] -> getValue(); 151 152 lsp_trace("bypass = %.3f, reset = %.3f, selector=%.3f", bypass, reset, fSelector); 153 bBypass = (bypass >= 0.5f) || (reset >= 0.5f); 154 155 if ((old_bypass != bBypass) && (bBypass)) 156 clear = true; 157 158 if (setTimeInterval(vPorts[TIME]->getValue(), false)) 159 clear = true; 160 setReactiveInterval(vPorts[REACTIVITY]->getValue()); 161 162 if (clear) 163 clearBuffers(); 164 } 165 process(size_t samples)166 void phase_detector::process(size_t samples) 167 { 168 // Store pointers to buffers 169 float *in_a = vPorts[IN_A]->getBuffer<float>(); //reinterpret_cast<float *>(vPorts[IN_A] -> getBuffer()); 170 float *in_b = vPorts[IN_B]->getBuffer<float>(); // reinterpret_cast<float *>(vPorts[IN_B] -> getBuffer()); 171 float *out_a = vPorts[OUT_A]->getBuffer<float>(); // reinterpret_cast<float *>(vPorts[OUT_A] -> getBuffer()); 172 float *out_b = vPorts[OUT_B]->getBuffer<float>(); // reinterpret_cast<float *>(vPorts[OUT_B] -> getBuffer()); 173 mesh_t *mesh = vPorts[FUNCTION]->getBuffer<mesh_t>(); //reinterpret_cast<mesh_t *>(vPorts[FUNCTION]->getBuffer()); 174 175 lsp_assert(in_a != NULL); 176 lsp_assert(in_b != NULL); 177 lsp_assert(out_a != NULL); 178 lsp_assert(out_b != NULL); 179 180 // Bypass the original signal 181 dsp::copy(out_a, in_a, samples); 182 dsp::copy(out_b, in_b, samples); 183 184 if (bBypass) 185 { 186 vPorts[BEST_TIME] -> setValue(0.0f); 187 vPorts[BEST_SAMPLES] -> setValue(0.0f); 188 vPorts[BEST_DISTANCE] -> setValue(0.0f); 189 vPorts[BEST_VALUE] -> setValue(0.0f); 190 191 vPorts[WORST_TIME] -> setValue(0.0f); 192 vPorts[WORST_SAMPLES] -> setValue(0.0f); 193 vPorts[WORST_DISTANCE] -> setValue(0.0f); 194 vPorts[WORST_VALUE] -> setValue(0.0f); 195 196 vPorts[SEL_TIME] -> setValue(0.0f); 197 vPorts[SEL_SAMPLES] -> setValue(0.0f); 198 vPorts[SEL_DISTANCE] -> setValue(0.0f); 199 vPorts[SEL_VALUE] -> setValue(0.0f); 200 201 if ((mesh != NULL) && (mesh->isEmpty())) 202 mesh->data(2, 0); // Set mesh to empty data 203 204 // Always query drawing 205 pWrapper->query_display_draw(); 206 return; 207 } 208 209 // Make calculations 210 while (samples > 0) 211 { 212 ssize_t filled = fillGap(in_a, in_b, samples); 213 samples -= filled; 214 215 // Subtract values 216 while (nGapOffset < nGapSize) 217 { 218 // Make assertions 219 lsp_assert((nGapOffset + nFuncSize) <= (nMaxVectorSize * 4)); 220 lsp_assert(nGapOffset <= (nMaxVectorSize * 3)); 221 lsp_assert((nGapOffset + nVectorSize + nFuncSize) < (nMaxVectorSize * 4)); 222 lsp_assert((nGapOffset + nVectorSize) <= (nMaxVectorSize * 3)); 223 224 // Update function peak values 225 // vFunction[i] = vFunction[i] - vB.pData[i + nGapOffset] * vA.pData[nGapOffset] + 226 // + vB.pData[i + nGapOffset + nVectorSize] * vA.pData[nGapOffset + nVectorSize] 227 dsp::mix_add2(vFunction, 228 &vB.pData[nGapOffset], &vB.pData[nGapOffset + nVectorSize], 229 -vA.pData[nGapOffset], vA.pData[nGapOffset + nVectorSize], 230 nFuncSize); 231 232 233 // Accumulate peak function value 234 // vAccumulated[i] = vAccumulated[i] * (1.0f - fTau) + vFunction * fTau 235 dsp::mix2(vAccumulated, vFunction, 1.0f - fTau, fTau, nFuncSize); 236 237 // Increment gap offset: move to next sample 238 nGapOffset++; 239 } 240 } 241 242 // Now analyze average function in the time 243 size_t best = nVectorSize, worst = nVectorSize; 244 ssize_t sel = nFuncSize * (1.0 - (fSelector + SELECTOR_MAX) / (SELECTOR_MAX - SELECTOR_MIN)); 245 if (sel >= ssize_t(nFuncSize)) 246 sel = nFuncSize - 1; 247 else if (sel < 0) 248 sel = 0; 249 250 dsp::normalize(vNormalized, vAccumulated, nFuncSize); 251 dsp::minmax_index(vNormalized, nFuncSize, &worst, &best); 252 253 // Output values 254 nSelected = ssize_t(nVectorSize - sel); 255 nBest = ssize_t(nVectorSize - best); 256 nWorst = ssize_t(nVectorSize - worst); 257 258 vPorts[BEST_TIME] -> setValue(samples_to_millis(fSampleRate, nBest)); 259 vPorts[BEST_SAMPLES] -> setValue(nBest); 260 vPorts[BEST_DISTANCE] -> setValue(samples_to_centimeters(fSampleRate, SOUND_SPEED_M_S, nBest)); 261 vPorts[BEST_VALUE] -> setValue(vNormalized[best]); 262 263 vPorts[WORST_TIME] -> setValue(samples_to_millis(fSampleRate, nWorst)); 264 vPorts[WORST_SAMPLES] -> setValue(nWorst); 265 vPorts[WORST_DISTANCE] -> setValue(samples_to_centimeters(fSampleRate, SOUND_SPEED_M_S, nWorst)); 266 vPorts[WORST_VALUE] -> setValue(vNormalized[worst]); 267 268 vPorts[SEL_TIME] -> setValue(samples_to_millis(fSampleRate, nSelected)); 269 vPorts[SEL_SAMPLES] -> setValue(nSelected); 270 vPorts[SEL_DISTANCE] -> setValue(samples_to_centimeters(fSampleRate, SOUND_SPEED_M_S, nSelected)); 271 vPorts[SEL_VALUE] -> setValue(vNormalized[sel]); 272 273 // Output mesh if specified 274 if ((mesh != NULL) && (mesh->isEmpty())) 275 { 276 float *x = mesh->pvData[0]; 277 float *y = mesh->pvData[1]; 278 float di = (nFuncSize - 1.0) / MESH_POINTS; 279 float dx = samples_to_millis(fSampleRate, di); 280 ssize_t ci = ssize_t(MESH_POINTS >> 1); 281 282 for (size_t i=0; i<MESH_POINTS; ++i) 283 { 284 *(x++) = dx * (ci - ssize_t(i)); 285 *(y++) = vNormalized[size_t(i * di)]; 286 } 287 288 mesh->data(2, MESH_POINTS); 289 } 290 291 // Always query drawing 292 if (pWrapper != NULL) 293 pWrapper->query_display_draw(); 294 } 295 setTimeInterval(float interval,bool force)296 bool phase_detector::setTimeInterval(float interval, bool force) 297 { 298 lsp_debug("interval = %.3f", interval); 299 300 // Calculate parameters 301 if ((!force) && (fTimeInterval == interval)) 302 return false; 303 304 // Re-calculate buffers 305 fTimeInterval = interval; 306 nVectorSize = (size_t(millis_to_samples(fSampleRate, interval)) >> 2) << 2; // Make number of samples multiple of SSE register size 307 nFuncSize = nVectorSize << 1; 308 vA.nSize = nFuncSize; 309 vB.nSize = nFuncSize + nVectorSize; 310 nMaxGapSize = (nMaxVectorSize * 3) - nFuncSize; // Size of A buffer - size of function 311 nGapSize = 0; 312 nGapOffset = 0; 313 314 // Yep, clear all buffers 315 return true; 316 } 317 setReactiveInterval(float interval)318 void phase_detector::setReactiveInterval(float interval) 319 { 320 lsp_debug("reactivity = %.3f", interval); 321 322 // Calculate Reduction 323 fReactivity = interval; 324 fTau = 1.0f - expf(logf(1.0 - M_SQRT1_2) / seconds_to_samples(fSampleRate, interval)); 325 } 326 clearBuffers()327 void phase_detector::clearBuffers() 328 { 329 lsp_debug("force buffer clear"); 330 lsp_assert(vA.pData != NULL); 331 lsp_assert(vB.pData != NULL); 332 lsp_assert(vFunction != NULL); 333 lsp_assert(vAccumulated != NULL); 334 lsp_assert(vNormalized != NULL); 335 336 dsp::fill_zero(vA.pData, nMaxVectorSize * 3); 337 dsp::fill_zero(vB.pData, nMaxVectorSize * 4); 338 dsp::fill_zero(vFunction, nMaxVectorSize * 2); 339 dsp::fill_zero(vAccumulated, nMaxVectorSize * 2); 340 dsp::fill_zero(vNormalized, nMaxVectorSize * 2); 341 } 342 dropBuffers()343 void phase_detector::dropBuffers() 344 { 345 // Drop previously used buffers 346 if (vA.pData != NULL) 347 { 348 lsp_debug("delete [] vA.pData (%p)", vA.pData); 349 delete [] vA.pData; 350 vA.pData = NULL; 351 } 352 if (vB.pData != NULL) 353 { 354 lsp_debug("delete [] vB.pData (%p)", vB.pData); 355 delete [] vB.pData; 356 vB.pData = NULL; 357 } 358 if (vFunction != NULL) 359 { 360 lsp_debug("delete [] vFunction (%p)", vFunction); 361 delete [] vFunction; 362 vFunction = NULL; 363 } 364 if (vAccumulated != NULL) 365 { 366 lsp_debug("delete [] vAccumulated (%p)", vAccumulated); 367 delete [] vAccumulated; 368 vAccumulated= NULL; 369 } 370 if (vNormalized != NULL) 371 { 372 lsp_debug("delete [] vNormalized (%p)", vNormalized); 373 delete [] vNormalized; 374 vNormalized = NULL; 375 } 376 if (pIDisplay != NULL) 377 { 378 pIDisplay->detroy(); 379 pIDisplay = NULL; 380 } 381 } 382 destroy()383 void phase_detector::destroy() 384 { 385 // Call parent plugin for destroy 386 dropBuffers(); 387 plugin_t::destroy(); 388 } 389 inline_display(ICanvas * cv,size_t width,size_t height)390 bool phase_detector::inline_display(ICanvas *cv, size_t width, size_t height) 391 { 392 // Check proportions 393 if (height > (R_GOLDEN_RATIO * width)) 394 height = R_GOLDEN_RATIO * width; 395 396 // Init canvas 397 if (!cv->init(width, height)) 398 return false; 399 width = cv->width(); 400 height = cv->height(); 401 float cx = width >> 1; 402 float cy = height >> 1; 403 404 // Clear background 405 cv->set_color_rgb((bBypass) ? CV_DISABLED : CV_BACKGROUND); 406 cv->paint(); 407 408 // Draw axis 409 cv->set_line_width(1.0); 410 cv->set_color_rgb(CV_WHITE, 0.5f); 411 cv->line(cx, 0, cx, height); 412 cv->line(0, cy, width, cy); 413 414 // Allocate buffer: t, f(t) 415 pIDisplay = float_buffer_t::reuse(pIDisplay, 2, width); 416 float_buffer_t *b = pIDisplay; 417 if (b == NULL) 418 return false; 419 420 if (!bBypass) 421 { 422 float di = (nFuncSize - 1.0) / width; 423 float dy = cy-2; 424 425 for (size_t i=0; i<width; ++i) 426 { 427 b->v[0][i] = width - i; 428 b->v[1][i] = cy - dy * vNormalized[size_t(i * di)]; 429 } 430 431 // Set color and draw 432 cv->set_color_rgb(CV_MESH); 433 cv->set_line_width(2); 434 cv->draw_lines(b->v[0], b->v[1], width); 435 436 // Draw worst meter 437 cv->set_line_width(1); 438 cv->set_color_rgb(CV_RED); 439 ssize_t point = ssize_t(nVectorSize) - nWorst; 440 float x = width - point/di; 441 float y = cy - dy * vNormalized[point]; 442 cv->line(x, 0, x, height); 443 cv->line(0, y, width, y); 444 445 // Draw best meter 446 cv->set_line_width(1); 447 cv->set_color_rgb(CV_GREEN); 448 point = ssize_t(nVectorSize) - nBest; 449 x = width - point/di; 450 y = cy - dy * vNormalized[point]; 451 cv->line(x, 0, x, height); 452 cv->line(0, y, width, y); 453 } 454 else 455 { 456 for (size_t i=0; i<width; ++i) 457 b->v[0][i] = i; 458 dsp::fill(b->v[1], cy, width); 459 460 // Set color and draw 461 cv->set_color_rgb(CV_SILVER); 462 cv->set_line_width(2); 463 cv->draw_lines(b->v[0], b->v[1], width); 464 } 465 466 return true; 467 } 468 469 } /* namespace ddb */ 470 471 472