|
| 1 | +/* |
| 2 | + * Copyright (C) 2024 Linux Studio Plugins Project <https://lsp-plug.in/> |
| 3 | + * (C) 2024 Vladimir Sadovnikov <sadko4u@gmail.com> |
| 4 | + * |
| 5 | + * This file is part of lsp-dsp-lib |
| 6 | + * Created on: 8 мар. 2024 г. |
| 7 | + * |
| 8 | + * lsp-dsp-lib 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-dsp-lib 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 deheads. |
| 17 | + * |
| 18 | + * You should have received a copy of the GNU Lesser General Public License |
| 19 | + * along with lsp-dsp-lib. If not, see <https://www.gnu.org/licenses/>. |
| 20 | + */ |
| 21 | + |
| 22 | + |
| 23 | +#ifndef PRIVATE_DSP_ARCH_GENERIC_CORRELATION_H_ |
| 24 | +#define PRIVATE_DSP_ARCH_GENERIC_CORRELATION_H_ |
| 25 | + |
| 26 | +#ifndef PRIVATE_DSP_ARCH_GENERIC_IMPL |
| 27 | + #error "This header should not be included directly" |
| 28 | +#endif /* PRIVATE_DSP_ARCH_GENERIC_IMPL */ |
| 29 | + |
| 30 | +namespace lsp |
| 31 | +{ |
| 32 | + namespace generic |
| 33 | + { |
| 34 | + void corr_init(dsp::correlation_t *corr, const float *a, const float *b, size_t count) |
| 35 | + { |
| 36 | + float xv = 0.0f; |
| 37 | + float xa = 0.0f; |
| 38 | + float xb = 0.0f; |
| 39 | + |
| 40 | + if (count >= 4) |
| 41 | + { |
| 42 | + float T[4], A[4], B[4]; |
| 43 | + |
| 44 | + T[0] = 0.0f; |
| 45 | + T[1] = 0.0f; |
| 46 | + T[2] = 0.0f; |
| 47 | + T[3] = 0.0f; |
| 48 | + |
| 49 | + A[0] = 0.0f; |
| 50 | + A[1] = 0.0f; |
| 51 | + A[2] = 0.0f; |
| 52 | + A[3] = 0.0f; |
| 53 | + |
| 54 | + B[0] = 0.0f; |
| 55 | + B[1] = 0.0f; |
| 56 | + B[2] = 0.0f; |
| 57 | + B[3] = 0.0f; |
| 58 | + |
| 59 | + for ( ; count >= 4; count -= 4) |
| 60 | + { |
| 61 | + T[0] += a[0] * b[0]; |
| 62 | + T[1] += a[1] * b[1]; |
| 63 | + T[2] += a[2] * b[2]; |
| 64 | + T[3] += a[3] * b[3]; |
| 65 | + |
| 66 | + A[0] += a[0] * a[0]; |
| 67 | + A[1] += a[1] * a[1]; |
| 68 | + A[2] += a[2] * a[2]; |
| 69 | + A[3] += a[3] * a[3]; |
| 70 | + |
| 71 | + B[0] += b[0] * b[0]; |
| 72 | + B[1] += b[1] * b[1]; |
| 73 | + B[2] += b[2] * b[2]; |
| 74 | + B[3] += b[3] * b[3]; |
| 75 | + |
| 76 | + a += 4; |
| 77 | + b += 4; |
| 78 | + } |
| 79 | + |
| 80 | + xv = T[0] + T[1] + T[2] + T[3]; |
| 81 | + xa = A[0] + A[1] + A[2] + A[3]; |
| 82 | + xb = B[0] + B[1] + B[2] + B[3]; |
| 83 | + } |
| 84 | + |
| 85 | + for ( ; count > 0; --count) |
| 86 | + { |
| 87 | + xv += a[0] * b[0]; |
| 88 | + xa += a[0] * a[0]; |
| 89 | + xb += b[0] * b[0]; |
| 90 | + |
| 91 | + a += 1; |
| 92 | + b += 1; |
| 93 | + } |
| 94 | + |
| 95 | + corr->v += xv; |
| 96 | + corr->a += xa; |
| 97 | + corr->b += xb; |
| 98 | + } |
| 99 | + |
| 100 | + void corr_incr(dsp::correlation_t *corr, float *dst, |
| 101 | + const float *a_head, const float *b_head, |
| 102 | + const float *a_tail, const float *b_tail, |
| 103 | + size_t count) |
| 104 | + { |
| 105 | + float T[4], BA[4], BB[4], B[4], DV[4], DA[4], DB[4]; |
| 106 | + |
| 107 | + float vv = corr->v; |
| 108 | + float va = corr->a; |
| 109 | + float vb = corr->b; |
| 110 | + |
| 111 | + for ( ; count >= 4; count -= 4) |
| 112 | + { |
| 113 | + DV[0] = a_head[0]*b_head[0] - a_tail[0]*b_tail[0]; |
| 114 | + DV[1] = a_head[1]*b_head[1] - a_tail[1]*b_tail[1]; |
| 115 | + DV[2] = a_head[2]*b_head[2] - a_tail[2]*b_tail[2]; |
| 116 | + DV[3] = a_head[3]*b_head[3] - a_tail[3]*b_tail[3]; |
| 117 | + |
| 118 | + DA[0] = a_head[0]*a_head[0] - a_tail[0]*a_tail[0]; |
| 119 | + DA[1] = a_head[1]*a_head[1] - a_tail[1]*a_tail[1]; |
| 120 | + DA[2] = a_head[2]*a_head[2] - a_tail[2]*a_tail[2]; |
| 121 | + DA[3] = a_head[3]*a_head[3] - a_tail[3]*a_tail[3]; |
| 122 | + |
| 123 | + DB[0] = b_head[0]*b_head[0] - b_tail[0]*b_tail[0]; |
| 124 | + DB[1] = b_head[1]*b_head[1] - b_tail[1]*b_tail[1]; |
| 125 | + DB[2] = b_head[2]*b_head[2] - b_tail[2]*b_tail[2]; |
| 126 | + DB[3] = b_head[3]*b_head[3] - b_tail[3]*b_tail[3]; |
| 127 | + |
| 128 | + T[0] = vv + DV[0]; |
| 129 | + T[1] = T[0] + DV[1]; |
| 130 | + T[2] = T[1] + DV[2]; |
| 131 | + T[3] = T[2] + DV[3]; |
| 132 | + |
| 133 | + BA[0] = va + DA[0]; |
| 134 | + BA[1] = BA[0] + DA[1]; |
| 135 | + BA[2] = BA[1] + DA[2]; |
| 136 | + BA[3] = BA[2] + DA[3]; |
| 137 | + |
| 138 | + BB[0] = vb + DB[0]; |
| 139 | + BB[1] = BB[0] + DB[1]; |
| 140 | + BB[2] = BB[1] + DB[2]; |
| 141 | + BB[3] = BB[2] + DB[3]; |
| 142 | + |
| 143 | + B[0] = BA[0] * BB[0]; |
| 144 | + B[1] = BA[1] * BB[1]; |
| 145 | + B[2] = BA[2] * BB[2]; |
| 146 | + B[3] = BA[3] * BB[3]; |
| 147 | + |
| 148 | + dst[0] = (B[0] >= 1e-10f) ? T[0] / sqrtf(B[0]) : 0.0f; |
| 149 | + dst[1] = (B[1] >= 1e-10f) ? T[1] / sqrtf(B[1]) : 0.0f; |
| 150 | + dst[2] = (B[2] >= 1e-10f) ? T[2] / sqrtf(B[2]) : 0.0f; |
| 151 | + dst[3] = (B[3] >= 1e-10f) ? T[3] / sqrtf(B[3]) : 0.0f; |
| 152 | + |
| 153 | + vv = T[3]; |
| 154 | + va = BA[3]; |
| 155 | + vb = BB[3]; |
| 156 | + |
| 157 | + a_head += 4; |
| 158 | + b_head += 4; |
| 159 | + a_tail += 4; |
| 160 | + b_tail += 4; |
| 161 | + dst += 4; |
| 162 | + } |
| 163 | + |
| 164 | + for (; count > 0; --count) |
| 165 | + { |
| 166 | + DV[0] = a_head[0]*b_head[0] - a_tail[0]*b_tail[0]; |
| 167 | + DA[0] = a_head[0]*a_head[0] - a_tail[0]*a_tail[0]; |
| 168 | + DB[0] = b_head[0]*b_head[0] - b_tail[0]*b_tail[0]; |
| 169 | + |
| 170 | + T[0] = vv + DV[0]; |
| 171 | + BA[0] = va + DA[0]; |
| 172 | + BB[0] = vb + DB[0]; |
| 173 | + B[0] = BA[0] * BB[0]; |
| 174 | + |
| 175 | + dst[0] = (B[0] >= 1e-10f) ? T[0] / sqrtf(B[0]) : 0.0f; |
| 176 | + |
| 177 | + vv = T[0]; |
| 178 | + va = BA[0]; |
| 179 | + vb = BB[0]; |
| 180 | + |
| 181 | + a_head += 1; |
| 182 | + b_head += 1; |
| 183 | + a_tail += 1; |
| 184 | + b_tail += 1; |
| 185 | + dst += 1; |
| 186 | + } |
| 187 | + |
| 188 | + corr->v = vv; |
| 189 | + corr->a = va; |
| 190 | + corr->b = vb; |
| 191 | + } |
| 192 | + |
| 193 | + } /* namespace generic */ |
| 194 | +} /* namespace lsp */ |
| 195 | + |
| 196 | + |
| 197 | + |
| 198 | +#endif /* PRIVATE_DSP_ARCH_GENERIC_CORRELATION_H_ */ |
0 commit comments