You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
377 lines
11 KiB
377 lines
11 KiB
/* lmath.c |
|
|
|
This file is part of a program that implements a Software-Defined Radio. |
|
|
|
Copyright (C) 2015, 2016, 2023 Warren Pratt, NR0V |
|
Copyright (C) 2024 Edouard Griffiths, F4EXB Adapted to SDRangel |
|
|
|
This program is free software; you can redistribute it and/or |
|
modify it under the terms of the GNU General Public License |
|
as published by the Free Software Foundation; either version 2 |
|
of the License, or (at your option) any later version. |
|
|
|
This program is distributed in the hope that it will be useful, |
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
GNU General Public License for more details. |
|
|
|
You should have received a copy of the GNU General Public License |
|
along with this program; if not, write to the Free Software |
|
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. |
|
|
|
The author can be reached by email at |
|
|
|
warren@wpratt.com |
|
|
|
*/ |
|
|
|
#include "comm.hpp" |
|
#include "bldr.hpp" |
|
|
|
namespace WDSP { |
|
|
|
BLDR::BLDR(int points, int ints) |
|
{ |
|
// for the create function, 'points' and 'ints' are the MAXIMUM values that will be encountered |
|
catxy = new double[2 * points]; |
|
sx.resize(points); |
|
sy.resize(points); |
|
h .resize(ints); |
|
p.resize(ints); |
|
np.resize(ints); |
|
taa.resize(ints); |
|
tab.resize(ints); |
|
tag.resize(ints); |
|
tad.resize(ints); |
|
tbb.resize(ints); |
|
tbg.resize(ints); |
|
tbd.resize(ints); |
|
tgg.resize(ints); |
|
tgd.resize(ints); |
|
tdd.resize(ints); |
|
int nsize = 3 * ints + 1; |
|
int intp1 = ints + 1; |
|
int intm1 = ints - 1; |
|
A .resize(intp1 * intp1); |
|
B .resize(intp1 * intp1); |
|
C .resize(intp1 * intp1); |
|
D .resize(intp1); |
|
E .resize(intp1 * intp1); |
|
F .resize(intm1 * intp1); |
|
G .resize(intp1); |
|
MAT.resize(nsize * nsize); |
|
RHS.resize(nsize); |
|
SLN.resize(nsize); |
|
z .resize(intp1); |
|
zp.resize(intp1); |
|
wrk.resize(nsize); |
|
ipiv.resize(nsize); |
|
} |
|
|
|
BLDR::~BLDR() |
|
{ |
|
delete[]catxy; |
|
} |
|
|
|
void BLDR::flush(int points) |
|
{ |
|
memset(catxy, 0, 2 * points * sizeof(double)); |
|
std::fill(sx.begin(), sx.end(), 0); |
|
std::fill(sy.begin(), sy.end(), 0); |
|
std::fill(h.begin(), h.end(), 0); |
|
std::fill(p.begin(), p.end(), 0); |
|
std::fill(np.begin(), np.end(), 0); |
|
std::fill(taa.begin(), taa.end(), 0); |
|
std::fill(tab.begin(), tab.end(), 0); |
|
std::fill(tag.begin(), tag.end(), 0); |
|
std::fill(tad.begin(), tad.end(), 0); |
|
std::fill(tbb.begin(), tbb.end(), 0); |
|
std::fill(tbg.begin(), tbg.end(), 0); |
|
std::fill(tbd.begin(), tbd.end(), 0); |
|
std::fill(tgg.begin(), tgg.end(), 0); |
|
std::fill(tgd.begin(), tgd.end(), 0); |
|
std::fill(tdd.begin(), tdd.end(), 0); |
|
std::fill(A.begin(), A.end(), 0); |
|
std::fill(B.begin(), B.end(), 0); |
|
std::fill(C.begin(), C.end(), 0); |
|
std::fill(D.begin(), D.end(), 0); |
|
std::fill(E.begin(), E.end(), 0); |
|
std::fill(F.begin(), F.end(), 0); |
|
std::fill(G.begin(), G.end(), 0); |
|
std::fill(MAT.begin(), MAT.end(), 0); |
|
std::fill(RHS.begin(), RHS.end(), 0); |
|
std::fill(SLN.begin(), SLN.end(), 0); |
|
std::fill(z.begin(), z.end(), 0); |
|
std::fill(zp.begin(), zp.end(), 0); |
|
std::fill(wrk.begin(), wrk.end(), 0); |
|
std::fill(ipiv.begin(), ipiv.end(), 0); |
|
} |
|
|
|
int BLDR::fcompare(const void* a, const void* b) |
|
{ |
|
if (*(double*)a < *(double*)b) |
|
return -1; |
|
else if (*(double*)a == *(double*)b) |
|
return 0; |
|
else |
|
return 1; |
|
} |
|
|
|
void BLDR::decomp(int n, std::vector<double>& a, std::vector<int>& piv, int* info, std::vector<double>& wrk) |
|
{ |
|
int i; |
|
int j; |
|
int t_piv; |
|
double m_row; |
|
double mt_row; |
|
double m_col; |
|
double mt_col; |
|
*info = 0; |
|
for (i = 0; i < n; i++) |
|
{ |
|
piv[i] = i; |
|
m_row = 0.0; |
|
for (j = 0; j < n; j++) |
|
{ |
|
mt_row = a[n * i + j]; |
|
if (mt_row < 0.0) mt_row = -mt_row; |
|
if (mt_row > m_row) m_row = mt_row; |
|
} |
|
if (m_row == 0.0) |
|
{ |
|
*info = i; |
|
goto cleanup; |
|
} |
|
wrk[i] = m_row; |
|
} |
|
for (int k = 0; k < n - 1; k++) |
|
{ |
|
j = k; |
|
m_col = a[n * piv[k] + k] / wrk[piv[k]]; |
|
if (m_col < 0) m_col = -m_col; |
|
for (i = k + 1; i < n; i++) |
|
{ |
|
mt_col = a[n * piv[i] + k] / wrk[piv[k]]; |
|
if (mt_col < 0.0) mt_col = -mt_col; |
|
if (mt_col > m_col) |
|
{ |
|
m_col = mt_col; |
|
j = i; |
|
} |
|
} |
|
if (m_col == 0) |
|
{ |
|
*info = -k; |
|
goto cleanup; |
|
} |
|
t_piv = piv[k]; |
|
piv[k] = piv[j]; |
|
piv[j] = t_piv; |
|
for (i = k + 1; i < n; i++) |
|
{ |
|
a[n * piv[i] + k] /= a[n * piv[k] + k]; |
|
for (j = k + 1; j < n; j++) |
|
a[n * piv[i] + j] -= a[n * piv[i] + k] * a[n * piv[k] + j]; |
|
} |
|
} |
|
if (a[n * n - 1] == 0.0) |
|
*info = -n; |
|
cleanup: |
|
return; |
|
} |
|
|
|
void BLDR::dsolve(int n, std::vector<double>& a, std::vector<int>& piv, std::vector<double>& b, std::vector<double>& x) |
|
{ |
|
int j; |
|
int k; |
|
double sum; |
|
|
|
for (k = 0; k < n; k++) |
|
{ |
|
sum = 0.0; |
|
for (j = 0; j < k; j++) |
|
sum += a[n * piv[k] + j] * x[j]; |
|
x[k] = b[piv[k]] - sum; |
|
} |
|
|
|
for (k = n - 1; k >= 0; k--) |
|
{ |
|
sum = 0.0; |
|
for (j = k + 1; j < n; j++) |
|
sum += a[n * piv[k] + j] * x[j]; |
|
x[k] = (x[k] - sum) / a[n * piv[k] + k]; |
|
} |
|
} |
|
|
|
void BLDR::cull(int* n, int ints, std::vector<double>& x, const double* t, double ptol) |
|
{ |
|
int k = 0; |
|
int i = *n; |
|
int ntopint; |
|
int npx; |
|
|
|
while (x[i - 1] > t[ints - 1]) |
|
i--; |
|
ntopint = *n - i; |
|
npx = (int)(ntopint * (1.0 - ptol)); |
|
i = *n; |
|
while ((k < npx) && (x[--i] > t[ints])) |
|
k++; |
|
*n -= k; |
|
} |
|
|
|
void BLDR::execute(int points, const double* x, const double* y, int ints, const double* t, int* info, double* c, double ptol) |
|
{ |
|
double u; |
|
double v; |
|
double alpha; |
|
double beta; |
|
double gamma; |
|
double delta; |
|
int nsize = 3 * ints + 1; |
|
int intp1 = ints + 1; |
|
int intm1 = ints - 1; |
|
int i; |
|
int j; |
|
int k; |
|
int m; |
|
int dinfo; |
|
flush(points); |
|
for (i = 0; i < points; i++) |
|
{ |
|
catxy[2 * i + 0] = x[i]; |
|
catxy[2 * i + 1] = y[i]; |
|
} |
|
qsort(catxy, points, 2 * sizeof(double), fcompare); |
|
for (i = 0; i < points; i++) |
|
{ |
|
sx[i] = catxy[2 * i + 0]; |
|
sy[i] = catxy[2 * i + 1]; |
|
} |
|
cull(&points, ints, sx, t, ptol); |
|
if (points <= 0 || sx[points - 1] > t[ints]) |
|
{ |
|
*info = -1000; |
|
goto cleanup; |
|
} |
|
else *info = 0; |
|
|
|
for (j = 0; j < ints; j++) |
|
h[j] = t[j + 1] - t[j]; |
|
p[0] = 0; |
|
j = 0; |
|
for (i = 0; i < points; i++) |
|
{ |
|
if (sx[i] <= t[j + 1]) |
|
np[j]++; |
|
else |
|
{ |
|
p[++j] = i; |
|
while (sx[i] > t[j + 1]) |
|
p[++j] = i; |
|
np[j] = 1; |
|
} |
|
} |
|
for (i = 0; i < ints; i++) |
|
for (j = p[i]; j < p[i] + np[i]; j++) |
|
{ |
|
u = (sx[j] - t[i]) / h[i]; |
|
v = u - 1.0; |
|
alpha = (2.0 * u + 1.0) * v * v; |
|
beta = u * u * (1.0 - 2.0 * v); |
|
gamma = h[i] * u * v * v; |
|
delta = h[i] * u * u * v; |
|
taa[i] += alpha * alpha; |
|
tab[i] += alpha * beta; |
|
tag[i] += alpha * gamma; |
|
tad[i] += alpha * delta; |
|
tbb[i] += beta * beta; |
|
tbg[i] += beta * gamma; |
|
tbd[i] += beta * delta; |
|
tgg[i] += gamma * gamma; |
|
tgd[i] += gamma * delta; |
|
tdd[i] += delta * delta; |
|
D[i + 0] += 2.0 * sy[j] * alpha; |
|
D[i + 1] += 2.0 * sy[j] * beta; |
|
G[i + 0] += 2.0 * sy[j] * gamma; |
|
G[i + 1] += 2.0 * sy[j] * delta; |
|
} |
|
for (i = 0; i < ints; i++) |
|
{ |
|
A[(i + 0) * intp1 + (i + 0)] += 2.0 * taa[i]; |
|
A[(i + 1) * intp1 + (i + 1)] = 2.0 * tbb[i]; |
|
A[(i + 0) * intp1 + (i + 1)] = 2.0 * tab[i]; |
|
A[(i + 1) * intp1 + (i + 0)] = 2.0 * tab[i]; |
|
B[(i + 0) * intp1 + (i + 0)] += 2.0 * tag[i]; |
|
B[(i + 1) * intp1 + (i + 1)] = 2.0 * tbd[i]; |
|
B[(i + 0) * intp1 + (i + 1)] = 2.0 * tbg[i]; |
|
B[(i + 1) * intp1 + (i + 0)] = 2.0 * tad[i]; |
|
E[(i + 0) * intp1 + (i + 0)] += 2.0 * tgg[i]; |
|
E[(i + 1) * intp1 + (i + 1)] = 2.0 * tdd[i]; |
|
E[(i + 0) * intp1 + (i + 1)] = 2.0 * tgd[i]; |
|
E[(i + 1) * intp1 + (i + 0)] = 2.0 * tgd[i]; |
|
} |
|
for (i = 0; i < intm1; i++) |
|
{ |
|
C[i * intp1 + (i + 0)] = +3.0 * h[i + 1] / h[i]; |
|
C[i * intp1 + (i + 2)] = -3.0 * h[i] / h[i + 1]; |
|
C[i * intp1 + (i + 1)] = -C[i * intp1 + (i + 0)] - C[i * intp1 + (i + 2)]; |
|
F[i * intp1 + (i + 0)] = h[i + 1]; |
|
F[i * intp1 + (i + 1)] = 2.0 * (h[i] + h[i + 1]); |
|
F[i * intp1 + (i + 2)] = h[i]; |
|
} |
|
for (i = 0, k = 0; i < intp1; i++, k++) |
|
{ |
|
for (j = 0, m = 0; j < intp1; j++, m++) |
|
MAT[k * nsize + m] = A[i * intp1 + j]; |
|
for (j = 0, m = intp1; j < intp1; j++, m++) |
|
MAT[k * nsize + m] = B[j * intp1 + i]; |
|
for (j = 0, m = 2 * intp1; j < intm1; j++, m++) |
|
MAT[k * nsize + m] = C[j * intp1 + i]; |
|
RHS[k] = D[i]; |
|
} |
|
for (i = 0, k = intp1; i < intp1; i++, k++) |
|
{ |
|
for (j = 0, m = 0; j < intp1; j++, m++) |
|
MAT[k * nsize + m] = B[i * intp1 + j]; |
|
for (j = 0, m = intp1; j < intp1; j++, m++) |
|
MAT[k * nsize + m] = E[i * intp1 + j]; |
|
for (j = 0, m = 2 * intp1; j < intm1; j++, m++) |
|
MAT[k * nsize + m] = F[j * intp1 + i]; |
|
RHS[k] = G[i]; |
|
} |
|
for (i = 0, k = 2 * intp1; i < intm1; i++, k++) |
|
{ |
|
for (j = 0, m = 0; j < intp1; j++, m++) |
|
MAT[k * nsize + m] = C[i * intp1 + j]; |
|
for (j = 0, m = intp1; j < intp1; j++, m++) |
|
MAT[k * nsize + m] = F[i * intp1 + j]; |
|
for (j = 0, m = 2 * intp1; j < intm1; j++, m++) |
|
MAT[k * nsize + m] = 0.0; |
|
RHS[k] = 0.0; |
|
} |
|
decomp(nsize, MAT, ipiv, &dinfo, wrk); |
|
dsolve(nsize, MAT, ipiv, RHS, SLN); |
|
if (dinfo != 0) |
|
{ |
|
*info = dinfo; |
|
goto cleanup; |
|
} |
|
|
|
for (i = 0; i <= ints; i++) |
|
{ |
|
z[i] = SLN[i]; |
|
zp[i] = SLN[i + ints + 1]; |
|
} |
|
for (i = 0; i < ints; i++) |
|
{ |
|
c[4 * i + 0] = z[i]; |
|
c[4 * i + 1] = zp[i]; |
|
c[4 * i + 2] = -3.0 / (h[i] * h[i]) * (z[i] - z[i + 1]) - 1.0 / h[i] * (2.0 * zp[i] + zp[i + 1]); |
|
c[4 * i + 3] = 2.0 / (h[i] * h[i] * h[i]) * (z[i] - z[i + 1]) + 1.0 / (h[i] * h[i]) * (zp[i] + zp[i + 1]); |
|
} |
|
cleanup: |
|
return; |
|
} |
|
|
|
} // namespace WDSP
|
|
|