SHS/qad
4
1
Fork 0
Code Issues 5 Pull Requests Releases Wiki Activity
Files
ba8bc27298e206e40ca908d1bbc7c36f674725d8
qad/_oldpip/pip_0.4.0_r5/pimath.cpp
Бычков Андрей ba8bc27298 1 
git-svn-id: svn://db.shs.com.ru/libs@1 a8b55f48-bf90-11e4-a774-851b48703e85
2015-02-28 21:28:53 +00:00

1628 lines
45 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/*
PIP - Platform Independent Primitives
Math
Copyright (C) 2014 Ivan Pelipenko peri4ko@gmail.com
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 3 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, see <http://www.gnu.org/licenses/>.
*/
#include "pimath.h"
double piJ0(const double & v) {
#ifndef PIP_MATH_J0
double x = v;
double xsq;
double nn;
double pzero;
double qzero;
double p1;
double q1;
double result;
if (x < 0) x = -x;
if (x > 8.) {
double xsq_;
double p2;
double q2;
double p3;
double q3;
xsq_ = 64. / (x * x);
p2 = 0.0;
p2 = 2485.271928957404011288128951 + xsq_ * p2;
p2 = 153982.6532623911470917825993 + xsq_ * p2;
p2 = 2016135.283049983642487182349 + xsq_ * p2;
p2 = 8413041.456550439208464315611 + xsq_ * p2;
p2 = 12332384.76817638145232406055 + xsq_ * p2;
p2 = 5393485.083869438325262122897 + xsq_ * p2;
q2 = 1.0;
q2 = 2615.700736920839685159081813 + xsq_ * q2;
q2 = 156001.7276940030940592769933 + xsq_ * q2;
q2 = 2025066.801570134013891035236 + xsq_ * q2;
q2 = 8426449.050629797331554404810 + xsq_ * q2;
q2 = 12338310.22786324960844856182 + xsq_ * q2;
q2 = 5393485.083869438325560444960 + xsq_ * q2;
p3 = -0.0;
p3 = -4.887199395841261531199129300 +xsq_ * p3;
p3 = -226.2630641933704113967255053 +xsq_ * p3;
p3 = -2365.956170779108192723612816 +xsq_ * p3;
p3 = -8239.066313485606568803548860 +xsq_ * p3;
p3 = -10381.41698748464093880530341 +xsq_ * p3;
p3 = -3984.617357595222463506790588 +xsq_ * p3;
q3 = 1.0;
q3 = 408.7714673983499223402830260 + xsq_ * q3;
q3 = 15704.89191515395519392882766 + xsq_ * q3;
q3 = 156021.3206679291652539287109 + xsq_ * q3;
q3 = 533291.3634216897168722255057 + xsq_ * q3;
q3 = 666745.4239319826986004038103 + xsq_ * q3;
q3 = 255015.5108860942382983170882 + xsq_ * q3;
pzero = p2 / q2;
qzero = 8. * p3 / q3 / x;
nn = x- M_PI / 4.;
result = sqrt(2. / M_PI / x) * (pzero * cos(nn) - qzero * sin(nn));
return result;
}
xsq = x * x;
p1 = 26857.86856980014981415848441;
p1 = -40504123.71833132706360663322 + xsq * p1;
p1 = 25071582855.36881945555156435 + xsq * p1;
p1 = -8085222034853.793871199468171 + xsq * p1;
p1 = 1434354939140344.111664316553 + xsq * p1;
p1 = -136762035308817138.6865416609 + xsq * p1;
p1 = 6382059341072356562.289432465 + xsq * p1;
p1 = -117915762910761053603.8440800 + xsq * p1;
p1 = 493378725179413356181.6813446 + xsq * p1;
q1 = 1.;
q1 = 1363.063652328970604442810507 + xsq * q1;
q1 = 1114636.098462985378182402543 + xsq * q1;
q1 = 669998767.2982239671814028660 + xsq * q1;
q1 = 312304311494.1213172572469442 + xsq * q1;
q1 = 112775673967979.8507056031594 + xsq * q1;
q1 = 30246356167094626.98627330784 + xsq * q1;
q1 = 5428918384092285160.200195092 + xsq * q1;
q1 = 493378725179413356211.3278438 + xsq * q1;
return p1 / q1;
#else
return j0(v);
#endif
}
double piJ1(const double & v) {
#ifndef PIP_MATH_J1
double x = v;
double s;
double xsq;
double nn;
double pzero;
double qzero;
double p1;
double q1;
double result;
s = sign(x);
if (x < 0)
x = -x;
if (x > 8.) {
double xsq_;
double p2;
double q2;
double p3;
double q3;
xsq_ = 64.0 / (x * x);
p2 = -1611.616644324610116477412898;
p2 = -109824.0554345934672737413139 + xsq_ * p2;
p2 = -1523529.351181137383255105722 + xsq_ * p2;
p2 = -6603373.248364939109255245434 + xsq_ * p2;
p2 = -9942246.505077641195658377899 + xsq_ * p2;
p2 = -4435757.816794127857114720794 + xsq_ * p2;
q2 = 1.0;
q2 = -1455.009440190496182453565068 + xsq_ * q2;
q2 = -107263.8599110382011903063867 + xsq_ * q2;
q2 = -1511809.506634160881644546358 + xsq_ * q2;
q2 = -6585339.479723087072826915069 + xsq_ * q2;
q2 = -9934124.389934585658967556309 + xsq_ * q2;
q2 = -4435757.816794127856828016962 + xsq_ * q2;
p3 = 35.26513384663603218592175580;
p3 = 1706.375429020768002061283546 + xsq_ * p3;
p3 = 18494.26287322386679652009819 + xsq_ * p3;
p3 = 66178.83658127083517939992166 + xsq_ * p3;
p3 = 85145.16067533570196555001171 + xsq_ * p3;
p3 = 33220.91340985722351859704442 + xsq_ * p3;
q3 = 1.0;
q3 = 863.8367769604990967475517183 + xsq_ * q3;
q3 = 37890.22974577220264142952256 + xsq_ * q3;
q3 = 400294.4358226697511708610813 + xsq_ * q3;
q3 = 1419460.669603720892855755253 + xsq_ * q3;
q3 = 1819458.042243997298924553839 + xsq_ * q3;
q3 = 708712.8194102874357377502472 + xsq_ * q3;
pzero = p2 / q2;
qzero = 8 * p3 / q3 / x;
nn = x - 3 * M_PI / 4;
result = sqrt(2 / M_PI / x) * (pzero * cos(nn) - qzero * sin(nn));
if (s < 0)
result = -result;
return result;
}
xsq = sqr(x);
p1 = 2701.122710892323414856790990;
p1 = -4695753.530642995859767162166 + xsq * p1;
p1 = 3413234182.301700539091292655 + xsq * p1;
p1 = -1322983480332.126453125473247 + xsq * p1;
p1 = 290879526383477.5409737601689 + xsq * p1;
p1 = -35888175699101060.50743641413 + xsq * p1;
p1 = 2316433580634002297.931815435 + xsq * p1;
p1 = -66721065689249162980.20941484 + xsq * p1;
p1 = 581199354001606143928.050809 + xsq * p1;
q1 = 1.0;
q1 = 1606.931573481487801970916749 + xsq * q1;
q1 = 1501793.594998585505921097578 + xsq * q1;
q1 = 1013863514.358673989967045588 + xsq * q1;
q1 = 524371026216.7649715406728642 + xsq * q1;
q1 = 208166122130760.7351240184229 + xsq * q1;
q1 = 60920613989175217.46105196863 + xsq * q1;
q1 = 11857707121903209998.37113348 + xsq * q1;
q1 = 1162398708003212287858.529400 + xsq * q1;
result = s * x * p1 / q1;
return result;
#else
return j1(v);
#endif
}
double piJn(int n, const double & v) {
#ifndef PIP_MATH_JN
double x = v;
double pkm2;
double pkm1;
double pk;
double xk;
double r;
double ans;
int k;
int sg;
double result;
if (n < 0) {
n = -n;
if (n % 2 == 0)
sg = 1;
else
sg = -1;
} else
sg = 1;
if (x < 0) {
if (n % 2 != 0)
sg = -sg;
x = -x;
}
if (n == 0) {
result = sg * piJ0(x);
return result;
}
if (n == 1) {
result = sg * piJ1(x);
return result;
}
if (n == 2) {
if (x == 0)
result = 0;
else
result = sg * (2.0 * piJ1(x) / x - piJ0(x));
return result;
}
if (x < 1E-16) {
result = 0;
return result;
}
k = 53;
pk = 2 * (n + k);
ans = pk;
xk = x * x;
do {
pk = pk - 2.0;
ans = pk - xk / ans;
k = k - 1;
} while (k != 0);
ans = x / ans;
pk = 1.0;
pkm1 = 1.0 / ans;
k = n - 1;
r = 2 * k;
do {
pkm2 = (pkm1 * r - pk * x) / x;
pk = pkm1;
pkm1 = pkm2;
r = r - 2.0;
k = k - 1;
} while (k != 0);
if (fabs(pk) > fabs(pkm1))
ans = piJ1(x) / pk;
else
ans = piJ0(x) / pkm1;
result = sg * ans;
return result;
#else
return jn(n, v);
#endif
}
double piY0(const double & v) {
#ifndef PIP_MATH_Y0
double x = v;
double nn;
double xsq;
double pzero;
double qzero;
double p4;
double q4;
double result;
if (x > 8.) {
double xsq_;
double p2;
double q2;
double p3;
double q3;
xsq_ = 64.0 / (x * x);
p2 = 0.0;
p2 = 2485.271928957404011288128951 + xsq_ * p2;
p2 = 153982.6532623911470917825993 + xsq_ * p2;
p2 = 2016135.283049983642487182349 + xsq_ * p2;
p2 = 8413041.456550439208464315611 + xsq_ * p2;
p2 = 12332384.76817638145232406055 + xsq_ * p2;
p2 = 5393485.083869438325262122897 + xsq_ * p2;
q2 = 1.0;
q2 = 2615.700736920839685159081813 + xsq_ * q2;
q2 = 156001.7276940030940592769933 + xsq_ * q2;
q2 = 2025066.801570134013891035236 + xsq_ * q2;
q2 = 8426449.050629797331554404810 + xsq_ * q2;
q2 = 12338310.22786324960844856182 + xsq_ * q2;
q2 = 5393485.083869438325560444960 + xsq_ * q2;
p3 = -0.0;
p3 = -4.887199395841261531199129300 + xsq_ * p3;
p3 = -226.2630641933704113967255053 + xsq_ * p3;
p3 = -2365.956170779108192723612816 + xsq_ * p3;
p3 = -8239.066313485606568803548860 + xsq_ * p3;
p3 = -10381.41698748464093880530341 + xsq_ * p3;
p3 = -3984.617357595222463506790588 + xsq_ * p3;
q3 = 1.0;
q3 = 408.7714673983499223402830260 + xsq_ * q3;
q3 = 15704.89191515395519392882766 + xsq_ * q3;
q3 = 156021.3206679291652539287109 + xsq_ * q3;
q3 = 533291.3634216897168722255057 + xsq_ * q3;
q3 = 666745.4239319826986004038103 + xsq_ * q3;
q3 = 255015.5108860942382983170882 + xsq_ * q3;
pzero = p2 / q2;
qzero = 8 * p3 / q3 / x;
nn = x - M_PI / 4;
result = sqrt(2 / M_PI / x) * (pzero * sin(nn) + qzero * cos(nn));
return result;
}
xsq = sqr(x);
p4 = -41370.35497933148554125235152;
p4 = 59152134.65686889654273830069 + xsq * p4;
p4 = -34363712229.79040378171030138 + xsq * p4;
p4 = 10255208596863.94284509167421 + xsq * p4;
p4 = -1648605817185729.473122082537 + xsq * p4;
p4 = 137562431639934407.8571335453 + xsq * p4;
p4 = -5247065581112764941.297350814 + xsq * p4;
p4 = 65874732757195549259.99402049 + xsq * p4;
p4 = -27502866786291095837.01933175 + xsq * p4;
q4 = 1.0;
q4 = 1282.452772478993804176329391 + xsq * q4;
q4 = 1001702.641288906265666651753 + xsq * q4;
q4 = 579512264.0700729537480087915 + xsq * q4;
q4 = 261306575504.1081249568482092 + xsq * q4;
q4 = 91620380340751.85262489147968 + xsq * q4;
q4 = 23928830434997818.57439356652 + xsq * q4;
q4 = 4192417043410839973.904769661 + xsq * q4;
q4 = 372645883898616588198.9980 + xsq * q4;
result = p4 / q4 + 2 / M_PI * piJ0(x) * log(x);
return result;
#else
return y0(v);
#endif
}
double piY1(const double & v) {
#ifndef PIP_MATH_Y1
double x = v;
double nn;
double xsq;
double pzero;
double qzero;
double p4;
double q4;
double result;
if (x > 8.) {
double xsq_;
double p2;
double q2;
double p3;
double q3;
xsq_ = 64.0 / (x * x);
p2 = -1611.616644324610116477412898;
p2 = -109824.0554345934672737413139 + xsq_ * p2;
p2 = -1523529.351181137383255105722 + xsq_ * p2;
p2 = -6603373.248364939109255245434 + xsq_ * p2;
p2 = -9942246.505077641195658377899 + xsq_ * p2;
p2 = -4435757.816794127857114720794 + xsq_ * p2;
q2 = 1.0;
q2 = -1455.009440190496182453565068 + xsq_ * q2;
q2 = -107263.8599110382011903063867 + xsq_ * q2;
q2 = -1511809.506634160881644546358 + xsq_ * q2;
q2 = -6585339.479723087072826915069 + xsq_ * q2;
q2 = -9934124.389934585658967556309 + xsq_ * q2;
q2 = -4435757.816794127856828016962 + xsq_ * q2;
p3 = 35.26513384663603218592175580;
p3 = 1706.375429020768002061283546 + xsq_ * p3;
p3 = 18494.26287322386679652009819 + xsq_ * p3;
p3 = 66178.83658127083517939992166 + xsq_ * p3;
p3 = 85145.16067533570196555001171 + xsq_ * p3;
p3 = 33220.91340985722351859704442 + xsq_ * p3;
q3 = 1.0;
q3 = 863.8367769604990967475517183 + xsq_ * q3;
q3 = 37890.22974577220264142952256 + xsq_ * q3;
q3 = 400294.4358226697511708610813 + xsq_ * q3;
q3 = 1419460.669603720892855755253 + xsq_ * q3;
q3 = 1819458.042243997298924553839 + xsq_ * q3;
q3 = 708712.8194102874357377502472 + xsq_ * q3;
pzero = p2 / q2;
qzero = 8 * p3 / q3 / x;
nn = x - 3 * M_PI / 4;
result = sqrt(2 / M_PI / x) * (pzero * sin(nn) + qzero * cos(nn));
return result;
}
xsq = sqr(x);
p4 = -2108847.540133123652824139923;
p4 = 3639488548.124002058278999428 + xsq * p4;
p4 = -2580681702194.450950541426399 + xsq * p4;
p4 = 956993023992168.3481121552788 + xsq * p4;
p4 = -196588746272214065.8820322248 + xsq * p4;
p4 = 21931073399177975921.11427556 + xsq * p4;
p4 = -1212297555414509577913.561535 + xsq * p4;
p4 = 26554738314348543268942.48968 + xsq * p4;
p4 = -99637534243069222259967.44354 + xsq * p4;
q4 = 1.0;
q4 = 1612.361029677000859332072312 + xsq * q4;
q4 = 1563282.754899580604737366452 + xsq * q4;
q4 = 1128686837.169442121732366891 + xsq * q4;
q4 = 646534088126.5275571961681500 + xsq * q4;
q4 = 297663212564727.6729292742282 + xsq * q4;
q4 = 108225825940881955.2553850180 + xsq * q4;
q4 = 29549879358971486742.90758119 + xsq * q4;
q4 = 5435310377188854170800.653097 + xsq * q4;
q4 = 508206736694124324531442.4152 + xsq * q4;
result = x * p4 / q4 + 2 / M_PI * (piJ1(x) * log(x) - 1 / x);
return result;
#else
return y1(v);
#endif
}
double piYn(int n, const double & v) {
#ifndef PIP_MATH_YN
int i;
double x = v;
double a;
double b;
double tmp;
double s;
double result;
s = 1;
if (n < 0) {
n = -n;
if (n % 2 != 0)
s = -1;
}
if (n == 0) {
result = piY0(x);
return result;
}
if (n == 1) {
result = s * piY1(x);
return result;
}
a = piY0(x);
b = piY1(x);
for (i = 1; i <= n - 1; i++) {
tmp = b;
b = 2 * i / x * b - a;
a = tmp;
}
result = s * b;
return result;
#else
return yn(n, v);
#endif
}
double randomn(double dv, double sv) {
static bool agen = false;
double s = 2., v0 = 0., v1 = 0.;
if (agen) {
agen = false;
v1 = v1 * sqrt(-2 * log(s) / s);
return v1 * sv + dv;
}
while (s > 1. || s == 0.) {
v0 = randomd();
v1 = randomd();
s = v0*v0 + v1*v1;
}
v0 = v0 * sqrt(-2 * log(s) / s);
return v0 * sv + dv;
}
const char Solver::methods_desc[] = "b{Methods:}\
\n -1 - Global settings\
\n 01 - Eyler 1\
\n 02 - Eyler 2\
\n 14 - Runge-Kutta 4\
\n 23 - Adams-Bashfort-Moulton 3\
\n 24 - Adams-Bashfort-Moulton 4\
\n 32 - Polynomial Approximation 2\
\n 33 - Polynomial Approximation 3\
\n 34 - Polynomial Approximation 4\
\n 35 - Polynomial Approximation 5";
Solver::Method Solver::method_global = Solver::Eyler_2;
void Solver::solve(double u, double h) {
switch (method) {
case Global:
switch (method_global) {
case Eyler_1: solveEyler1(u, h); break;
case Eyler_2: solveEyler2(u, h); break;
case RungeKutta_4: solveRK4(u, h); break;
case AdamsBashfortMoulton_2: solveABM2(u, h); break;
case AdamsBashfortMoulton_3: solveABM3(u, h); break;
case AdamsBashfortMoulton_4: default: solveABM4(u, h); break;
case PolynomialApproximation_2: solvePA2(u, h); break;
case PolynomialApproximation_3: solvePA3(u, h); break;
case PolynomialApproximation_4: solvePA4(u, h); break;
case PolynomialApproximation_5: solvePA5(u, h); break;
}
break;
case Eyler_1: solveEyler1(u, h); break;
case Eyler_2: solveEyler2(u, h); break;
case RungeKutta_4: solveRK4(u, h); break;
case AdamsBashfortMoulton_2: solveABM2(u, h); break;
case AdamsBashfortMoulton_3: solveABM3(u, h); break;
case AdamsBashfortMoulton_4: default: solveABM4(u, h); break;
case PolynomialApproximation_2: solvePA2(u, h); break;
case PolynomialApproximation_3: solvePA3(u, h); break;
case PolynomialApproximation_4: solvePA4(u, h); break;
case PolynomialApproximation_5: solvePA5(u, h); break;
}
step++;
}
void Solver::fromTF(const TransferFunction & TF) {
if (TF.vector_An.size() >= TF.vector_Bm.size())
size = TF.vector_An.size()-1;
else {
piCout << "Solver error: {A} should be greater than {B}";
return;
}
if (size == 0) return;
step = 0;
times.fill(0.);
A.resize(size, size);
d.resize(size + 1); d.fill(0.);
a1.resize(size + 1); a1.fill(0.);
b1.resize(size + 1); b1.fill(0.);
X.resize(size); X.fill(0.);
F.resize(5);
for (uint i = 0; i < 5; ++i)
F[i].resize(size), F[i].fill(0.);
k1.resize(size); k1.fill(0.);
k2.resize(size); k2.fill(0.);
k3.resize(size); k3.fill(0.);
k4.resize(size); k4.fill(0.);
xx.resize(size); xx.fill(0.);
XX.resize(size); XX.fill(0.);
for (uint i = 0; i < size + 1; ++i)
a1[size - i] = TF.vector_An[i];
for (uint i = 0; i < TF.vector_Bm.size(); ++i)
b1[size - i] = TF.vector_Bm[i];
double a0 = a1[0];
a1 /= a0;
b1 /= a0;
d[0] = b1[0]; // Рассчитываем вектор d
for (uint i = 1; i < size + 1; ++i) {
sum = 0.;
for (uint m = 0; m < i; ++m)
sum += a1[i - m] * d[m];
d[i] = b1[i] - sum;
}
for (uint i = 0; i < size - 1; ++i) // Заполняем матрицу А
for (uint j = 0; j < size; ++j)
A[j][i] = (j == i + 1);
for (uint i = 0; i < size; ++i)
A[i][size - 1] = -a1[size - i];
for (uint i = 0; i < size; ++i)
d[i] = d[i + 1];
}
void Solver::solveEyler1(double u, double h) {
/*for (uint i = 0; i < size; ++i) {
* sum = 0.;
* for (uint j = 0; j < size; ++j)
* sum += A[j][i] * X[j];
* xx[i] = sum + d[i] * u;
}*/
F[0] = A * X + d * u;
X += F[0] * h;
moveF();
}
void Solver::solveEyler2(double u, double h) {
F[0] = A * X + d * u;
X += (F[0] + F[1]) * h / 2.;
moveF();
}
void Solver::solveRK4(double u, double h) {
td = X[0] - F[0][0];
k1 = A * X + d * u; xx = k1 * h / 2.; XX = X + xx;
k2 = A * (XX + k1 * h / 2.) + d * (u + td/3.); xx = k2 * h / 2.; XX += xx;
k3 = A * (XX + k2 * h / 2.) + d * (u + td*2./3.); xx = k3 * h; XX += xx;
k4 = A * (XX + k3 * h) + d * (u + td);
//cout << k1 << k2 << k3 << k4 << endl;
X += (k1 + k2 * 2. + k3 * 2. + k4) * h / 6.;
F[0] = X;
}
void Solver::solveABM2(double u, double h) {
F[0] = A * X + d * u;
XX = X + (F[0] * 3. - F[1]) * (h / 2.);
F[1] = A * XX + d * u;
X += (F[1] + F[0]) * (h / 2.);
moveF();
}
void Solver::solveABM3(double u, double h) {
F[0] = A * X + d * u;
XX = X + (F[0] * 23. - F[1] * 16. + F[2] * 5.) * (h / 12.);
F[2] = A * XX + d * u;
X += (F[2] * 5. + F[0] * 8. - F[1]) * (h / 12.);
moveF();
}
void Solver::solveABM4(double u, double h) {
F[0] = A * X + d * u;
XX = X + (F[0] * 55. - F[1] * 59. + F[2] * 37. - F[3] * 9.) * (h / 24.);
F[3] = A * XX + d * u;
X += (F[3] * 9. + F[0] * 19. - F[1] * 5. + F[2]) * (h / 24.);
moveF();
}
void Solver::solvePA(double u, double h, uint deg) {
F[0] = A * X + d * u;
M.resize(deg, deg);
Y.resize(deg);
pY.resize(deg);
for (uint k = 0; k < size; ++k) {
for (uint i = 0; i < deg; ++i) {
td = 1.;
ct = times[i];
for (uint j = 0; j < deg; ++j) {
M[j][i] = td;
td *= ct;
}
}
for (uint i = 0; i < deg; ++i)
Y[i] = F[i][k];
/// find polynom
//if (step == 1) cout << M << endl << Y << endl;
M.invert(&ok, &Y);
//if (step == 1) cout << Y << endl;
if (!ok) {
solveEyler2(u, h);
break;
}
/// calc last piece
x0 = 0.;
td = 1.;
t = times[0];
for (uint i = 0; i < deg; ++i) {
x0 += Y[i] * td / (i + 1.);
td *= t;
}
x0 *= t;
x1 = 0.;
td = 1.;
t = times[1];
for (uint i = 0; i < deg; ++i) {
x1 += Y[i] * td / (i + 1.);
td *= t;
}
x1 *= t;
lp = x0 - x1;
if (deg > 2) {
/// calc prev piece
x0 = 0.;
td = 1.;
dh = times[1] - times[2];
if (dh != 0. && step > 1) {
t = times[2];
for (uint i = 0; i < deg; ++i) {
x0 += Y[i] * td / (i + 1.);
td *= t;
}
x0 *= t;
ct = x1 - x0;
/// calc correction
ct -= pY[k];
}
/// calc final
X[k] += lp - ct;
pY[k] = lp;
} else {
X[k] += lp;
pY[k] = lp;
}
}
moveF();
}
PIFFT::PIFFT() {
prepared = false;
}
PIVector<complexd> * PIFFT::calcFFT(const PIVector<complexd> & val) {
// for (uint i=0; i<result.size(); i+=2)
// {
// result[i] = val.at(indexes[i]) + val.at(indexes[i+1]);
// result[i+1] = val.at(indexes[i]) - val.at(indexes[i+1]);
// }
// return &result;
result.clear();
if (val.size_s() < 4) return &result;
fftc1d(val, val.size());
return &result;
}
PIVector<complexd> *PIFFT::calcFFTinverse(const PIVector<complexd> &val)
{
result.clear();
if (val.size_s() < 4) return &result;
fftc1dinv(val, val.size());
return &result;
}
PIVector<complexd> *PIFFT::calcHilbert(const PIVector<double> &val)
{
result.clear();
if (val.size_s() < 4) return &result;
fftc1r(val, val.size());
for (uint i=0; i<result.size()/2; i++) result[i] = result[i]*2.;
for (uint i=result.size()/2; i<result.size(); i++) result[i] = 0;
fftc1dinv(result, result.size());
return &result;
}
PIVector< complexd >* PIFFT::calcFFT(const PIVector<double> & val) {
result.clear();
if (val.size_s() < 4) return &result;
fftc1r(val, val.size());
return &result;
}
PIVector<double> PIFFT::getAmplitude() {
PIVector<double> a;
double tmp;
for (uint i=0; i<result.size(); i++) {
tmp = sqrt(result.at(i).real()*result.at(i).real()+result.at(i).imag()*result.at(i).imag());
a.push_back(tmp);
}
return a;
}
void PIFFT::fftc1d(const PIVector<complexd> &a, uint n) {
createPlan(n);
uint i;
PIVector<double> buf;
buf.resize(2*n);
for(i=0; i<n; i++) {
buf[2*i+0] = a.at(i).real();// a->ptr.p_complex[i].x;
buf[2*i+1] = a.at(i).imag();//a->ptr.p_complex[i].y;
}
ftbaseexecuteplan(&buf, 0, n, &curplan);
result.resize(n);
for(i=0; i<n; i++)
result[i]=complexd(buf[2*i+0],buf[2*i+1]);
}
void PIFFT::fftc1r(const PIVector<double> & a, uint n) {
uint i;
if( n%2==0) {
PIVector<double> buf;
uint n2 = n/2;
//buf.resize(n);
buf = a;
createPlan(n2);
//cout << "fftr " << n2 << endl;
ftbaseexecuteplan(&buf, 0, n2, &curplan);
result.resize(n);
uint idx;
complexd hn, hmnc, v;
for(i=0; i<=n2; i++) {
idx = 2*(i%n2);
hn = complexd(buf[idx+0], buf[idx+1]);
idx = 2*((n2-i)%n2);
hmnc = complexd(buf[idx+0], -buf[idx+1]);
v = complexd(sin(M_PI*i/n2), cos(M_PI*i/n2));
result[i] = ((hn + hmnc) - (v * (hn - hmnc)));
result[i] *= 0.5;
}
for(i=n2+1; i<n; i++)
result[i] = conj(result[n-i]);
} else {
PIVector<complexd> cbuf;
cbuf.resize(n);
for(i=0; i<n; i++)
cbuf[i] = complexd(a[i], 0.);
fftc1d(cbuf, n);
}
}
void PIFFT::fftc1dinv(const PIVector<complexd> &a, uint n)
{
PIVector<complexd> cbuf;
cbuf.resize(n);
uint i;
for(i=0; i<n; i++)
{
cbuf[i] = conj(a[i]);
}
fftc1d(cbuf, n);
// result.resize(n);
for(i=0; i<n; i++)
{
result[i] = conj(result[i] / (double)n);
}
}
void PIFFT::createPlan(uint n) {
curplan.plan.clear();
curplan.precomputed.clear();
curplan.stackbuf.clear();
curplan.tmpbuf.clear();
if (n<2) return;
ftbasegeneratecomplexfftplan(n, &curplan);
prepared = true;
}
void PIFFT::ftbasegeneratecomplexfftplan(uint n, ftplan* plan) {
int planarraysize;
int plansize;
int precomputedsize;
int tmpmemsize;
int stackmemsize;
ae_int_t stackptr;
planarraysize = 1;
plansize = 0;
precomputedsize = 0;
stackmemsize = 0;
stackptr = 0;
tmpmemsize = 2*n;
curplan.plan.resize(planarraysize);
int ftbase_ftbasecffttask = 0;
ftbase_ftbasegenerateplanrec(n, ftbase_ftbasecffttask, plan, &plansize, &precomputedsize, &planarraysize, &tmpmemsize, &stackmemsize, stackptr);
if (stackptr!=0) { return;}//ae_assert(stackptr==0, "Internal error in FTBaseGenerateComplexFFTPlan: stack ptr!");
curplan.stackbuf.resize(piMax<int>(stackmemsize,1));//ae_vector_set_length(&curplan.stackbuf, ae_maxint(stackmemsize, 1));
curplan.tmpbuf.resize(piMax<int>(tmpmemsize,1));//ae_vector_set_length(&(curplan.tmpbuf), ae_maxint(tmpmemsize, 1));
curplan.precomputed.resize(piMax<int>(precomputedsize,1));//ae_vector_set_length(&curplan.precomputed, ae_maxint(precomputedsize, 1));
stackptr = 0;
ftbase_ftbaseprecomputeplanrec(plan, 0, stackptr);
if (stackptr!=0) { return;}//ae_assert(stackptr==0, "Internal error in FTBaseGenerateComplexFFTPlan: stack ptr!");
}
/*************************************************************************
Recurrent subroutine for the FFTGeneratePlan:
PARAMETERS:
N plan size
IsReal whether input is real or not.
subroutine MUST NOT ignore this flag because real
inputs comes with non-initialized imaginary parts,
so ignoring this flag will result in corrupted output
HalfOut whether full output or only half of it from 0 to
floor(N/2) is needed. This flag may be ignored if
doing so will simplify calculations
Plan plan array
PlanSize size of used part (in integers)
PrecomputedSize size of precomputed array allocated yet
PlanArraySize plan array size (actual)
TmpMemSize temporary memory required size
BluesteinMemSize temporary memory required size
-- ALGLIB --
Copyright 01.05.2009 by Bochkanov Sergey
*************************************************************************/
void PIFFT::ftbase_ftbasegenerateplanrec(
int n,
int tasktype,
ftplan* plan,
int* plansize,
int* precomputedsize,
int* planarraysize,
int* tmpmemsize,
int* stackmemsize,
ae_int_t stackptr, int debugi)
{
int k, m, n1, n2, esize, entryoffset;
int ftbase_ftbaseplanentrysize = 8;
int ftbase_ftbasecffttask = 0;
int ftbase_fftcooleytukeyplan = 0;
int ftbase_fftbluesteinplan = 1;
int ftbase_fftcodeletplan = 2;
int ftbase_fftrealcooleytukeyplan = 5;
int ftbase_fftemptyplan = 6;
if( *plansize+ftbase_ftbaseplanentrysize>(*planarraysize)) {
curplan.plan.resize(8*(*planarraysize));
*planarraysize = 8*(*planarraysize);
}
entryoffset = *plansize;
esize = ftbase_ftbaseplanentrysize;
*plansize = *plansize+esize;
if( n==1) {
curplan.plan[entryoffset+0] = esize;
curplan.plan[entryoffset+1] = -1;
curplan.plan[entryoffset+2] = -1;
curplan.plan[entryoffset+3] = ftbase_fftemptyplan;
curplan.plan[entryoffset+4] = -1;
curplan.plan[entryoffset+5] = -1;
curplan.plan[entryoffset+6] = -1;
curplan.plan[entryoffset+7] = -1;
return;
}
ftbasefactorize(n, &n1, &n2);
if( n1!=1) {
*tmpmemsize = piMax<int>(*tmpmemsize, 2*n1*n2);
curplan.plan[entryoffset+0] = esize;
curplan.plan[entryoffset+1] = n1;
curplan.plan[entryoffset+2] = n2;
if( tasktype==ftbase_ftbasecffttask)
curplan.plan[entryoffset+3] = ftbase_fftcooleytukeyplan;
else
curplan.plan[entryoffset+3] = ftbase_fftrealcooleytukeyplan;
curplan.plan[entryoffset+4] = 0;
curplan.plan[entryoffset+5] = *plansize;
debugi++;
ftbase_ftbasegenerateplanrec(n1, ftbase_ftbasecffttask, plan, plansize, precomputedsize, planarraysize, tmpmemsize, stackmemsize, stackptr,debugi);
curplan.plan[entryoffset+6] = *plansize;
ftbase_ftbasegenerateplanrec(n2, ftbase_ftbasecffttask, plan, plansize, precomputedsize, planarraysize, tmpmemsize, stackmemsize, stackptr,debugi);
curplan.plan[entryoffset+7] = -1;
return;
} else {
if (n>=2 && n<=5) {
curplan.plan[entryoffset+0] = esize;
curplan.plan[entryoffset+1] = n1;
curplan.plan[entryoffset+2] = n2;
curplan.plan[entryoffset+3] = ftbase_fftcodeletplan;
curplan.plan[entryoffset+4] = 0;
curplan.plan[entryoffset+5] = -1;
curplan.plan[entryoffset+6] = -1;
curplan.plan[entryoffset+7] = *precomputedsize;
if( n==3)
*precomputedsize = *precomputedsize+2;
if( n==5)
*precomputedsize = *precomputedsize+5;
return;
} else {
k = 2*n2-1;
m = ftbasefindsmooth(k);
*tmpmemsize = piMax<int>(*tmpmemsize, 2*m);
curplan.plan[entryoffset+0] = esize;
curplan.plan[entryoffset+1] = n2;
curplan.plan[entryoffset+2] = -1;
curplan.plan[entryoffset+3] = ftbase_fftbluesteinplan;
curplan.plan[entryoffset+4] = m;
curplan.plan[entryoffset+5] = *plansize;
stackptr = stackptr+2*2*m;
*stackmemsize = piMax<int>(*stackmemsize, stackptr);
ftbase_ftbasegenerateplanrec(m, ftbase_ftbasecffttask, plan, plansize, precomputedsize, planarraysize, tmpmemsize, stackmemsize, stackptr);
stackptr = stackptr-2*2*m;
curplan.plan[entryoffset+6] = -1;
curplan.plan[entryoffset+7] = *precomputedsize;
*precomputedsize = *precomputedsize+2*m+2*n;
return;
}
}
}
/*************************************************************************
Recurrent subroutine for precomputing FFT plans
-- ALGLIB --
Copyright 01.05.2009 by Bochkanov Sergey
*************************************************************************/
void PIFFT::ftbase_ftbaseprecomputeplanrec(ftplan* plan,
int entryoffset,
ae_int_t stackptr)
{
int n1, n2, n, m, offs;
double v, bx, by;
int ftbase_fftcooleytukeyplan = 0;
int ftbase_fftbluesteinplan = 1;
int ftbase_fftcodeletplan = 2;
int ftbase_fhtcooleytukeyplan = 3;
int ftbase_fhtcodeletplan = 4;
int ftbase_fftrealcooleytukeyplan = 5;
if( (curplan.plan[entryoffset+3]==ftbase_fftcooleytukeyplan||curplan.plan[entryoffset+3]==ftbase_fftrealcooleytukeyplan)||curplan.plan[entryoffset+3]==ftbase_fhtcooleytukeyplan) {
ftbase_ftbaseprecomputeplanrec(plan, curplan.plan[entryoffset+5], stackptr);
ftbase_ftbaseprecomputeplanrec(plan, curplan.plan[entryoffset+6], stackptr);
return;
}
if( curplan.plan[entryoffset+3]==ftbase_fftcodeletplan||curplan.plan[entryoffset+3]==ftbase_fhtcodeletplan) {
n1 = curplan.plan[entryoffset+1];
n2 = curplan.plan[entryoffset+2];
n = n1*n2;
if( n==3) {
offs = curplan.plan[entryoffset+7];
curplan.precomputed[offs+0] = cos(2*M_PI/3)-1;
curplan.precomputed[offs+1] = sin(2*M_PI/3);
return;
}
if( n==5) {
offs = curplan.plan[entryoffset+7];
v = 2*M_PI/5;
curplan.precomputed[offs+0] = (cos(v)+cos(2*v))/2-1;
curplan.precomputed[offs+1] = (cos(v)-cos(2*v))/2;
curplan.precomputed[offs+2] = -sin(v);
curplan.precomputed[offs+3] = -(sin(v)+sin(2*v));
curplan.precomputed[offs+4] = sin(v)-sin(2*v);
return;
}
}
if( curplan.plan[entryoffset+3]==ftbase_fftbluesteinplan) {
ftbase_ftbaseprecomputeplanrec(plan, curplan.plan[entryoffset+5], stackptr);
n = curplan.plan[entryoffset+1];
m = curplan.plan[entryoffset+4];
offs = curplan.plan[entryoffset+7];
for(int i=0; i<=2*m-1; i++)
curplan.precomputed[offs+i] = 0;
for(int i=0; i<n; i++) {
bx = cos(M_PI*sqr(i)/n);
by = sin(M_PI*sqr(i)/n);
curplan.precomputed[offs+2*i+0] = bx;
curplan.precomputed[offs+2*i+1] = by;
curplan.precomputed[offs+2*m+2*i+0] = bx;
curplan.precomputed[offs+2*m+2*i+1] = by;
if( i>0) {
curplan.precomputed[offs+2*(m-i)+0] = bx;
curplan.precomputed[offs+2*(m-i)+1] = by;
}
}
ftbaseexecuteplanrec(&curplan.precomputed, offs, plan, curplan.plan[entryoffset+5], stackptr);
return;
}
}
void PIFFT::ftbasefactorize(int n, int* n1, int* n2) {
*n1 = *n2 = 0;
int ftbase_ftbasecodeletrecommended = 5;
if( (*n1)*(*n2)!=n) {
for(int j=ftbase_ftbasecodeletrecommended; j>=2; j--) {
if( n%j==0) {
*n1 = j;
*n2 = n/j;
break;
}
}
}
if( (*n1)*(*n2)!=n) {
for(int j=ftbase_ftbasecodeletrecommended+1; j<=n-1; j++) {
if( n%j==0) {
*n1 = j;
*n2 = n/j;
break;
}
}
}
if( (*n1)*(*n2)!=n) {
*n1 = 1;
*n2 = n;
}
if( (*n2)==1 && (*n1)!=1) {
*n2 = *n1;
*n1 = 1;
}
}
/*************************************************************************
Is number smooth?
-- ALGLIB --
Copyright 01.05.2009 by Bochkanov Sergey
*************************************************************************/
void PIFFT::ftbase_ftbasefindsmoothrec(int n, int seed, int leastfactor, int* best) {
if( seed>=n) {
*best = piMin<int>(*best, seed);
return;
}
if( leastfactor<=2)
ftbase_ftbasefindsmoothrec(n, seed*2, 2, best);
if( leastfactor<=3)
ftbase_ftbasefindsmoothrec(n, seed*3, 3, best);
if( leastfactor<=5)
ftbase_ftbasefindsmoothrec(n, seed*5, 5, best);
}
int PIFFT::ftbasefindsmooth(int n) {
int best, result;
best = 2;
while(best<n)
best = 2*best;
ftbase_ftbasefindsmoothrec(n, 1, 2, &best);
result = best;
return result;
}
void PIFFT::ftbase_internalreallintranspose(PIVector<double>* a, int m, int n, int astart, PIVector<double>* buf) {
ftbase_fftirltrec(a, astart, n, buf, 0, m, m, n);
for (int i=0; i<2*m*n; i++) (*a)[astart+i] = (*buf)[i];
}
void PIFFT::ftbase_fftirltrec(PIVector<double>* a, int astart, int astride, PIVector<double>* b, int bstart, int bstride, int m, int n) {
int idx1, idx2;
int m1, n1;
if( m==0||n==0)
return;
if( piMax<int>(m, n)<=8) {
for(int i=0; i<=m-1; i++) {
idx1 = bstart+i;
idx2 = astart+i*astride;
for(int j=0; j<=n-1; j++) {
(*b)[idx1] = a->at(idx2);
idx1 = idx1+bstride;
idx2 = idx2+1;
}
}
return;
}
if( n>m) {
n1 = n/2;
if( n-n1>=8&&n1%8!=0)
n1 = n1+(8-n1%8);
ftbase_fftirltrec(a, astart, astride, b, bstart, bstride, m, n1);
ftbase_fftirltrec(a, astart+n1, astride, b, bstart+n1*bstride, bstride, m, n-n1);
} else {
m1 = m/2;
if( m-m1>=8&&m1%8!=0)
m1 = m1+(8-m1%8);
ftbase_fftirltrec(a, astart, astride, b, bstart, bstride, m1, n);
ftbase_fftirltrec(a, astart+m1*astride, astride, b, bstart+m1, bstride, m-m1, n);
}
}
void PIFFT::ftbase_internalcomplexlintranspose(PIVector<double>* a, int m, int n, int astart, PIVector<double>* buf) {
ftbase_ffticltrec(a, astart, n, buf, 0, m, m, n);
for (int i=0; i<2*m*n; i++)
(*a)[astart+i] = (*buf)[i];
}
void PIFFT::ftbase_ffticltrec(PIVector<double>* a, int astart, int astride, PIVector<double>* b, int bstart, int bstride, int m, int n) {
int idx1, idx2, m2, m1, n1;
if( m==0||n==0)
return;
if( piMax<int>(m, n)<=8) {
m2 = 2*bstride;
for(int i=0; i<=m-1; i++) {
idx1 = bstart+2*i;
idx2 = astart+2*i*astride;
for(int j=0; j<=n-1; j++) {
(*b)[idx1+0] = a->at(idx2+0);
(*b)[idx1+1] = a->at(idx2+1);
idx1 = idx1+m2;
idx2 = idx2+2;
}
}
return;
}
if( n>m) {
n1 = n/2;
if( n-n1>=8&&n1%8!=0)
n1 = n1+(8-n1%8);
ftbase_ffticltrec(a, astart, astride, b, bstart, bstride, m, n1);
ftbase_ffticltrec(a, astart+2*n1, astride, b, bstart+2*n1*bstride, bstride, m, n-n1);
} else {
m1 = m/2;
if( m-m1>=8&&m1%8!=0)
m1 = m1+(8-m1%8);
ftbase_ffticltrec(a, astart, astride, b, bstart, bstride, m1, n);
ftbase_ffticltrec(a, astart+2*m1*astride, astride, b, bstart+2*m1, bstride, m-m1, n);
}
}
void PIFFT::ftbaseexecuteplan(PIVector<double>* a, int aoffset, int n, ftplan* plan) {
ae_int_t stackptr;
stackptr = 0;
ftbaseexecuteplanrec(a, aoffset, plan, 0, stackptr);
}
/*************************************************************************
Recurrent subroutine for the FTBaseExecutePlan
Parameters:
A FFT'ed array
AOffset offset of the FFT'ed part (distance is measured in doubles)
-- ALGLIB --
Copyright 01.05.2009 by Bochkanov Sergey
*************************************************************************/
void PIFFT::ftbaseexecuteplanrec(PIVector<double>* a, int aoffset, ftplan* plan, int entryoffset, ae_int_t stackptr) {
int n1, n2, n, m, offs, offs1, offs2, offsa, offsb, offsp;
double hk, hnk, x, y, bx, by, v0, v1, v2, v3;
double a0x, a0y, a1x, a1y, a2x, a2y, a3x, a3y;
double t1x, t1y, t2x, t2y, t3x, t3y, t4x, t4y, t5x, t5y;
double m1x, m1y, m2x, m2y, m3x, m3y, m4x, m4y, m5x, m5y;
double s1x, s1y, s2x, s2y, s3x, s3y, s4x, s4y, s5x, s5y;
double c1, c2, c3, c4, c5;
int ftbase_fftcooleytukeyplan = 0;
int ftbase_fftbluesteinplan = 1;
int ftbase_fftcodeletplan = 2;
int ftbase_fhtcooleytukeyplan = 3;
int ftbase_fhtcodeletplan = 4;
int ftbase_fftrealcooleytukeyplan = 5;
int ftbase_fftemptyplan = 6;
PIVector<double> & tmpb(curplan.tmpbuf);
if( curplan.plan[entryoffset+3]==ftbase_fftemptyplan)
return;
if( curplan.plan[entryoffset+3]==ftbase_fftcooleytukeyplan) {
n1 = curplan.plan[entryoffset+1];
n2 = curplan.plan[entryoffset+2];
ftbase_internalcomplexlintranspose(a, n1, n2, aoffset, &(curplan.tmpbuf));
for(int i=0; i<=n2-1; i++)
ftbaseexecuteplanrec(a, aoffset+i*n1*2, plan, curplan.plan[entryoffset+5], stackptr);
ftbase_ffttwcalc(a, aoffset, n1, n2);
ftbase_internalcomplexlintranspose(a, n2, n1, aoffset, &(curplan.tmpbuf));
for(int i=0; i<=n1-1; i++)
ftbaseexecuteplanrec(a, aoffset+i*n2*2, plan, curplan.plan[entryoffset+6], stackptr);
ftbase_internalcomplexlintranspose(a, n1, n2, aoffset, &(curplan.tmpbuf));
return;
}
if( curplan.plan[entryoffset+3]==ftbase_fftrealcooleytukeyplan) {
n1 = curplan.plan[entryoffset+1];
n2 = curplan.plan[entryoffset+2];
ftbase_internalcomplexlintranspose(a, n2, n1, aoffset, &(curplan.tmpbuf));
for(int i=0; i<=n1/2-1; i++) {
offs = aoffset+2*i*n2*2;
for(int k=0; k<=n2-1; k++)
(*a)[offs+2*k+1] = (*a)[offs+2*n2+2*k+0];
ftbaseexecuteplanrec(a, offs, plan, curplan.plan[entryoffset+6], stackptr);
tmpb[0] = (*a)[offs+0];
tmpb[1] = 0;
tmpb[2*n2+0] = (*a)[offs+1];
tmpb[2*n2+1] = 0;
for(int k=1; k<=n2-1; k++) {
offs1 = 2*k;
offs2 = 2*n2+2*k;
hk = (*a)[offs+2*k+0];
hnk = (*a)[offs+2*(n2-k)+0];
tmpb[offs1+0] = 0.5*(hk+hnk);
tmpb[offs2+1] = -0.5*(hk-hnk);
hk = (*a)[offs+2*k+1];
hnk = (*a)[offs+2*(n2-k)+1];
tmpb[offs2+0] = 0.5*(hk+hnk);
tmpb[offs1+1] = 0.5*(hk-hnk);
}
for (int i=0; i<2*n2*2; i++) (*a)[offs+i] = tmpb[i];
}
if( n1%2!=0)
ftbaseexecuteplanrec(a, aoffset+(n1-1)*n2*2, plan, curplan.plan[entryoffset+6], stackptr);
ftbase_ffttwcalc(a, aoffset, n2, n1);
ftbase_internalcomplexlintranspose(a, n1, n2, aoffset, &(curplan.tmpbuf));
for(int i=0; i<=n2-1; i++)
ftbaseexecuteplanrec(a, aoffset+i*n1*2, plan, curplan.plan[entryoffset+5], stackptr);
ftbase_internalcomplexlintranspose(a, n2, n1, aoffset, &(curplan.tmpbuf));
return;
}
if( curplan.plan[entryoffset+3]==ftbase_fhtcooleytukeyplan) {
n1 = curplan.plan[entryoffset+1];
n2 = curplan.plan[entryoffset+2];
n = n1*n2;
ftbase_internalreallintranspose(a, n1, n2, aoffset, &(curplan.tmpbuf));
for(int i=0; i<=n2-1; i++)
ftbaseexecuteplanrec(a, aoffset+i*n1, plan, curplan.plan[entryoffset+5], stackptr);
for(int i=0; i<=n2-1; i++) {
for(int j=0; j<=n1-1; j++) {
offsa = aoffset+i*n1;
hk = (*a)[offsa+j];
hnk = (*a)[offsa+(n1-j)%n1];
offs = 2*(i*n1+j);
tmpb[offs+0] = -0.5*(hnk-hk);
tmpb[offs+1] = 0.5*(hk+hnk);
}
}
ftbase_ffttwcalc(&(curplan.tmpbuf), 0, n1, n2);
for(int j=0; j<=n1-1; j++)
(*a)[aoffset+j] = tmpb[2*j+0]+tmpb[2*j+1];
if( n2%2==0) {
offs = 2*(n2/2)*n1;
offsa = aoffset+n2/2*n1;
for(int j=0; j<=n1-1; j++)
(*a)[offsa+j] = tmpb[offs+2*j+0]+tmpb[offs+2*j+1];
}
for(int i=1; i<=(n2+1)/2-1; i++) {
offs = 2*i*n1;
offs2 = 2*(n2-i)*n1;
offsa = aoffset+i*n1;
for(int j=0; j<=n1-1; j++)
(*a)[offsa+j] = tmpb[offs+2*j+1]+tmpb[offs2+2*j+0];
offsa = aoffset+(n2-i)*n1;
for(int j=0; j<=n1-1; j++)
(*a)[offsa+j] = tmpb[offs+2*j+0]+tmpb[offs2+2*j+1];
}
ftbase_internalreallintranspose(a, n2, n1, aoffset, &(curplan.tmpbuf));
for(int i=0; i<=n1-1; i++)
ftbaseexecuteplanrec(a, aoffset+i*n2, plan, curplan.plan[entryoffset+6], stackptr);
ftbase_internalreallintranspose(a, n1, n2, aoffset, &(curplan.tmpbuf));
return;
}
if( curplan.plan[entryoffset+3]==ftbase_fftcodeletplan) {
n1 = curplan.plan[entryoffset+1];
n2 = curplan.plan[entryoffset+2];
n = n1*n2;
if( n==2) {
a0x = (*a)[aoffset+0];
a0y = (*a)[aoffset+1];
a1x = (*a)[aoffset+2];
a1y = (*a)[aoffset+3];
v0 = a0x+a1x;
v1 = a0y+a1y;
v2 = a0x-a1x;
v3 = a0y-a1y;
(*a)[aoffset+0] = v0;
(*a)[aoffset+1] = v1;
(*a)[aoffset+2] = v2;
(*a)[aoffset+3] = v3;
return;
}
if( n==3) {
offs = curplan.plan[entryoffset+7];
c1 = curplan.precomputed[offs+0];
c2 = curplan.precomputed[offs+1];
a0x = (*a)[aoffset+0];
a0y = (*a)[aoffset+1];
a1x = (*a)[aoffset+2];
a1y = (*a)[aoffset+3];
a2x = (*a)[aoffset+4];
a2y = (*a)[aoffset+5];
t1x = a1x+a2x;
t1y = a1y+a2y;
a0x = a0x+t1x;
a0y = a0y+t1y;
m1x = c1*t1x;
m1y = c1*t1y;
m2x = c2*(a1y-a2y);
m2y = c2*(a2x-a1x);
s1x = a0x+m1x;
s1y = a0y+m1y;
a1x = s1x+m2x;
a1y = s1y+m2y;
a2x = s1x-m2x;
a2y = s1y-m2y;
(*a)[aoffset+0] = a0x;
(*a)[aoffset+1] = a0y;
(*a)[aoffset+2] = a1x;
(*a)[aoffset+3] = a1y;
(*a)[aoffset+4] = a2x;
(*a)[aoffset+5] = a2y;
return;
}
if( n==4) {
a0x = (*a)[aoffset+0];
a0y = (*a)[aoffset+1];
a1x = (*a)[aoffset+2];
a1y = (*a)[aoffset+3];
a2x = (*a)[aoffset+4];
a2y = (*a)[aoffset+5];
a3x = (*a)[aoffset+6];
a3y = (*a)[aoffset+7];
t1x = a0x+a2x;
t1y = a0y+a2y;
t2x = a1x+a3x;
t2y = a1y+a3y;
m2x = a0x-a2x;
m2y = a0y-a2y;
m3x = a1y-a3y;
m3y = a3x-a1x;
(*a)[aoffset+0] = t1x+t2x;
(*a)[aoffset+1] = t1y+t2y;
(*a)[aoffset+4] = t1x-t2x;
(*a)[aoffset+5] = t1y-t2y;
(*a)[aoffset+2] = m2x+m3x;
(*a)[aoffset+3] = m2y+m3y;
(*a)[aoffset+6] = m2x-m3x;
(*a)[aoffset+7] = m2y-m3y;
return;
}
if( n==5) {
offs = curplan.plan[entryoffset+7];
c1 = curplan.precomputed[offs+0];
c2 = curplan.precomputed[offs+1];
c3 = curplan.precomputed[offs+2];
c4 = curplan.precomputed[offs+3];
c5 = curplan.precomputed[offs+4];
t1x = (*a)[aoffset+2]+(*a)[aoffset+8];
t1y = (*a)[aoffset+3]+(*a)[aoffset+9];
t2x = (*a)[aoffset+4]+(*a)[aoffset+6];
t2y = (*a)[aoffset+5]+(*a)[aoffset+7];
t3x = (*a)[aoffset+2]-(*a)[aoffset+8];
t3y = (*a)[aoffset+3]-(*a)[aoffset+9];
t4x = (*a)[aoffset+6]-(*a)[aoffset+4];
t4y = (*a)[aoffset+7]-(*a)[aoffset+5];
t5x = t1x+t2x;
t5y = t1y+t2y;
(*a)[aoffset+0] = (*a)[aoffset+0]+t5x;
(*a)[aoffset+1] = (*a)[aoffset+1]+t5y;
m1x = c1*t5x;
m1y = c1*t5y;
m2x = c2*(t1x-t2x);
m2y = c2*(t1y-t2y);
m3x = -c3*(t3y+t4y);
m3y = c3*(t3x+t4x);
m4x = -c4*t4y;
m4y = c4*t4x;
m5x = -c5*t3y;
m5y = c5*t3x;
s3x = m3x-m4x;
s3y = m3y-m4y;
s5x = m3x+m5x;
s5y = m3y+m5y;
s1x = (*a)[aoffset+0]+m1x;
s1y = (*a)[aoffset+1]+m1y;
s2x = s1x+m2x;
s2y = s1y+m2y;
s4x = s1x-m2x;
s4y = s1y-m2y;
(*a)[aoffset+2] = s2x+s3x;
(*a)[aoffset+3] = s2y+s3y;
(*a)[aoffset+4] = s4x+s5x;
(*a)[aoffset+5] = s4y+s5y;
(*a)[aoffset+6] = s4x-s5x;
(*a)[aoffset+7] = s4y-s5y;
(*a)[aoffset+8] = s2x-s3x;
(*a)[aoffset+9] = s2y-s3y;
return;
}
}
if( curplan.plan[entryoffset+3]==ftbase_fhtcodeletplan) {
n1 = curplan.plan[entryoffset+1];
n2 = curplan.plan[entryoffset+2];
n = n1*n2;
if( n==2) {
a0x = (*a)[aoffset+0];
a1x = (*a)[aoffset+1];
(*a)[aoffset+0] = a0x+a1x;
(*a)[aoffset+1] = a0x-a1x;
return;
}
if( n==3) {
offs = curplan.plan[entryoffset+7];
c1 = curplan.precomputed[offs+0];
c2 = curplan.precomputed[offs+1];
a0x = (*a)[aoffset+0];
a1x = (*a)[aoffset+1];
a2x = (*a)[aoffset+2];
t1x = a1x+a2x;
a0x = a0x+t1x;
m1x = c1*t1x;
m2y = c2*(a2x-a1x);
s1x = a0x+m1x;
(*a)[aoffset+0] = a0x;
(*a)[aoffset+1] = s1x-m2y;
(*a)[aoffset+2] = s1x+m2y;
return;
}
if( n==4) {
a0x = (*a)[aoffset+0];
a1x = (*a)[aoffset+1];
a2x = (*a)[aoffset+2];
a3x = (*a)[aoffset+3];
t1x = a0x+a2x;
t2x = a1x+a3x;
m2x = a0x-a2x;
m3y = a3x-a1x;
(*a)[aoffset+0] = t1x+t2x;
(*a)[aoffset+1] = m2x-m3y;
(*a)[aoffset+2] = t1x-t2x;
(*a)[aoffset+3] = m2x+m3y;
return;
}
if( n==5) {
offs = curplan.plan[entryoffset+7];
c1 = curplan.precomputed[offs+0];
c2 = curplan.precomputed[offs+1];
c3 = curplan.precomputed[offs+2];
c4 = curplan.precomputed[offs+3];
c5 = curplan.precomputed[offs+4];
t1x = (*a)[aoffset+1]+(*a)[aoffset+4];
t2x = (*a)[aoffset+2]+(*a)[aoffset+3];
t3x = (*a)[aoffset+1]-(*a)[aoffset+4];
t4x = (*a)[aoffset+3]-(*a)[aoffset+2];
t5x = t1x+t2x;
v0 = (*a)[aoffset+0]+t5x;
(*a)[aoffset+0] = v0;
m2x = c2*(t1x-t2x);
m3y = c3*(t3x+t4x);
s3y = m3y-c4*t4x;
s5y = m3y+c5*t3x;
s1x = v0+c1*t5x;
s2x = s1x+m2x;
s4x = s1x-m2x;
(*a)[aoffset+1] = s2x-s3y;
(*a)[aoffset+2] = s4x-s5y;
(*a)[aoffset+3] = s4x+s5y;
(*a)[aoffset+4] = s2x+s3y;
return;
}
}
if( curplan.plan[entryoffset+3]==ftbase_fftbluesteinplan) {
n = curplan.plan[entryoffset+1];
m = curplan.plan[entryoffset+4];
offs = curplan.plan[entryoffset+7];
for(int i=stackptr+2*n; i<=stackptr+2*m-1; i++)
curplan.stackbuf[i] = 0;
offsp = offs+2*m;
offsa = aoffset;
offsb = stackptr;
for(int i=0; i<n; i++) {
bx = curplan.precomputed[offsp+0];
by = curplan.precomputed[offsp+1];
x = (*a)[offsa+0];
y = (*a)[offsa+1];
curplan.stackbuf[offsb+0] = x*bx-y*(-by);
curplan.stackbuf[offsb+1] = x*(-by)+y*bx;
offsp = offsp+2;
offsa = offsa+2;
offsb = offsb+2;
}
ftbaseexecuteplanrec(&curplan.stackbuf, stackptr, plan, curplan.plan[entryoffset+5], stackptr+2*2*m);
offsb = stackptr;
offsp = offs;
for(int i=0; i<=m-1; i++) {
x = curplan.stackbuf[offsb+0];
y = curplan.stackbuf[offsb+1];
bx = curplan.precomputed[offsp+0];
by = curplan.precomputed[offsp+1];
curplan.stackbuf[offsb+0] = x*bx-y*by;
curplan.stackbuf[offsb+1] = -(x*by+y*bx);
offsb = offsb+2;
offsp = offsp+2;
}
ftbaseexecuteplanrec(&curplan.stackbuf, stackptr, plan, curplan.plan[entryoffset+5], stackptr+2*2*m);
offsb = stackptr;
offsp = offs+2*m;
offsa = aoffset;
for(int i=0; i<n; i++) {
x = curplan.stackbuf[offsb+0]/m;
y = -curplan.stackbuf[offsb+1]/m;
bx = curplan.precomputed[offsp+0];
by = curplan.precomputed[offsp+1];
(*a)[offsa+0] = x*bx-y*(-by);
(*a)[offsa+1] = x*(-by)+y*bx;
offsp = offsp+2;
offsa = offsa+2;
offsb = offsb+2;
}
return;
}
}
/*************************************************************************
Twiddle factors calculation
-- ALGLIB --
Copyright 01.05.2009 by Bochkanov Sergey
*************************************************************************/
void PIFFT::ftbase_ffttwcalc(PIVector<double> * a, int aoffset, int n1, int n2) {
int n, idx, offs;
double x, y, twxm1, twy, twbasexm1, twbasey, twrowxm1, twrowy, tmpx, tmpy, v;
int ftbase_ftbaseupdatetw = 4;
n = n1*n2;
v = -2*M_PI/n;
twbasexm1 = -2*sqr(sin(0.5*v));
twbasey = sin(v);
twrowxm1 = 0;
twrowy = 0;
for(int i=0, j = 0; i<=n2-1; i++) {
twxm1 = 0;
twy = 0;
for(j=0; j<=n1-1; j++) {
idx = i*n1+j;
offs = aoffset+2*idx;
x = (*a)[offs+0];
y = (*a)[offs+1];
tmpx = x*twxm1-y*twy;
tmpy = x*twy+y*twxm1;
(*a)[offs+0] = x+tmpx;
(*a)[offs+1] = y+tmpy;
if( j<n1-1) {
if( j%ftbase_ftbaseupdatetw==0) {
v = -2*M_PI*i*(j+1)/n;
twxm1 = -2*sqr(sin(0.5*v));
twy = sin(v);
} else {
tmpx = twrowxm1+twxm1*twrowxm1-twy*twrowy;
tmpy = twrowy+twxm1*twrowy+twy*twrowxm1;
twxm1 = twxm1+tmpx;
twy = twy+tmpy;
}
}
}
if( i<n2-1) {
if( j%ftbase_ftbaseupdatetw==0) {
v = -2*M_PI*(i+1)/n;
twrowxm1 = -2*sqr(sin(0.5*v));
twrowy = sin(v);
} else {
tmpx = twbasexm1+twrowxm1*twbasexm1-twrowy*twbasey;
tmpy = twbasey+twrowxm1*twbasey+twrowy*twbasexm1;
twrowxm1 = twrowxm1+tmpx;
twrowy = twrowy+tmpy;
}
}
}
}
Reference in New Issue View Git Blame Copy Permalink