C++ while loop using bisection method. Help on break - c++

I need some help here. Please excuse the complexity of the code. Basically, I am looking to use the bisection method to find a value "Theta" and each i increment.
I know that all the calculations work fine when I know the Theta, and I have the code run to just simply calculate all the values, but when I introduce a while loop and the bisection method to have the code approximate Theta, I can't seem to get it to run correctly. I am assuming I have my while loop set up incorrectly....
#include <math.h>
#include <iostream>
#include <vector>
#include <iomanip>
#include <algorithm> // std::max
using namespace std;
double FuncM(double theta, double r, double F, double G, double Gprime, double d_t, double sig);
double FuncM(double theta, double r, double F, double G, double Gprime, double d_t, double sig)
{
double eps = 0.0001;
return ((log(max((r + (theta + F - 0.5 * G * Gprime ) * d_t), eps))) / sig);
}
double FuncJSTAR(double m, double x_0, double d_x);
double FuncJSTAR(double m, double x_0, double d_x)
{
return (int(((m - x_0) / d_x)+ 0.5));
}
double FuncCN(double m, double x_0, double j, double d_x);
double FuncCN(double m, double x_0, double j, double d_x)
{
return (m - x_0 - j * d_x);
}
double FuncPup(double d_t, double cn, double d_x);
double FuncPup(double d_t, double cn, double d_x)
{
return (((d_t + pow(cn, 2.0)) / (2.0 * pow(d_x, 2.0))) + (cn / (2.0 * d_x)));
}
double FuncPdn(double d_t, double cn, double d_x);
double FuncPdn(double d_t, double cn, double d_x)
{
return (((d_t + pow(cn, 2.0)) / (2.0 * pow(d_x, 2.0))) - (cn / (2.0 * d_x)));
}
double FuncPmd(double pd, double pu);
double FuncPmd(double pd, double pu)
{
return (1 - pu - pd);
}
int main()
{
const int Maturities = 5;
const double EPS = 0.00001;
double TermStructure[Maturities][2] = {
{0.5 , 0.05},
{1.0 , 0.06},
{1.5 , 0.07},
{2.0 , 0.075},
{3.0 , 0.085} };
//--------------------------------------------------------------------------------------------------------
vector<double> Price(Maturities);
double Initial_Price = 1.00;
for (int i = 0; i < Maturities; i++)
{
Price[i] = Initial_Price * exp(-TermStructure[i][1] * TermStructure[i][0]);
}
//--------------------------------------------------------------------------------------------------------
int j_max = 8;
int j_range = ((j_max * 2) + 1);
//--------------------------------------------------------------------------------------------------------
// Set up vector of possible j values
vector<int> j_value(j_range);
for (int j = 0; j < j_range; j++)
{
j_value[j] = j_max - j;
}
//--------------------------------------------------------------------------------------------------------
double dt = 0.5;
double dx = sqrt(3 * dt);
double sigma = 0.15;
double mean_reversion = 0.2; // "a" value
//--------------------------------------------------------------------------------------------------------
double r0 = TermStructure[0][1]; // Initialise r(0) in case no corresponding dt rate in term structure
//--------------------------------------------------------------------------------------------------------
double x0 = log(r0) / sigma;
//--------------------------------------------------------------------------------------------------------
vector<double> r_j(j_range); // rate at each j
vector<double> F_r(j_range);
vector<double> G_r(j_range);
vector<double> G_prime_r(j_range);
for(int j = 0; j < j_range; j++)
{
if (j == j_max)
{
r_j[j] = r0;
}
else
{
r_j[j] = exp((x0 + j_value[j]*dx) * sigma);
}
F_r[j] = -mean_reversion * r_j[j];
G_r[j] = sigma * r_j[j];
G_prime_r[j] = sigma;
}
//--------------------------------------------------------------------------------------------------------
vector<vector<double>> m((j_range), vector<double>(Maturities));
vector<vector<int>> j_star((j_range), vector<int>(Maturities));
vector<vector<double>> Central_Node((j_range), vector<double>(Maturities));
vector<double> Theta(Maturities - 1);
vector<vector<double>> Pu((j_range), vector<double>(Maturities));
vector<vector<double>> Pd((j_range), vector<double>(Maturities));
vector<vector<double>> Pm((j_range), vector<double>(Maturities));
vector<vector<double>> Q((j_range), vector<double>(Maturities));// = {}; // Arrow Debreu Price. Initialised all array values to 0
vector<double> Q_dt_sum(Maturities);// = {}; // Sum of Arrow Debreu Price at each time step. Initialised all array values to 0
//--------------------------------------------------------------------------------------------------------
double Theta_A, Theta_B, Theta_C;
int JSTART;
int JEND;
int TempStart;
int TempEnd;
int max;
int min;
vector<vector<int>> Up((j_range), vector<int>(Maturities));
vector<vector<int>> Down((j_range), vector<int>(Maturities));
// Theta[0] = 0.0498039349327417;
// Theta[1] = 0.0538710670441647;
// Theta[2] = 0.0181648634139392;
// Theta[3] = 0.0381183886467521;
for(int i = 0; i < (Maturities-1); i++)
{
Theta_A = 0.00;
Theta_B = TermStructure[i][1];
Q_dt_sum[0] = Initial_Price;
Q_dt_sum[i+1] = 0.0;
while (fabs(Theta_A - Theta_B) >= 0.0000001)
{
max = 1;
min = 10;
if (i == 0)
{
JSTART = j_max;
JEND = j_max;
}
else
{
JSTART = TempStart;
JEND = TempEnd;
}
for(int j = JSTART; j >= JEND; j--)
{
Theta_C = (Theta_A + Theta_B) / 2.0; // If Theta C is too low, the associated Price will be higher than Price from initial term structure. (ie P(Theta C) > P(i+2) for Theta C < Theta)
// If P_C > P(i+2), set Theta_B = Theta_C, else if P_C < P(i+2), set Theta_A = Theta_C, Else if P_C = P(i+2), Theta_C = Theta[i]
//cout << Theta_A << " " << Theta_B << " " << Theta_C << endl;
m[j][i] = FuncM(Theta[i], r_j[j], F_r[j], G_r[j], G_prime_r[j], dt, sigma);
j_star[j][i] = FuncJSTAR(m[j][i], x0, dx);
Central_Node[j][i] = FuncCN(m[j][i], x0, j_star[j][i], dx);
Pu[j][i] = FuncPup(dt, Central_Node[j][i], dx);
Pd[j][i] = FuncPdn(dt, Central_Node[j][i], dx);
Pm[j][i] = FuncPmd(Pd[j][i], Pu[j][i]);
for (int p = 0; p < j_range; p++)
{
Q[p][i] = 0; // Clear Q array
}
Q[j_max][0] = Initial_Price;
Q[j_max -(j_star[j][i]+1)][i+1] = Q[j_max - (j_star[j][i]+1)][i+1] + Q[j][i] * Pu[j][i] * exp(-r_j[j] * dt);
Q[j_max -(j_star[j][i] )][i+1] = Q[j_max - (j_star[j][i] )][i+1] + Q[j][i] * Pm[j][i] * exp(-r_j[j] * dt);
Q[j_max -(j_star[j][i]-1)][i+1] = Q[j_max - (j_star[j][i]-1)][i+1] + Q[j][i] * Pd[j][i] * exp(-r_j[j] * dt);
}
for (int j = 0; j < j_range; j++)
{
Up[j][i] = j_star[j][i] + 1;
Down[j][i] = j_star[j][i] - 1;
if (Up[j][i] > max)
{
max = Up[j][i];
}
if ((Down[j][i] < min) && (Down[j][i] > 0))
{
min = Down[j][i];
}
}
TempEnd = j_max - (max);
TempStart = j_max - (min);
for (int j = 0; j < j_range; j++)
{
Q_dt_sum[i+1] = Q_dt_sum[i+1] + Q[j][i] * exp(-r_j[j] * dt);
cout << Q_dt_sum[i+1] << endl;
}
if (Q_dt_sum[i+1] == Price[i+2])
{
Theta[i] = Theta_C;
break;
}
if (Q_dt_sum[i+1] > Price[i+2])
{
Theta_B = Theta_C;
}
else if (Q_dt_sum[i+1] < Price[i+2])
{
Theta_A = Theta_C;
}
}
cout << Theta[i] << endl;
}
return 0;
}

Ok, my bad. I had a value being called incorrectly.
All good.

Related

C++ compiles and runs fine but I can't generate any types of C++ outputs after certain point in the code

The code starts with declaring various arrays with a size that is pre-calculated, and will be used in the rest of the program. However, after a certain point in the list of declarations, C++ will fail to generate any output even after a successful compilation. After the comment in the middle of the code, no outputs can be generated. I have tried simple outputs like "cout" and writing in a file.
Edit: I have added a sample output written by one of the answers to demonstrate. The program just runs and does not generate anything. This is the terminal output:
"
PS C:\Users\umroot.COLLAR\projects\CrackHole> g++ .\Peridynamics.cpp -o peri
PS C:\Users\umroot.COLLAR\projects\CrackHole> .\peri.exe
PS C:\Users\umroot.COLLAR\projects\CrackHole>
#include <math.h>
#include <iostream>
#include <vector>
#include <string>
#include <conio.h>
// #include "Ellipse.h"
#include <fstream>
using namespace std;
int main () {
float length = 0.5;
float width = 0.5;
float radiusMajor = 0.05;
float radiusMinor = 0.05;
double ellipseCurvature = radiusMinor * radiusMinor / radiusMajor;
float radiusPath = 0.08;
int dt = 1;
const double ELASTIC_MODULUS = 200e9;
const float POISSON_RATIO = 0.3;
const int NumofDiv_x = 100;
const int NumofDiv_y = 100;
int timeInterval = 2500;
const double appliedPressure = 500e7;
int initialTotalNumMatPoint = NumofDiv_x * NumofDiv_y;
int maxFam = 200;
float dx = length / NumofDiv_x;
float delta = 3.015 * dx;
float thick = dx;
float volCorrRadius = dx / 2;
const double SHEAR_MODULUS = ELASTIC_MODULUS / (2 * (1 + POISSON_RATIO));
const double BULK_MODULUS = ELASTIC_MODULUS / (2 * (1 - POISSON_RATIO));
const double ALPHA = 0.5 * (BULK_MODULUS - 2 * SHEAR_MODULUS);
float area = dx * dx;
float volume = area * thick;
const float BCD = 2 / (M_PI * thick * pow(delta, 4));
int temp = floor(9 * M_PI * initialTotalNumMatPoint);
float nodeFam[100000][3] = {0.0};
int nnum = 0;
float coord_excess[initialTotalNumMatPoint][2] = {0.0};
int path_horizontal[NumofDiv_x] = {0};
// Ellipse centerHole(0, 0, radiusMajor, radiusMinor);
// Ellipse leftTip((-1) * radiusMajor, 0, 0.005, 0.005);
// Ellipse rightTip(radiusMajor, 0, 0.005, 0.005);
float coordx = 0.0;
float coordy = 0.0;
int counter = 0;
for (int i = 0; i < NumofDiv_x; i++) {
for (int j = 0; j < NumofDiv_y; j++) {
coordx = (length / 2) * (-1) + (dx / 2) + i * dx;
coordy = (width / 2) * (-1) + (dx/2) + j * dx;
// if (centerHole.InEllipse(coordx, coordy)){
// continue;
// }
if (abs(coordy) <= dx && coordx >= 0) {
path_horizontal[counter] = nnum;
counter++;
}
coord_excess[nnum][0] = coordx;
coord_excess[nnum][1] = coordy;
nnum++;
}
}
int totalNumMatPoint = nnum;
float coord[totalNumMatPoint][2] = {0.0};
for (int j = 0; j < 2; j++ ) {
for (int i = 0; i < totalNumMatPoint; i++) {
coord[i][j] = coord_excess[i][j];
}
}
int numFam[totalNumMatPoint] = {0};
int pointFam[totalNumMatPoint] = {0};
float PDForce[totalNumMatPoint][2] = {0.0};
float bodyForce[totalNumMatPoint][2] = {0.0};
float PDforceold[totalNumMatPoint][2] = {0.0};
float PD_SED_Distortion[totalNumMatPoint][2] = {0.0};
float surCorrFactorDilatation[totalNumMatPoint][2] = {0.0};
float surCorrFactorDistorsion[totalNumMatPoint][2] = {0.0};
float disp[totalNumMatPoint][2] = {0.0};
float totalDisp[totalNumMatPoint][2] = {0.0};
float vel[totalNumMatPoint][2] = {0.0};
// AFTER THIS POINT DOWNWARDS, NO OUTPUTS WILL BE GENERATED
float velhalfold[totalNumMatPoint][2] = {0.0};
float velhalf[totalNumMatPoint][2] = {0.0};
float massvec[totalNumMatPoint][2] = {0.0};
float PD_SED_Dilatation[totalNumMatPoint][2] = {0.0};
float PD_SED_Dilatation_Fixed[totalNumMatPoint][2] = {0.0};
int checkTime[timeInterval] = {0};
float steadyCheck_x[timeInterval] = {0.0};
float steadyCheck_y[timeInterval] = {0.0};
float relPositionVector = 0.0;
for (int j = 0; j < 2; j++ ) {
for (int i = 0; i < totalNumMatPoint; i++) {
coord[i][j] = coord_excess[i][j];
std::cout << coord[i][j] << std::endl;
}
}
Your code, as is, is not "outputting" anything. I compiled and ran your code and added std::cout statements below and above your comment "AFTER THIS POINT DOWNWARDS, NO OUTPUTS WILL BE GENERATED". This successfully writes to stdout.
If, for example, you wanted to output all the values in the coords array you could do something like this while you are building it:
for (int j = 0; j < 2; j++ ) {
for (int i = 0; i < totalNumMatPoint; i++) {
coord[i][j] = coord_excess[i][j];
std::cout << coord[i][j] << std::endl;
}
}
I used another PC with a different OS (i.e. Ubuntu) and it is running fine. Not sure what the problem was. Probably something run with my compiler and/or editor on the first computer.

How to to find smallest (optimized) distance between two vectors in C++

I'm translating Python's version of 'page_dewarper' (https://mzucker.github.io/2016/08/15/page-dewarping.html) into C++. I'm going to use dlib, which is a fantastic tool, that helped me in a few optimization problems before. In line 748 of Github repo (https://github.com/mzucker/page_dewarp/blob/master/page_dewarp.py) Matt uses optimize function from Scipy, to find the minimal distance between two vectors. I think, my C++ equivalent should be solve_least_squares_lm() or solve_least_squares(). I'll give a concrete example to analyze.
My data:
a) dstpoints is a vector with OpenCV points - std::vector<cv::Point2f> (I have 162 points in this example, they are not changing),
b) ppts is also std::vector<cv::Point2f> and the same size as dstpoints.
std::vector<cv::Point2f> ppts = project_keypoints(params, input);
It is dependent on:
- dlib::column_vector 'input' is 2*162=324 long and is not changing,
- dlib::column_vector 'params' is 189 long and its values should be changed to get the minimal value of variable 'suma', something like this:
double suma = 0.0;
for (int i=0; i<dstpoints_size; i++)
{
suma += pow(dstpoints[i].x - ppts[i].x, 2);
suma += pow(dstpoints[i].y - ppts[i].y, 2);
}
I'm looking for 'params' vector that will give me the smallest value of 'suma' variable. Least squares algorithm seems to be a good option to solve it: http://dlib.net/dlib/optimization/optimization_least_squares_abstract.h.html#solve_least_squares, but I don't know if it is good for my case.
I think, my problem is that for every different 'params' vector I get different 'ppts' vector, not only single value, and I don't know if solve_least_squares function can match my example.
I must calculate residual for every point. I think, my 'list' from aforementioned link should be something like this:
(ppts[i].x - dstpoints[i].x, ppts[i].y - dstpoints[i].y, ppts[i+1].x - dstpoints[i+1].x, ppts[i+1].y - dstpoints[i+1].y, etc.)
, where 'ppts' vector depends on 'params' vector and then this problem can be solved with least squares algorithm. I don't know how to create data_samples with these assumptions, because it requires dlib::input_vector for every sample, as it is shown in example: http://dlib.net/least_squares_ex.cpp.html.
Am I thinking right?
I'm doing the same thing this days. My solution is writing a Powell Class by myself. It works, but really slowly. The program takes 2 minutes in dewarping linguistics_thesis.jpg.
I don't know what cause the program running so slowly. Maybe because of the algorithm or the code has some extra loop. I'm a Chinese student and my school only have java lessons. So it is normal if you find some extra codes in my codes.
Here is my Powell class.
using namespace std;
using namespace cv;
class MyPowell
{
public:
vector<vector<double>> xi;
vector<double> pcom;
vector<double> xicom;
vector<Point2d> dstpoints;
vector<double> myparams;
vector<double> params;
vector<Point> keypoint_index;
Point2d dst_br;
Point2d dims;
int N;
int itmax;
int ncom;
int iter;
double fret, ftol;
int usingAorB;
MyPowell(vector<Point2d> &dstpoints, vector<double> &params, vector<Point> &keypoint_index);
MyPowell(Point2d &dst_br, vector<double> &params, Point2d & dims);
MyPowell();
double obj(vector<double> &params);
void powell(vector<double> &p, vector<vector<double>> &xi, double ftol, double &fret);
double sign(double a);// , double b);
double sqr(double a);
void linmin(vector<double> &p, vector<double> &xit, int n, double &fret);
void mnbrak(double & ax, double & bx, double & cx,
double & fa, double & fb, double & fc);
double f1dim(double x);
double brent(double ax, double bx, double cx, double & xmin, double tol);
vector<double> usePowell();
void erase(vector<double>& pbar, vector<double> &prr, vector<double> &pr);
};
#include"Powell.h"
MyPowell::MyPowell(vector<Point2d> &dstpoints, vector<double>& params, vector<Point> &keypoint_index)
{
this->dstpoints = dstpoints;
this->myparams = params;
this->keypoint_index = keypoint_index;
N = params.size();
itmax = N * N;
usingAorB = 1;
}
MyPowell::MyPowell(Point2d & dst_br, vector<double>& params, Point2d & dims)
{
this->dst_br = dst_br;
this->myparams.push_back(dims.x);
this->myparams.push_back(dims.y);
this->params = params;
this->dims = dims;
N = 2;
itmax = N * 1000;
usingAorB = 2;
}
MyPowell::MyPowell()
{
usingAorB = 3;
}
double MyPowell::obj(vector<double> &myparams)
{
if (1 == usingAorB)
{
vector<Point2d> ppts = Dewarp::projectKeypoints(keypoint_index, myparams);
double total = 0;
for (int i = 0; i < ppts.size(); i++)
{
double x = dstpoints[i].x - ppts[i].x;
double y = dstpoints[i].y - ppts[i].y;
total += (x * x + y * y);
}
return total;
}
else if(2 == usingAorB)
{
dims.x = myparams[0];
dims.y = myparams[1];
//cout << "dims.x " << dims.x << " dims.y " << dims.y << endl;
vector<Point2d> vdims = { dims };
vector<Point2d> proj_br = Dewarp::projectXY(vdims, params);
double total = 0;
double x = dst_br.x - proj_br[0].x;
double y = dst_br.y - proj_br[0].y;
total += (x * x + y * y);
return total;
}
return 0;
}
void MyPowell::powell(vector<double> &x, vector<vector<double>> &direc, double ftol, double &fval)
{
vector<double> x1;
vector<double> x2;
vector<double> direc1;
int myitmax = 20;
if(N>500)
myitmax = 10;
else if (N > 300)
{
myitmax = 15;
}
double fx2, t, fx, dum, delta;
fval = obj(x);
int bigind;
for (int j = 0; j < N; j++)
{
x1.push_back(x[j]);
}
int iter = 0;
while (true)
{
do
{
do
{
iter += 1;
fx = fval;
bigind = 0;
delta = 0.0;
for (int i = 0; i < N; i++)
{
direc1 = direc[i];
fx2 = fval;
linmin(x, direc1, N, fval);
if (fabs(fx2 - fval) > delta)
{
delta = fabs(fx2 - fval);
bigind = i;
}
}
if (2.0 * fabs(fx - fval) <= ftol * (fabs(fx) + fabs(fval)) + 1e-7)
{
erase(direc1, x2, x1);
return;
}
if (iter >= itmax)
{
cout << "powell exceeding maximum iterations" << endl;
return;
}
if (!x2.empty())
{
x2.clear();
}
for (int j = 0; j < N; j++)
{
x2.push_back(2.0*x[j] - x1[j]);
direc1[j] = x[j] - x1[j];
x1[j] = x[j];
}
myitmax--;
cout << fx2 << endl;
fx2 = obj(x2);
if (myitmax < 0)
return;
} while (fx2 >= fx);
dum = fx - 2 * fval + fx2;
t = 2.0*dum*pow((fx - fval - delta), 2) - delta * pow((fx - fx2), 2);
} while (t >= 0.0);
linmin(x, direc1, N, fval);
direc[bigind] = direc1;
}
}
double MyPowell::sign(double a)//, double b)
{
if (a > 0.0)
{
return 1;
}
else
{
if (a < 0.0)
{
return -1;
}
}
return 0;
}
double MyPowell::sqr(double a)
{
return a * a;
}
void MyPowell::linmin(vector<double>& p, vector<double>& xit, int n, double &fret)
{
double tol = 1e-2;
ncom = n;
pcom = p;
xicom = xit;
double ax = 0.0;
double xx = 1.0;
double bx = 0.0;
double fa, fb, fx, xmin;
mnbrak(ax, xx, bx, fa, fx, fb);
fret = brent(ax, xx, bx, xmin, tol);
for (int i = 0; i < n; i++)
{
xit[i] = (xmin * xit[i]);
p[i] += xit[i];
}
}
void MyPowell::mnbrak(double & ax, double & bx, double & cx,
double & fa, double & fb, double & fc)
{
const double GOLD = 1.618034, GLIMIT = 110.0, TINY = 1e-20;
double val, fw, tmp2, tmp1, w, wlim;
double denom;
fa = f1dim(ax);
fb = f1dim(bx);
if (fb > fa)
{
val = ax;
ax = bx;
bx = val;
val = fb;
fb = fa;
fa = val;
}
cx = bx + GOLD * (bx - ax);
fc = f1dim(cx);
int iter = 0;
while (fb >= fc)
{
tmp1 = (bx - ax) * (fb - fc);
tmp2 = (bx - cx) * (fb - fa);
val = tmp2 - tmp1;
if (fabs(val) < TINY)
{
denom = 2.0*TINY;
}
else
{
denom = 2.0*val;
}
w = bx - ((bx - cx)*tmp2 - (bx - ax)*tmp1) / (denom);
wlim = bx + GLIMIT * (cx - bx);
if ((bx - w) * (w - cx) > 0.0)
{
fw = f1dim(w);
if (fw < fc)
{
ax = bx;
fa = fb;
bx = w;
fb = fw;
return;
}
else if (fw > fb)
{
cx = w;
fc = fw;
return;
}
w = cx + GOLD * (cx - bx);
fw = f1dim(w);
}
else
{
if ((cx - w)*(w - wlim) >= 0.0)
{
fw = f1dim(w);
if (fw < fc)
{
bx = cx;
cx = w;
w = cx + GOLD * (cx - bx);
fb = fc;
fc = fw;
fw = f1dim(w);
}
}
else if ((w - wlim)*(wlim - cx) >= 0.0)
{
w = wlim;
fw = f1dim(w);
}
else
{
w = cx + GOLD * (cx - bx);
fw = f1dim(w);
}
}
ax = bx;
bx = cx;
cx = w;
fa = fb;
fb = fc;
fc = fw;
}
}
double MyPowell::f1dim(double x)
{
vector<double> xt;
for (int j = 0; j < ncom; j++)
{
xt.push_back(pcom[j] + x * xicom[j]);
}
return obj(xt);
}
double MyPowell::brent(double ax, double bx, double cx, double & xmin, double tol = 1.48e-8)
{
const double CGOLD = 0.3819660, ZEPS = 1.0e-4;
int itmax = 500;
double a = MIN(ax, cx);
double b = MAX(ax, cx);
double v = bx;
double w = v, x = v;
double deltax = 0.0;
double fx = f1dim(x);
double fv = fx;
double fw = fx;
double rat = 0, u = 0, fu;
int iter;
int done;
double dx_temp, xmid, tol1, tol2, tmp1, tmp2, p;
for (iter = 0; iter < 500; iter++)
{
xmid = 0.5 * (a + b);
tol1 = tol * fabs(x) + ZEPS;
tol2 = 2.0*tol1;
if (fabs(x - xmid) <= (tol2 - 0.5*(b - a)))
break;
done = -1;
if (fabs(deltax) > tol1)
{
tmp1 = (x - w) * (fx - fv);
tmp2 = (x - v) * (fx - fw);
p = (x - v) * tmp2 - (x - w) * tmp1;
tmp2 = 2.0 * (tmp2 - tmp1);
if (tmp2 > 0.0)
p = -p;
tmp2 = fabs(tmp2);
dx_temp = deltax;
deltax = rat;
if ((p > tmp2 * (a - x)) && (p < tmp2 * (b - x)) &&
fabs(p) < fabs(0.5 * tmp2 * dx_temp))
{
rat = p / tmp2;
u = x + rat;
if ((u - a) < tol2 || (b - u) < tol2)
{
rat = fabs(tol1) * sign(xmid - x);
}
done = 0;
}
}
if(done)
{
if (x >= xmid)
{
deltax = a - x;
}
else
{
deltax = b - x;
}
rat = CGOLD * deltax;
}
if (fabs(rat) >= tol1)
{
u = x + rat;
}
else
{
u = x + fabs(tol1) * sign(rat);
}
fu = f1dim(u);
if (fu > fx)
{
if (u < x)
{
a = u;
}
else
{
b = u;
}
if (fu <= fw || w == x)
{
v = w;
w = u;
fv = fw;
fw = fu;
}
else if (fu <= fv || v == x || v == w)
{
v = u;
fv = fu;
}
}
else
{
if (u >= x)
a = x;
else
b = x;
v = w;
w = x;
x = u;
fv = fw;
fw = fx;
fx = fu;
}
}
if(iter > itmax)
cout << "\n Brent exceed maximum iterations.\n\n";
xmin = x;
return fx;
}
vector<double> MyPowell::usePowell()
{
ftol = 1e-4;
vector<vector<double>> xi;
for (int i = 0; i < N; i++)
{
vector<double> xii;
for (int j = 0; j < N; j++)
{
xii.push_back(0);
}
xii[i]=(1.0);
xi.push_back(xii);
}
double fret = 0;
powell(myparams, xi, ftol, fret);
//for (int i = 0; i < xi.size(); i++)
//{
// double a = obj(xi[i]);
// if (fret > a)
// {
// fret = a;
// myparams = xi[i];
// }
//}
cout << "final result" << fret << endl;
return myparams;
}
void MyPowell::erase(vector<double>& pbar, vector<double>& prr, vector<double>& pr)
{
for (int i = 0; i < pbar.size(); i++)
{
pbar[i] = 0;
}
for (int i = 0; i < prr.size(); i++)
{
prr[i] = 0;
}
for (int i = 0; i < pr.size(); i++)
{
pr[i] = 0;
}
}
I used PRAXIS library, because it doesn't need derivative information and is fast.
I modified the code a little to my needs and now it is faster than original version written in Python.

Bicubic interpolation with EasyBMP does not working properly

I am writing a program which is using bicubic interpolation, and I am using EasyBMP, but i have a problem. While input image are peppers.
My output looks like this.
#define BOUNDS(val, min, max) if (val < min) { val = min; } else if (val > max) { val = max; }
void resize(float value)
{
BMP* temp = new BMP();
int in_w = image->TellWidth();
int in_h = image->TellHeight();
temp->SetSize(in_w*value, in_h*value);
RGBApixel input;
for (int y = 0; y < temp->TellHeight(); ++y)
{
float v = float(y) / float(temp->TellHeight() - 1);
for (int x = 0; x < temp->TellWidth(); ++x)
{
float u = float(x) / float(temp->TellWidth() - 1);
input = this->bicubicInterpolation(u,v);
temp->SetPixel(x,y,input);
}
}
delete image;
image=temp;
}
RGBApixel bicubicInterpolation(float u, float v)
{
RGBApixel p[4][4];
int q,w;
float x = (u * image->TellWidth()) - 0.5;
int xint = int(x);
float dx = x - floor(x);
float y = (v * image->TellHeight()) - 0.5;
int yint = int(y);
float dy = y - floor(y);
for(int i=0; i<4; i++)
{
for(int j=0; j<4; j++)
{
q=xint - 1 + j;
BOUNDS(q,0,image->TellWidth()-1)
w=yint - 1 + i;
BOUNDS(w,0,image->TellHeight()-1)
p[j][i] = image->GetPixel(q,w);
}
}
RGBApixel toReturn;
RGBApixel el1 = this->interpolation(p[0][0],p[1][0], p[2][0], p[3][0], dx);
RGBApixel el2 = this->interpolation(p[0][1],p[1][1], p[2][1], p[3][1], dx);
RGBApixel el3 = this->interpolation(p[0][2],p[1][2], p[2][2], p[3][2], dx);
RGBApixel el4 = this->interpolation(p[0][3],p[1][3], p[2][3], p[3][3], dx);
RGBApixel value = this->interpolation(el1, el2, el3, el4, dy);
return value;
}
RGBApixel interpolation(RGBApixel A, RGBApixel B, RGBApixel C, RGBApixel D, float t)
{
float a[3],b[3],c[3],d[3];
RGBApixel toRet;
a[0]=A.Red;
b[0]=B.Red;
c[0]=C.Red;
d[0]=D.Red;
a[1]=A.Green;
b[1]=B.Green;
c[1]=C.Green;
d[1]=D.Green;
a[2]=A.Blue;
b[2]=B.Blue;
c[2]=C.Blue;
d[2]=D.Blue;
float w[3];
float x[3];
float y[3];
float z[3];
float color[3];
for(int i=0; i<3; i++)
{
w[i]= -a[i] / 2.0f + (3.0f*b[i]) / 2.0f - (3.0f*c[i]) / 2.0f + d[i] / 2.0f;
x[i]= a[i] - (5.0f*b[i]) / 2.0f + 2.0f*c[i] - d[i] / 2.0f;
y[i]= -a[i] / 2.0f + c[i] / 2.0f;
z[i]= b[i];
color[i]= w[i] * t*t*t + x[i] * t*t + y[i] * t +z[i];
}
toRet.Red=color[0];
toRet.Green=color[1];
toRet.Blue=color[2];
toRet.Alpha=255;
return toRet;
}
Have you noticed mistake that I made?
It was beign out of range. Just need to:
color[i]= w[i] * t*t*t + x[i] * t*t + y[i] * t +z[i];
BOUNDS(color[i],0,255);

c++ nbody simulation passing array as reference

I have a problem when trying to write a code to solve the nbody problem when using an array which contains all the bodies. My code doesn't do the right thing and i have no idea where it goes wrong though i suspect it has something to do with passing the array as a reference. To make it easier to spot my mistakes i will inculde a working version of the code which doesn't use the array containing all the bodies in the same way. The following is the code which doesn't work( when calculataing the orbit of a body you get a straight line instead of an ellipse with this code):
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <fstream>
#define h 10000.0 // size of the timestep
#define N 3 // number of bodies
#define G 6.67384*pow(10.0,-11) // gravitational constant
using namespace std;
class particle{
public:
double kx1,kx2,kx3,kx4, kv1, kv2, kv3, kv4;
double ky1, ky2, ky3, ky4, kvy1, kvy2, kvy3, kvy4;
double x,y,vx,vy,m;
double dist(particle body){
double dx = x - body.x;
double dy = y - body.y;
return sqrt(pow(dx,2.0)+pow(dy,2.0));
}
double g(double x1, double y1,particle body){
return G*body.m*(body.x-x1)/pow(dist(body),3.0);
}
double p(double x1, double y1, particle body){
return G*body.m*(body.y-y1)/pow(dist(body),3.0);
}
void update(){ //object advances 1 step
x = x + (1/6.0)*(kx1+2*kx2+2*kx3+kx4);
vx = vx + (1/6.0)*(kv1+2*kv2+2*kv3+kv4);
y = y + (1/6.0)*(ky1+2*ky2+2*ky3+ky4);
vy = vy + (1/6.0)*(kvy1+2*kvy2+2*kvy3+kvy4);
}
void create(double x1, double y1, double vx1, double vy1, double m1){ //choose the inital conditions for a new object
x = x1;
y = y1;
vx = vx1;
vy = vy1;
m =m1;
}
bool operator ==(particle &other){
if(x == other.x && y == other.y && vx == other.vx && vy == other.vy){
return true;
}
}
};
particle bodies[N];
void set(particle (&bodies)[N]){
bodies[0].create(1, 1, -2, 1, 2*pow(10.0,30));
bodies[1].create(2870671*pow(10.0,6), 0, 0, 6800, 8.6810*pow(10.0,25));
bodies[2].create(4498542*pow(10.0,6),0 ,0, 5430, 1.0243*pow(10.0,26));
}
double xforce(double x1, double y1, particle body, particle bodies[N]){ //force in the x- direction
double fx = 0;
for (int i = 0; i < N; i++){
if (bodies[i] == body ){;}
else{
fx += body.g(x1,y1,bodies[i]);
}
}
return fx;
}
double yforce(double x1, double y1, particle body, particle bodies[N]){ //force in the y- direction
double fy = 0;
for (int i = 0; i < N; i++){
if (bodies[i] == body) {;}
else{
fy += body.p(x1,y1,bodies[i]);
}
}
return fy;
}
void corr(double t, particle bodies[N]){ //runge kutta 4
for(int i =0; i <= N; i++){
bodies[i].kx1 = t*bodies[i].vx;
bodies[i].kv1 = t*xforce(bodies[i].x, bodies[i].y, bodies[i], bodies);
bodies[i].ky1 = t*bodies[i].vy;
bodies[i].kvy1 = t*yforce(bodies[i].x, bodies[i].y, bodies[i], bodies);
bodies[i].kx2 = t*(bodies[i].vx + 0.5*bodies[i].kv1);
bodies[i].kv2 = t*xforce(bodies[i].x + 0.5*bodies[i].kx1, bodies[i].y + 0.5*bodies[i].ky1, bodies[i], bodies);
bodies[i].ky2 = t*(bodies[i].vy + 0.5*bodies[i].kvy1);
bodies[i].kvy2 = t*yforce(bodies[i].x + 0.5*bodies[i].kx1, bodies[i].y + 0.5*bodies[i].ky1, bodies[i], bodies);
bodies[i].kx3 = t*(bodies[i].vx+ 0.5*bodies[i].kv2);
bodies[i].kv3 = t*xforce(bodies[i].x + 0.5*bodies[i].kx2, bodies[i].y + 0.5*bodies[i].ky2, bodies[i], bodies);
bodies[i].ky3 = t*(bodies[i].vy+ 0.5*bodies[i].kvy2);
bodies[i].kvy3 = t*yforce(bodies[i].x + 0.5*bodies[i].kx2, bodies[i].y + 0.5*bodies[i].ky2,bodies[i], bodies);
bodies[i].kx4 = t*(bodies[i].vx + bodies[i].kv3);
bodies[i].kv4 = t*xforce(bodies[i].x+ bodies[i].kx3, bodies[i].y + bodies[i].ky3, bodies[i], bodies);
bodies[i].ky4 = t*(bodies[i].vy + bodies[i].kvy3);
bodies[i].kvy4 = t*yforce(bodies[i].x + bodies[i].kx3, bodies[i].y + bodies[i].ky3, bodies[i], bodies);
}
}
void calculate(particle (&bodies)[N]){
set(bodies);
ofstream file;
file.open("tester.txt");
for(int i =0; i <=50000; i++){
corr(h, bodies);
for(int j = 0; j <= N; j++){
bodies[j].update();
}
if( i%1000 == 0){
file << i*h;
for(int j = 0; j <=N ; j++){
file <<" "<<bodies[j].x << " "<< bodies[j].y;
}
file <<" "<<"\n";
}
else{;}
}
file.close();
}
int main()
{
calculate(bodies);
system("pause");
return 0;
}
Here is the working version of the code, both are supposed to solve the same problem:
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <fstream>
#define h 10000.0
#define N 3
#define G 6.67384*pow(10.0,-11)
using namespace std;
class particle{
public:
double kx1,kx2,kx3,kx4, kv1, kv2, kv3, kv4;
double ky1, ky2, ky3, ky4, kvy1, kvy2, kvy3, kvy4;
double x,y,vx,vy,m;
double dist(particle body){
double dx = x - body.x;
double dy = y - body.y;
return sqrt(pow(dx,2.0)+pow(dy,2.0));
}
double g(double x1, double y1,particle body){
return G*body.m*(body.x-x1)/pow(dist(body),3.0);
}
double p(double x1, double y1, particle body){
return G*body.m*(body.y-y1)/pow(dist(body),3.0);
}
void update(){
x = x + (1/6.0)*(kx1+2*kx2+2*kx3+kx4);
vx = vx + (1/6.0)*(kv1+2*kv2+2*kv3+kv4);
y = y + (1/6.0)*(ky1+2*ky2+2*ky3+ky4);
vy = vy + (1/6.0)*(kvy1+2*kvy2+2*kvy3+kvy4);
}
void create(double x1, double y1, double vx1, double vy1, double m1){
x = x1;
y = y1;
vx = vx1;
vy = vy1;
m =m1;
}
bool operator ==(particle &other){
if(x == other.x && y == other.y && vx == other.vx && vy == other.vy){
return true;
}
}
bool operator !=(particle &other){
if(x != other.x || y != other.y || vx != other.vx || vy != other.vy){
return true;
}
}
};
particle zon, uranus, neptunus;
particle closest[] = {uranus, neptunus};
void set(){
zon.create(1, 1, -2, 1, 2*pow(10.0,30));
uranus.create(2870671*pow(10.0,6), 0, 0, 6800, 8.6810*pow(10.0,25));
neptunus.create(4498542*pow(10.0,6),0 ,0, 5430, 1.0243*pow(10.0,26));
}
double xforce(double x1, double y1, particle body){
particle bodies[] = {zon, uranus, neptunus};
double fx = 0;
for (int i = 0; i < 3; i++){
if (bodies[i] == body ){;}
else{
fx += body.g(x1,y1,bodies[i]);
}
}
return fx;
}
double yforce(double x1, double y1, particle body){
particle bodies[] = {zon, uranus, neptunus};
double fy = 0;
for (int i = 0; i <= 3; i++){
if (bodies[i] == body) {;}
else{
fy += body.p(x1,y1,bodies[i]);
}
}
return fy;
}
void corr(particle& body, double t){
body.kx1 = t*body.vx;
body.kv1 = t*xforce(body.x, body.y, body);
body.ky1 = t*body.vy;
body.kvy1 = t*yforce(body.x, body.y, body);
body.kx2 = t*(body.vx + 0.5*body.kv1);
body.kv2 = t*xforce(body.x + 0.5*body.kx1, body.y + 0.5*body.ky1, body);
body.ky2 = t*(body.vy + 0.5*body.kvy1);
body.kvy2 = t*yforce(body.x + 0.5*body.kx1, body.y + 0.5*body.ky1, body);
body.kx3 = t*(body.vx+ 0.5*body.kv2);
body.kv3 = t*xforce(body.x + 0.5*body.kx2, body.y + 0.5*body.ky2, body);
body.ky3 = t*(body.vy+ 0.5*body.kvy2);
body.kvy3 = t*yforce(body.x + 0.5*body.kx2, body.y + 0.5*body.ky2,body);
body.kx4 = t*(body.vx+body.kv3);
body.kv4 = t*xforce(body.x+ body.kx3, body.y + body.ky3, body);
body.ky4 = t*(body.vy + body.kvy3);
body.kvy4 = t*yforce(body.x + body.kx3, body.y + body.ky3, body);
}
void bereken(){
set();
ofstream file;
file.open("tester.txt");
for(int i =0; i <=50000; i++){
corr(zon, h);
corr(uranus, h);
corr(neptunus, h);
zon.update();
uranus.update();
neptunus.update();
if( i%1000 == 0){
file << i*h <<" "<<zon.x << " "<< zon.y <<" "<<uranus.x<<" " <<uranus.y <<" "<< neptunus.x<<" "<<neptunus.y<<" "<<"\n";
}
else{;}
}
file.close();
}
int main()
{
bereken();
system("pause");
return 0;
}
One problem is that you are overflowing your bodies[] array in 3 places:
#define N 3
particle bodies[N];
for (int i = 0; i <= N; i++) {
bodies[i].x = ... // Oops, access bodies[3] which doesn't exist
The correct loop, which you do use in two places, is:
for (int i = 0; i < N; i++) { // 0 to < N

bandpass butterworth filter implementation in C++

I am implementing an image analysis algorithm using openCV and c++, but I found out openCV doesnt have any function for Butterworth Bandpass filter officially.
in my project I have to pass a time series of pixels into the Butterworth 5 order filter and the function will return the filtered time series pixels. Butterworth(pixelseries,order, frequency), if you have any idea to help me of how to start please let me know. Thank you
EDIT :
after getting help, finally I come up with the following code. which can calculate the Numerator Coefficients and Denominator Coefficients, but the problem is that some of the numbers is not as same as matlab results. here is my code:
#include <iostream>
#include <stdio.h>
#include <vector>
#include <math.h>
using namespace std;
#define N 10 //The number of images which construct a time series for each pixel
#define PI 3.14159
double *ComputeLP( int FilterOrder )
{
double *NumCoeffs;
int m;
int i;
NumCoeffs = (double *)calloc( FilterOrder+1, sizeof(double) );
if( NumCoeffs == NULL ) return( NULL );
NumCoeffs[0] = 1;
NumCoeffs[1] = FilterOrder;
m = FilterOrder/2;
for( i=2; i <= m; ++i)
{
NumCoeffs[i] =(double) (FilterOrder-i+1)*NumCoeffs[i-1]/i;
NumCoeffs[FilterOrder-i]= NumCoeffs[i];
}
NumCoeffs[FilterOrder-1] = FilterOrder;
NumCoeffs[FilterOrder] = 1;
return NumCoeffs;
}
double *ComputeHP( int FilterOrder )
{
double *NumCoeffs;
int i;
NumCoeffs = ComputeLP(FilterOrder);
if(NumCoeffs == NULL ) return( NULL );
for( i = 0; i <= FilterOrder; ++i)
if( i % 2 ) NumCoeffs[i] = -NumCoeffs[i];
return NumCoeffs;
}
double *TrinomialMultiply( int FilterOrder, double *b, double *c )
{
int i, j;
double *RetVal;
RetVal = (double *)calloc( 4 * FilterOrder, sizeof(double) );
if( RetVal == NULL ) return( NULL );
RetVal[2] = c[0];
RetVal[3] = c[1];
RetVal[0] = b[0];
RetVal[1] = b[1];
for( i = 1; i < FilterOrder; ++i )
{
RetVal[2*(2*i+1)] += c[2*i] * RetVal[2*(2*i-1)] - c[2*i+1] * RetVal[2*(2*i-1)+1];
RetVal[2*(2*i+1)+1] += c[2*i] * RetVal[2*(2*i-1)+1] + c[2*i+1] * RetVal[2*(2*i-1)];
for( j = 2*i; j > 1; --j )
{
RetVal[2*j] += b[2*i] * RetVal[2*(j-1)] - b[2*i+1] * RetVal[2*(j-1)+1] +
c[2*i] * RetVal[2*(j-2)] - c[2*i+1] * RetVal[2*(j-2)+1];
RetVal[2*j+1] += b[2*i] * RetVal[2*(j-1)+1] + b[2*i+1] * RetVal[2*(j-1)] +
c[2*i] * RetVal[2*(j-2)+1] + c[2*i+1] * RetVal[2*(j-2)];
}
RetVal[2] += b[2*i] * RetVal[0] - b[2*i+1] * RetVal[1] + c[2*i];
RetVal[3] += b[2*i] * RetVal[1] + b[2*i+1] * RetVal[0] + c[2*i+1];
RetVal[0] += b[2*i];
RetVal[1] += b[2*i+1];
}
return RetVal;
}
double *ComputeNumCoeffs(int FilterOrder)
{
double *TCoeffs;
double *NumCoeffs;
int i;
NumCoeffs = (double *)calloc( 2*FilterOrder+1, sizeof(double) );
if( NumCoeffs == NULL ) return( NULL );
TCoeffs = ComputeHP(FilterOrder);
if( TCoeffs == NULL ) return( NULL );
for( i = 0; i < FilterOrder; ++i)
{
NumCoeffs[2*i] = TCoeffs[i];
NumCoeffs[2*i+1] = 0.0;
}
NumCoeffs[2*FilterOrder] = TCoeffs[FilterOrder];
free(TCoeffs);
return NumCoeffs;
}
double *ComputeDenCoeffs( int FilterOrder, double Lcutoff, double Ucutoff )
{
int k; // loop variables
double theta; // PI * (Ucutoff - Lcutoff) / 2.0
double cp; // cosine of phi
double st; // sine of theta
double ct; // cosine of theta
double s2t; // sine of 2*theta
double c2t; // cosine 0f 2*theta
double *RCoeffs; // z^-2 coefficients
double *TCoeffs; // z^-1 coefficients
double *DenomCoeffs; // dk coefficients
double PoleAngle; // pole angle
double SinPoleAngle; // sine of pole angle
double CosPoleAngle; // cosine of pole angle
double a; // workspace variables
cp = cos(PI * (Ucutoff + Lcutoff) / 2.0);
theta = PI * (Ucutoff - Lcutoff) / 2.0;
st = sin(theta);
ct = cos(theta);
s2t = 2.0*st*ct; // sine of 2*theta
c2t = 2.0*ct*ct - 1.0; // cosine of 2*theta
RCoeffs = (double *)calloc( 2 * FilterOrder, sizeof(double) );
TCoeffs = (double *)calloc( 2 * FilterOrder, sizeof(double) );
for( k = 0; k < FilterOrder; ++k )
{
PoleAngle = PI * (double)(2*k+1)/(double)(2*FilterOrder);
SinPoleAngle = sin(PoleAngle);
CosPoleAngle = cos(PoleAngle);
a = 1.0 + s2t*SinPoleAngle;
RCoeffs[2*k] = c2t/a;
RCoeffs[2*k+1] = s2t*CosPoleAngle/a;
TCoeffs[2*k] = -2.0*cp*(ct+st*SinPoleAngle)/a;
TCoeffs[2*k+1] = -2.0*cp*st*CosPoleAngle/a;
}
DenomCoeffs = TrinomialMultiply(FilterOrder, TCoeffs, RCoeffs );
free(TCoeffs);
free(RCoeffs);
DenomCoeffs[1] = DenomCoeffs[0];
DenomCoeffs[0] = 1.0;
for( k = 3; k <= 2*FilterOrder; ++k )
DenomCoeffs[k] = DenomCoeffs[2*k-2];
return DenomCoeffs;
}
void filter(int ord, double *a, double *b, int np, double *x, double *y)
{
int i,j;
y[0]=b[0] * x[0];
for (i=1;i<ord+1;i++)
{
y[i]=0.0;
for (j=0;j<i+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<i;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
for (i=ord+1;i<np+1;i++)
{
y[i]=0.0;
for (j=0;j<ord+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<ord;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
}
int main(int argc, char *argv[])
{
//Frequency bands is a vector of values - Lower Frequency Band and Higher Frequency Band
//First value is lower cutoff and second value is higher cutoff
double FrequencyBands[2] = {0.25,0.375};//these values are as a ratio of f/fs, where fs is sampling rate, and f is cutoff frequency
//and therefore should lie in the range [0 1]
//Filter Order
int FiltOrd = 5;
//Pixel Time Series
/*int PixelTimeSeries[N];
int outputSeries[N];
*/
//Create the variables for the numerator and denominator coefficients
double *DenC = 0;
double *NumC = 0;
//Pass Numerator Coefficients and Denominator Coefficients arrays into function, will return the same
NumC = ComputeNumCoeffs(FiltOrd);
for(int k = 0; k<11; k++)
{
printf("NumC is: %lf\n", NumC[k]);
}
//is A in matlab function and the numbers are correct
DenC = ComputeDenCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1]);
for(int k = 0; k<11; k++)
{
printf("DenC is: %lf\n", DenC[k]);
}
double y[5];
double x[5]={1,2,3,4,5};
filter(5, DenC, NumC, 5, x, y);
return 1;
}
I get this resutls for my code :
B= 1,0,-5,0,10,0,-10,0,5,0,-1
A= 1.000000000000000, -4.945988709743181, 13.556489496973796, -24.700711850327743,
32.994881546824828, -33.180726698160655, 25.546126213403539, -14.802008410165968,
6.285430089797051, -1.772929809750849, 0.277753012228403
but if I want to test the coefficinets in same frequency band in MATLAB, I get the following results:
>> [B, A]=butter(5, [0.25,0.375])
B = 0.0002, 0, -0.0008, 0, 0.0016, 0, -0.0016, 0, 0.0008, 0, -0.0002
A = 1.0000, -4.9460, 13.5565, -24.7007, 32.9948, -33.1806, 25.5461, -14.8020, 6.2854, -1.7729, 0.2778
I have test this website :http://www.exstrom.com/journal/sigproc/ code, but the result is equal as mine, not matlab. anybody knows why? or how can I get the same result as matlab toolbox?
I know this is a post on an old thread, and I would usually leave this as a comment, but I'm apparently not able to do that.
In any case, for people searching for similar code, I thought I would post the link from where this code originates (it also has C code for other types of Butterworth filter coefficients and some other cool signal processing code).
The code is located here:
http://www.exstrom.com/journal/sigproc/
Additionally, I think there is a piece of code which calculates said scaling factor for you already.
/**********************************************************************
sf_bwbp - calculates the scaling factor for a butterworth bandpass filter.
The scaling factor is what the c coefficients must be multiplied by so
that the filter response has a maximum value of 1.
*/
double sf_bwbp( int n, double f1f, double f2f )
{
int k; // loop variables
double ctt; // cotangent of theta
double sfr, sfi; // real and imaginary parts of the scaling factor
double parg; // pole angle
double sparg; // sine of pole angle
double cparg; // cosine of pole angle
double a, b, c; // workspace variables
ctt = 1.0 / tan(M_PI * (f2f - f1f) / 2.0);
sfr = 1.0;
sfi = 0.0;
for( k = 0; k < n; ++k )
{
parg = M_PI * (double)(2*k+1)/(double)(2*n);
sparg = ctt + sin(parg);
cparg = cos(parg);
a = (sfr + sfi)*(sparg - cparg);
b = sfr * sparg;
c = -sfi * cparg;
sfr = b - c;
sfi = a - b - c;
}
return( 1.0 / sfr );
}
I finally found it.
I just need to implement the following code from matlab source code to c++ . "the_mandrill" were right, I need to add the normalizing constant into the coefficient:
kern = exp(-j*w*(0:length(b)-1));
b = real(b*(kern*den(:))/(kern*b(:)));
EDIT:
and here is the final edition, which the whole code will return numbers exactly equal to MATLAB :
double *ComputeNumCoeffs(int FilterOrder,double Lcutoff, double Ucutoff, double *DenC)
{
double *TCoeffs;
double *NumCoeffs;
std::complex<double> *NormalizedKernel;
double Numbers[11]={0,1,2,3,4,5,6,7,8,9,10};
int i;
NumCoeffs = (double *)calloc( 2*FilterOrder+1, sizeof(double) );
if( NumCoeffs == NULL ) return( NULL );
NormalizedKernel = (std::complex<double> *)calloc( 2*FilterOrder+1, sizeof(std::complex<double>) );
if( NormalizedKernel == NULL ) return( NULL );
TCoeffs = ComputeHP(FilterOrder);
if( TCoeffs == NULL ) return( NULL );
for( i = 0; i < FilterOrder; ++i)
{
NumCoeffs[2*i] = TCoeffs[i];
NumCoeffs[2*i+1] = 0.0;
}
NumCoeffs[2*FilterOrder] = TCoeffs[FilterOrder];
double cp[2];
double Bw, Wn;
cp[0] = 2*2.0*tan(PI * Lcutoff/ 2.0);
cp[1] = 2*2.0*tan(PI * Ucutoff / 2.0);
Bw = cp[1] - cp[0];
//center frequency
Wn = sqrt(cp[0]*cp[1]);
Wn = 2*atan2(Wn,4);
double kern;
const std::complex<double> result = std::complex<double>(-1,0);
for(int k = 0; k<11; k++)
{
NormalizedKernel[k] = std::exp(-sqrt(result)*Wn*Numbers[k]);
}
double b=0;
double den=0;
for(int d = 0; d<11; d++)
{
b+=real(NormalizedKernel[d]*NumCoeffs[d]);
den+=real(NormalizedKernel[d]*DenC[d]);
}
for(int c = 0; c<11; c++)
{
NumCoeffs[c]=(NumCoeffs[c]*den)/b;
}
free(TCoeffs);
return NumCoeffs;
}
There are code which could be found online implementing butterworth filter. If you use the source code to try to get result matching MATLAB results, there will be the same problem.Basically the result you got from the code hasn't been normalized, and in the source code there is a variable sff in bwhp.c. If you set that to 1, the problem will be easily solved.
I recommend you to use this source code and
the source code and usage could be found here
I added the final edition of function ComputeNumCoeffs to the program and fix "FilterOrder" (k<11 to k<2*FiltOrd+1). Maybe it will save someone's time.
f1=0.5Gz, f2=10Gz, fs=127Gz/2
In MatLab
a={1.000000000000000,-3.329746259105707, 4.180522138699884,-2.365540522960743,0.514875789136976};
b={0.041065495448784, 0.000000000000000,-0.082130990897568, 0.000000000000000,0.041065495448784};
Program:
#include <iostream>
#include <stdio.h>
#include <vector>
#include <math.h>
#include <complex>
using namespace std;
#define N 10 //The number of images which construct a time series for each pixel
#define PI 3.1415926535897932384626433832795
double *ComputeLP(int FilterOrder)
{
double *NumCoeffs;
int m;
int i;
NumCoeffs = (double *)calloc(FilterOrder+1, sizeof(double));
if(NumCoeffs == NULL) return(NULL);
NumCoeffs[0] = 1;
NumCoeffs[1] = FilterOrder;
m = FilterOrder/2;
for(i=2; i <= m; ++i)
{
NumCoeffs[i] =(double) (FilterOrder-i+1)*NumCoeffs[i-1]/i;
NumCoeffs[FilterOrder-i]= NumCoeffs[i];
}
NumCoeffs[FilterOrder-1] = FilterOrder;
NumCoeffs[FilterOrder] = 1;
return NumCoeffs;
}
double *ComputeHP(int FilterOrder)
{
double *NumCoeffs;
int i;
NumCoeffs = ComputeLP(FilterOrder);
if(NumCoeffs == NULL) return(NULL);
for(i = 0; i <= FilterOrder; ++i)
if(i % 2) NumCoeffs[i] = -NumCoeffs[i];
return NumCoeffs;
}
double *TrinomialMultiply(int FilterOrder, double *b, double *c)
{
int i, j;
double *RetVal;
RetVal = (double *)calloc(4 * FilterOrder, sizeof(double));
if(RetVal == NULL) return(NULL);
RetVal[2] = c[0];
RetVal[3] = c[1];
RetVal[0] = b[0];
RetVal[1] = b[1];
for(i = 1; i < FilterOrder; ++i)
{
RetVal[2*(2*i+1)] += c[2*i] * RetVal[2*(2*i-1)] - c[2*i+1] * RetVal[2*(2*i-1)+1];
RetVal[2*(2*i+1)+1] += c[2*i] * RetVal[2*(2*i-1)+1] + c[2*i+1] * RetVal[2*(2*i-1)];
for(j = 2*i; j > 1; --j)
{
RetVal[2*j] += b[2*i] * RetVal[2*(j-1)] - b[2*i+1] * RetVal[2*(j-1)+1] +
c[2*i] * RetVal[2*(j-2)] - c[2*i+1] * RetVal[2*(j-2)+1];
RetVal[2*j+1] += b[2*i] * RetVal[2*(j-1)+1] + b[2*i+1] * RetVal[2*(j-1)] +
c[2*i] * RetVal[2*(j-2)+1] + c[2*i+1] * RetVal[2*(j-2)];
}
RetVal[2] += b[2*i] * RetVal[0] - b[2*i+1] * RetVal[1] + c[2*i];
RetVal[3] += b[2*i] * RetVal[1] + b[2*i+1] * RetVal[0] + c[2*i+1];
RetVal[0] += b[2*i];
RetVal[1] += b[2*i+1];
}
return RetVal;
}
double *ComputeNumCoeffs(int FilterOrder,double Lcutoff, double Ucutoff, double *DenC)
{
double *TCoeffs;
double *NumCoeffs;
std::complex<double> *NormalizedKernel;
double Numbers[11]={0,1,2,3,4,5,6,7,8,9,10};
int i;
NumCoeffs = (double *)calloc(2*FilterOrder+1, sizeof(double));
if(NumCoeffs == NULL) return(NULL);
NormalizedKernel = (std::complex<double> *)calloc(2*FilterOrder+1, sizeof(std::complex<double>));
if(NormalizedKernel == NULL) return(NULL);
TCoeffs = ComputeHP(FilterOrder);
if(TCoeffs == NULL) return(NULL);
for(i = 0; i < FilterOrder; ++i)
{
NumCoeffs[2*i] = TCoeffs[i];
NumCoeffs[2*i+1] = 0.0;
}
NumCoeffs[2*FilterOrder] = TCoeffs[FilterOrder];
double cp[2];
//double Bw;
double Wn;
cp[0] = 2*2.0*tan(PI * Lcutoff/ 2.0);
cp[1] = 2*2.0*tan(PI * Ucutoff/2.0);
//Bw = cp[1] - cp[0];
//center frequency
Wn = sqrt(cp[0]*cp[1]);
Wn = 2*atan2(Wn,4);
//double kern;
const std::complex<double> result = std::complex<double>(-1,0);
for(int k = 0; k<2*FilterOrder+1; k++)
{
NormalizedKernel[k] = std::exp(-sqrt(result)*Wn*Numbers[k]);
}
double b=0;
double den=0;
for(int d = 0; d<2*FilterOrder+1; d++)
{
b+=real(NormalizedKernel[d]*NumCoeffs[d]);
den+=real(NormalizedKernel[d]*DenC[d]);
}
for(int c = 0; c<2*FilterOrder+1; c++)
{
NumCoeffs[c]=(NumCoeffs[c]*den)/b;
}
free(TCoeffs);
return NumCoeffs;
}
double *ComputeDenCoeffs(int FilterOrder, double Lcutoff, double Ucutoff)
{
int k; // loop variables
double theta; // PI * (Ucutoff - Lcutoff)/2.0
double cp; // cosine of phi
double st; // sine of theta
double ct; // cosine of theta
double s2t; // sine of 2*theta
double c2t; // cosine 0f 2*theta
double *RCoeffs; // z^-2 coefficients
double *TCoeffs; // z^-1 coefficients
double *DenomCoeffs; // dk coefficients
double PoleAngle; // pole angle
double SinPoleAngle; // sine of pole angle
double CosPoleAngle; // cosine of pole angle
double a; // workspace variables
cp = cos(PI * (Ucutoff + Lcutoff)/2.0);
theta = PI * (Ucutoff - Lcutoff)/2.0;
st = sin(theta);
ct = cos(theta);
s2t = 2.0*st*ct; // sine of 2*theta
c2t = 2.0*ct*ct - 1.0; // cosine of 2*theta
RCoeffs = (double *)calloc(2 * FilterOrder, sizeof(double));
TCoeffs = (double *)calloc(2 * FilterOrder, sizeof(double));
for(k = 0; k < FilterOrder; ++k)
{
PoleAngle = PI * (double)(2*k+1)/(double)(2*FilterOrder);
SinPoleAngle = sin(PoleAngle);
CosPoleAngle = cos(PoleAngle);
a = 1.0 + s2t*SinPoleAngle;
RCoeffs[2*k] = c2t/a;
RCoeffs[2*k+1] = s2t*CosPoleAngle/a;
TCoeffs[2*k] = -2.0*cp*(ct+st*SinPoleAngle)/a;
TCoeffs[2*k+1] = -2.0*cp*st*CosPoleAngle/a;
}
DenomCoeffs = TrinomialMultiply(FilterOrder, TCoeffs, RCoeffs);
free(TCoeffs);
free(RCoeffs);
DenomCoeffs[1] = DenomCoeffs[0];
DenomCoeffs[0] = 1.0;
for(k = 3; k <= 2*FilterOrder; ++k)
DenomCoeffs[k] = DenomCoeffs[2*k-2];
return DenomCoeffs;
}
void filter(int ord, double *a, double *b, int np, double *x, double *y)
{
int i,j;
y[0]=b[0] * x[0];
for (i=1;i<ord+1;i++)
{
y[i]=0.0;
for (j=0;j<i+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<i;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
for (i=ord+1;i<np+1;i++)
{
y[i]=0.0;
for (j=0;j<ord+1;j++)
y[i]=y[i]+b[j]*x[i-j];
for (j=0;j<ord;j++)
y[i]=y[i]-a[j+1]*y[i-j-1];
}
}
int main(int argc, char *argv[])
{
(void)argc;
(void)argv;
//Frequency bands is a vector of values - Lower Frequency Band and Higher Frequency Band
//First value is lower cutoff and second value is higher cutoff
//f1 = 0.5Gz f2=10Gz
//fs=127Gz
//Kotelnikov/2=Nyquist (127/2)
double FrequencyBands[2] = {0.5/(127.0/2.0),10.0/(127.0/2.0)};//these values are as a ratio of f/fs, where fs is sampling rate, and f is cutoff frequency
//and therefore should lie in the range [0 1]
//Filter Order
int FiltOrd = 2;//5;
//Pixel Time Series
/*int PixelTimeSeries[N];
int outputSeries[N];
*/
//Create the variables for the numerator and denominator coefficients
double *DenC = 0;
double *NumC = 0;
//Pass Numerator Coefficients and Denominator Coefficients arrays into function, will return the same
printf("\n");
//is A in matlab function and the numbers are correct
DenC = ComputeDenCoeffs(FiltOrd, FrequencyBands[0], FrequencyBands[1]);
for(int k = 0; k<2*FiltOrd+1; k++)
{
printf("DenC is: %lf\n", DenC[k]);
}
printf("\n");
NumC = ComputeNumCoeffs(FiltOrd,FrequencyBands[0],FrequencyBands[1],DenC);
for(int k = 0; k<2*FiltOrd+1; k++)
{
printf("NumC is: %lf\n", NumC[k]);
}
double y[5];
double x[5]={1,2,3,4,5};
filter(5, DenC, NumC, 5, x, y);
return 1;
}