Added complex correlation calculation function

lsp-plugins · Nov 10, 2024 · 34f678e · 34f678e
1 parent 8af34f3
commit 34f678e
Show file tree

Hide file tree

Showing 3 changed files with 50 additions and 0 deletions.
diff --git a/include/lsp-plug.in/dsp/common/pcomplex.h b/include/lsp-plug.in/dsp/common/pcomplex.h
@@ -238,4 +238,13 @@ LSP_DSP_LIB_SYMBOL(void, pcomplex_r2c_div2, float *dst, const float *src, size_t
  */
 LSP_DSP_LIB_SYMBOL(void, pcomplex_r2c_rdiv2, float *dst, const float *src, size_t count);
 
+/** Compute complex correlation between two sources and store to the result array
+ *
+ * @param dst_corr array to store normalized correlation
+ * @param src1 set of complex numbers
+ * @param src2 set of complex numbers
+ * @param count count number of elements to process
+ */
+LSP_DSP_LIB_SYMBOL(void, pcomplex_corr, float *dst_corr, const float *src1, const float *src2, size_t count);
+
 #endif /* LSP_PLUG_IN_DSP_COMMON_PCOMPLEX_H_ */
diff --git a/include/private/dsp/arch/generic/pcomplex.h b/include/private/dsp/arch/generic/pcomplex.h
@@ -347,6 +347,46 @@ namespace lsp
                 dst        += 2;
             }
         }
+
+        void pcomplex_corr(float *dst_corr, const float *src1, const float *src2, size_t count)
+        {
+            /*
+             * Complex correlation can be defined as:
+             *   corr = re(norm(S1) * conj(norm(S2)))
+             *
+             * if S1 = a + j*b, S2 = c + j*d, then:
+             *
+             *   corr = re((a + j*b) * (c - j*d)) / (sqrtf(a^2 + b^2) * sqrtf(c^2 + d^2))
+             *
+             * After simplifications:
+             *
+             *   corr = (a*c + b*d) / sqrtf((a^2 + b^2)*(c^2 + d^2))
+             *
+             * In programming terms:
+             *
+             *   den  = (a*a + b*b)*(c*c + d*d)
+             *   nom  = a*c + b*d
+             *   corr = (den > threshold) ? nom / sqrt(den) : 0.0
+             */
+
+            while (count--)
+            {
+                const float a   = src1[0];
+                const float b   = src1[1];
+                const float c   = src2[0];
+                const float d   = src2[1];
+
+                const float den = (a*a + b*b)*(c*c + d*d);
+                const float nom = a*c + b*d;
+
+                *dst_corr       = (den > 1e-20f) ? nom / sqrtf(den) : 0.0f;
+
+                src1           += 2;
+                src2           += 2;
+                ++dst_corr;
+            }
+        }
+
     } /* namespace generic */
 } /* namespace lsp */
 

diff --git a/src/main/generic/generic.cpp b/src/main/generic/generic.cpp
@@ -341,6 +341,7 @@ namespace lsp
             EXPORT1(pcomplex_mod);
             EXPORT1(pcomplex_arg);
             EXPORT1(pcomplex_modarg);
+            EXPORT1(pcomplex_corr);
 
             EXPORT1(pcomplex_c2r_add2);
             EXPORT1(pcomplex_c2r_sub2);