double newton(int N, double *Nt, double *x, double *y){
int i;
double iks[N];
for(i = 0; i < N; i++){
Nt[i] = 0;
iks[i] = 0;
}
iks[0] = 1;
Nt[0] = y[0];
double ai;
for(i = 1; i < N; i++){
int j;
for(j = N-1; j > 0; j--){
iks[j] += iks[j-1];
iks[j-1] *= -x[i-1];
}
ai = (y[i] - Polynomial(N, Nt, x[i]))/(Polynomial(N, iks, x[i]));
// printf("ai: %f, y[i] = %f, rmian = %f, Licznik = %f\n", ai, y[i], Polynomial(N, Nt, x[i]), Polynomial(N, iks, x[i]));
// for(j = 0; j < N; j++){
// printf("j: %i, iks: %f, Nt: %f \n", j, iks[j], Nt[j]);
// }
nadd(N, Nt, iks, ai);
}
// int j;
// for(j = 0; j < N; j++){
// printf("j: %i, iks: %f, Nt: %f \n", j, iks[j], Nt[j]);
// }
return 0;
}
Polynomial Hermite_pro(unsigned i)
{
if(i==0)
return Polynomial({1.0f});
else if(i==1)
return Polynomial({0.0f, 1.0f});
else
return Hermite_pro(i-1)*Polynomial({0.0f, 1.0f})-Hermite_pro(i-2)*(i-1);
}
Polynomial Legendre(unsigned i)
{
if(i==0)
return Polynomial({1.0f});
else if(i==1)
return Polynomial({0.0f, 1.0f});
else
return (Legendre(i-1)*Polynomial({0.0f, 2.0f*(i-1)+1.0f})-Legendre(i-2)*float(i-1))/float(i);
}
Polynomial Laguerre(unsigned i)
{
if(i==0)
return Polynomial({1.0f});
else if(i==1)
return Polynomial({1.0f, -1.0f});
else
return (Laguerre(i-1)*Polynomial({2.0f*(i-1)+1.0f, -1.0f})-Laguerre(i-2)*float(i-1))/float(i);
}
Polynomial Tchebychev(unsigned i)
{
if(i==0)
return Polynomial({1.0f});
else if(i==1)
return Polynomial({0.0f, 1.0f});
else
return Tchebychev(i-1)*Polynomial({0.0f, 2.0f})-Tchebychev(i-2);
}
Polynomial Hermite_phi(unsigned i)
{
if(i==0)
return Polynomial({1.0f});
else if(i==1)
return Polynomial({0.0f, 2.0f});
else
return Hermite_phi(i-1)*Polynomial({0.0f, 2.0f})-2.0f*Hermite_phi(i-2)*(i-1);
}
void PolynomialFitting::Init(const std::string& file)
{
std::vector<double> x;
std::vector<double> y;
Load(file, x, y);
MatrixData X(x.size(), std::vector<double>(mPower + 1, 0));
for(size_t i = 0;i < x.size(); ++i)
{
X[i][0] = 1;
for(size_t j = 1; j <= mPower; ++j)
X[i][j] = X[i][j - 1] * x[i];
}
MatrixData Y(1, std::vector<double>(y));
Matrix xm(X), ym(Y);
ym = ym.Transpose();
Matrix res = xm.Transpose() * xm;
res = res.Inverse() * xm.Transpose() * ym;
X = res.Transpose().GetData();
std::cout << "Value: " << std::endl;
for(int i = 0;i < X[0].size(); ++i)
std::cout << X[0][i] << " ";
std::cout << std::endl;
mCofficient = Polynomial(X[0]);
}
Polynomial Polynomial::derivative() const {
vector<real_t> ret_p(p_.size()-1);
for (int k=1; k<p_.size(); ++k) {
ret_p[k-1] = k*p_[k];
}
return Polynomial(ret_p);
}
Polynomial Polynomial::derivative(){
int derivDegree = (this->degree == 0) ? 0 : this->degree-1;
std::vector<double> temp(derivDegree+1,0);
for(int i = 0; i < derivDegree+1; ++i){
temp[i] = (i+1)*this->coeffs[i+1];
}
return Polynomial(temp, derivDegree);
}
Polynomial Polynomial::operator+(const Polynomial& poly){
int maxDegree = (degree > poly.degree) ? degree : poly.degree;
std::vector<double> temp;
for(int i = 0; i < maxDegree+1; ++i){
temp.push_back(coeffs[i] + poly.coeffs[i]);
}
return Polynomial(temp, maxDegree);
}
Polynomial Polynomial::differentiate() const
{
std::vector<float> new_coeffs = coefficients;
new_coeffs.erase(new_coeffs.begin());
for(std::size_t i=1, I = new_coeffs.size();i<I;++i)
new_coeffs[i]*=i+1;
return Polynomial(new_coeffs);
}
Polynomial Polynomial::integrate(float c) const
{
std::vector<float> new_coeffs = coefficients;
new_coeffs.insert(new_coeffs.begin(), c);
for(std::size_t i=2, I = new_coeffs.size();i<I;++i)
new_coeffs[i]/=i;
return Polynomial(new_coeffs);
}
Polynomial Polynomial::anti_derivative() const {
vector<real_t> ret_p(p_.size()+1);
ret_p[0] = 0;
for (int k=0; k<p_.size(); ++k) {
ret_p[k+1] = p_[k]/(k+1);
}
return Polynomial(ret_p);
}
/*
* Only handles the case where poly evenly divides this. That
* is poly = (this) * h(x) for some polynomial h of degree
* poly.degree - this->degree. Uses synthetic division
*/
Polynomial Polynomial::syntheticDiv(double root){
int divDegree = this->degree - 1;
std::vector<double> temp(divDegree+1,0);
temp[divDegree] = this->coeffs[this->degree];
for(int i = divDegree-1; i >= 0; --i){
temp[i] = (root*temp[i+1]) + this->coeffs[i+1];
}
return Polynomial(temp, divDegree);
}
Polynomial Polynomial::operator*(const Polynomial& a) const {
vector<real_t> p_ret(p_.size() + a.p_.size() - 1, 0);
for (int d=0; d<p_.size(); ++d) {
for (int d_a=0; d_a<a.p_.size(); ++d_a) {
p_ret[d+d_a] += p_[d] * a.p_[d_a];
}
}
return Polynomial(p_ret);
}
Polynomial Polynomial::lower_polynomial()
{
if (first.is_valid())
{
if (first->getDegree() == 0) { return 0 }
if (first->next.is_valid) { return Polynomial(first->next); }
return 0;
}
return 0;
}
PolynomialSet PolynomialSet::markedTermIdeal()const
{
PolynomialSet LT(theRing);
for(const_iterator i=begin();i!=end();i++)
{
LT.push_back(Polynomial(i->getMarked()));
}
return LT;
}
Polynomial Polynomial::CoordinateMonomial(const Index numVars,
const Index i,
const Power p) {
if (!(numVars > 0))
throw PolynomialIndexError();
if (!isNonnegativePower(p))
throw PolynomialValueError();
MapPowers powers(numVars);
powers.setPower(i, p);
return Polynomial(powers);
}
Polynomial Polynomial::operator*(const Polynomial& poly){
const int prodDegree = degree + poly.degree;
std::vector<double> temp(prodDegree+1,0);
//initialize to zero
for(int i = 0; i < degree+1; ++i){
for(int j = 0; j < poly.degree+1; ++j){
double t = coeffs[i] * poly.coeffs[j];
temp[i+j] += t;
}
}
return Polynomial(temp, prodDegree);
}
bool test() {
Polynomial p = Polynomial();
p.push(0.0f,0.0f);
p.push(M_PI/6,0.5f);
p.push(2*M_PI/6,0.866f);
p.push(3*M_PI/6,1.0f);
p.print();
/* std::cout << "p(-2.0f) = " << p.eval(-2.0f) << std::endl;
std::cout << "p(0.0f) = " << p.eval(0.0f) << std::endl;
std::cout << "p(1.0f) = " << p.eval(1.0f) << std::endl;
std::cout << "p(2.0f) = " << p.eval(2.0f) << std::endl;
std::cout << "p(4.0f) = " << p.eval(4.0f) << std::endl;*/
return true;
}
void Polynomial<T>::input(std::istream &in)
{
T coef;
int data;
in.clear();
in>>coef>>data;
while(in.good())
{
operator+=(Polynomial(coef,data));
in>>coef>>data;
}
in.clear();
in.get();
}
Polynomial add (Polynomial a, Polynomial b, int(*compar)(const Monomial, const Monomial))
{
Rational c;//Used for checking zeroes
vector<Monomial> vec;//Holds the polynomial list
int num1 = a.poly.size();
int num2 = b.poly.size();
int inc1 = 0;
int inc2 = 0;
while (true)
{
if (inc1 == num1 && inc2 == num2)//If all the polynomials are emptied out then break
break;
if (inc1 == num1)//Check if at limit first
{
vec.push_back(b.poly[inc2]);
inc2++;
}
else
{
if (inc2 == num2)//Check if at limit first
{
vec.push_back(a.poly[inc1]);
inc1++;
}
else
{
switch (compar(a.poly[inc1], b.poly[inc2]))
{
case 0:
c = a.poly[inc1].coeff + b.poly[inc2].coeff;
if (c.numer != 0)
vec.push_back(a.poly[inc1] + b.poly[inc2]);
inc1++, inc2++;
break;
case 1:
vec.push_back(a.poly[inc1]);
inc1++;
break;
case -1:
vec.push_back(b.poly[inc2]);
inc2++;
break;
}
}
}
}
return Polynomial(vec);
}
Polynomial add(Polynomial a, Monomial b, int(*compar)(const Monomial, const Monomial))
{
Rational c;//Used for checking zeroes
vector<Monomial> vec;//Holds the polynomial list
int num = a.poly.size();
int inc = 0;
bool added = false;//Checks if b was added
while (true)
{
if (inc == num && added)
break;
if (inc == num)//Check if all of "a" added
{
vec.push_back(b);
added = true;
}
else
{
if (added)//Check if b already added
{
vec.push_back(a.poly[inc]);
inc++;
}
else
{
switch (compar(a.poly[inc], b))
{
case 0:
c = a.poly[inc].coeff + b.coeff;
if (c.numer != 0)
vec.push_back(a.poly[inc] + b);
inc++, added = true;
break;
case 1:
vec.push_back(a.poly[inc]);
inc++;
break;
case -1:
vec.push_back(b);
added = true;
break;
}
}
}
}
return Polynomial(vec);
}
SturmSequence::SturmSequence(const Polynomial& polynomial) {
assert(f.size() == 0);
Polynomial pol0 = Polynomial(0.0);
f.push_back(polynomial);
f.push_back(polynomial.derivative());
if (!(f[1] == pol0)) {
uint32_t n = 2;
Polynomial remainder;
do {
f[n-2].division(f[n-1],remainder);
remainder = remainder * -1;
f.push_back(remainder);
n++;
} while (!(remainder == pol0));
};
}
Polynomial Ideal::divideR(Polynomial p, int(*compar)(const Monomial, const Monomial))
{
vector<Polynomial> vec;//Holds the polynomials from the division
int leng = gen.size();
for (int i = 0; i < leng + 1; i++)//An extra one for the remainder
{
vector<Monomial> a;
vec.push_back(Polynomial(a));
}
while (p.poly.size() > 0)
{
bool divided = false;//Checks if a division occured
for (int i = 0; i < leng && divided == false; i++)
{
//cout << "here" << i;
if (divis(p.poly[0], gen[i].poly[0]))
{
Monomial div = p.poly[0] / gen[i].poly[0];
//cout << div.print(0);
vec[i] = add(vec[i], div, compar);
div.coeff.numer *= -1;//Now make it minus
Polynomial minus = div*gen[i];
//cout << endl << p.print() << endl;
//cout << minus.print() << endl;
p = add(p, minus, compar);
//cout << p.print() << endl;
divided = true;
}
}
//cout << "HERE";
if (divided == false)//The leading term could not be divided
{
vec[leng] = add(vec[leng], p.poly[0], compar);
p.poly.erase(p.poly.begin());
}
}
//Print the results
/*cout << endl << "Results" << endl;
for (int i = 0; i < leng; i++)
{
cout << "f" << i << ": " << vec[i].print() << " * (" << gen[i].print() << ")" << endl;
}
cout << "Remainder: " << vec[leng].print() << endl;
cout << endl << "Returning..." << endl;*/
return vec[leng];
}
//operator Definitions
const Polynomial Polynomial::operator+(const Polynomial& L_Poly) const // Adds two polynomials together
{
//iterators
list<Term>::const_iterator Itr1 = this->List_Terms.begin();
list<Term>::const_iterator Itr2 = L_Poly.List_Terms.begin();
//the result
Polynomial result = Polynomial();
Term temp; //a temp term to store term when adding
//continue till one iterator points to the tail of the list
while (Itr1!= this->List_Terms.end() && Itr2!=L_Poly.List_Terms.end())
{
//add the term with a bigger exponent to the new list
if ((Term)*Itr1 > (Term)*Itr2)
{
result.List_Terms.push_back(*Itr1);
Itr1++;
}
else if ((Term)*Itr1 < (Term)*Itr2)
{
result.List_Terms.push_back(*Itr2);
Itr2++;
}
//if 2 term have the same exponent, just add them together and add the result to new list
else
{
temp = (Term)*Itr1+(Term)*Itr2;
if (temp.get_coefficient() != 0) // only add to the result if the term has non-zero coefficient
result.List_Terms.push_back(temp);
Itr1++;
Itr2++;
}
}
//Add any remaining terms to the new list
while (Itr2 != L_Poly.List_Terms.end()){
result.List_Terms.push_back(*Itr2);
++Itr2;
}
while (Itr1 != this->List_Terms.end()){
result.List_Terms.push_back(*Itr1);
++Itr1;
}
return result; //return new list
}
Polynomial Polynomial::pow(const Power p) const {
if (!isNonnegativePower(p))
throw PolynomialValueError();
if (p == 1)
return Polynomial(*this);
Polynomial res = Polynomial::One(numVars_);
if (p > 0) {
Power mask(1);
//position of p's most-significant bit
while (mask <= p)
mask <<= 1;
mask >>= 1;
//use repeated squaring for binary expansion power
while (mask > 0) {
res *= res;
if (p & mask)
res *= (*this);
mask >>= 1;
}
}
int main(){
FILE *input, *output, *output2, *output3, *time1, *time2, *time3;
time1=fopen("time_lagrange.txt", "w");
time2=fopen("time_newton.txt", "w");
time3=fopen("time_gsl.txt", "w");
int N;
clock_t startTime = clock();
for(N = min; N < max; N++){
input=fopen("dane.txt","w");
output=fopen("custom_lagrange.txt","w");
output2=fopen("gsl_polynomial.txt","w");
output3=fopen("custom_newton.txt", "w");
fprintf(time1, "%i, ", N);
fprintf(time2, "%i, ", N);
fprintf(time3, "%i, ", N);
double x[N];
double y[N];
int i;
generateData(N, x, y);
for(i = 0; i < N; i ++){
fprintf (input,"%g %g\n", x[i], y[i]);
}
double Lg[N];
startTime = clock();
lagrange(N, Lg, x, y);
fprintf(time1, "%f\n", (double)(clock() - startTime)/(double)(CLOCKS_PER_SEC));
double Nt[N];
startTime = clock();
newton(N, Nt, x, y);
fprintf(time2, "%f\n", (double)(clock() - startTime)/(double)(CLOCKS_PER_SEC));
// for(i = 0; i < N; i++){
// printf("%f\n", Nt[i]);
// }
double xi, yi;
{
startTime = clock();
gsl_interp_accel *acc = gsl_interp_accel_alloc ();
gsl_spline *spline = gsl_spline_alloc (gsl_interp_polynomial,N);
gsl_spline_init (spline, x, y, N);
fprintf(time3, "%f\n", (double)(clock() - startTime)/(double)(CLOCKS_PER_SEC));
for (xi = x[0]; xi < x[N-1]; xi += 0.01)
{
yi = gsl_spline_eval (spline, xi, acc);
fprintf (output2,"%g %g\n", xi, yi);
}
gsl_spline_free (spline);
gsl_interp_accel_free(acc);
}
for(xi = x[0]; xi < x[N-1]; xi += 0.01){
yi = Polynomial(N, Lg, xi);
fprintf(output, "%g %g\n", xi, yi);
}
for(xi = x[0]; xi < x[N-1]; xi += 0.01){
yi = Polynomial(N, Nt, xi);
fprintf(output3, "%g %g\n", xi, yi);
}
fclose(input);
fclose(output);
fclose(output2);
fclose(output3);
}
return 0;
}
void CollocationIntegratorInternal::setupFG() {
// Interpolation order
deg_ = getOption("interpolation_order");
// All collocation time points
std::vector<long double> tau_root = collocationPointsL(deg_, getOption("collocation_scheme"));
// Coefficients of the collocation equation
vector<vector<double> > C(deg_+1, vector<double>(deg_+1, 0));
// Coefficients of the continuity equation
vector<double> D(deg_+1, 0);
// Coefficients of the quadratures
vector<double> B(deg_+1, 0);
// For all collocation points
for (int j=0; j<deg_+1; ++j) {
// Construct Lagrange polynomials to get the polynomial basis at the collocation point
Polynomial p = 1;
for (int r=0; r<deg_+1; ++r) {
if (r!=j) {
p *= Polynomial(-tau_root[r], 1)/(tau_root[j]-tau_root[r]);
}
}
// Evaluate the polynomial at the final time to get the
// coefficients of the continuity equation
D[j] = zeroIfSmall(p(1.0L));
// Evaluate the time derivative of the polynomial at all collocation points to
// get the coefficients of the continuity equation
Polynomial dp = p.derivative();
for (int r=0; r<deg_+1; ++r) {
C[j][r] = zeroIfSmall(dp(tau_root[r]));
}
// Integrate polynomial to get the coefficients of the quadratures
Polynomial ip = p.anti_derivative();
B[j] = zeroIfSmall(ip(1.0L));
}
// Symbolic inputs
MX x0 = MX::sym("x0", f_.input(DAE_X).sparsity());
MX p = MX::sym("p", f_.input(DAE_P).sparsity());
MX t = MX::sym("t", f_.input(DAE_T).sparsity());
// Implicitly defined variables (z and x)
MX v = MX::sym("v", deg_*(nx_+nz_));
vector<int> v_offset(1, 0);
for (int d=0; d<deg_; ++d) {
v_offset.push_back(v_offset.back()+nx_);
v_offset.push_back(v_offset.back()+nz_);
}
vector<MX> vv = vertsplit(v, v_offset);
vector<MX>::const_iterator vv_it = vv.begin();
// Collocated states
vector<MX> x(deg_+1), z(deg_+1);
for (int d=1; d<=deg_; ++d) {
x[d] = reshape(*vv_it++, this->x0().shape());
z[d] = reshape(*vv_it++, this->z0().shape());
}
casadi_assert(vv_it==vv.end());
// Collocation time points
vector<MX> tt(deg_+1);
for (int d=0; d<=deg_; ++d) {
tt[d] = t + h_*tau_root[d];
}
// Equations that implicitly define v
vector<MX> eq;
// Quadratures
MX qf = MX::zeros(f_.output(DAE_QUAD).sparsity());
// End state
MX xf = D[0]*x0;
// For all collocation points
for (int j=1; j<deg_+1; ++j) {
//for (int j=deg_; j>=1; --j) {
// Evaluate the DAE
vector<MX> f_arg(DAE_NUM_IN);
f_arg[DAE_T] = tt[j];
f_arg[DAE_P] = p;
f_arg[DAE_X] = x[j];
f_arg[DAE_Z] = z[j];
vector<MX> f_res = f_.call(f_arg);
// Get an expression for the state derivative at the collocation point
MX xp_j = C[0][j] * x0;
for (int r=1; r<deg_+1; ++r) {
xp_j += C[r][j] * x[r];
}
// Add collocation equation
eq.push_back(vec(h_*f_res[DAE_ODE] - xp_j));
// Add the algebraic conditions
eq.push_back(vec(f_res[DAE_ALG]));
// Add contribution to the final state
xf += D[j]*x[j];
// Add contribution to quadratures
qf += (B[j]*h_)*f_res[DAE_QUAD];
}
// Form forward discrete time dynamics
vector<MX> F_in(DAE_NUM_IN);
F_in[DAE_T] = t;
F_in[DAE_X] = x0;
F_in[DAE_P] = p;
F_in[DAE_Z] = v;
vector<MX> F_out(DAE_NUM_OUT);
F_out[DAE_ODE] = xf;
F_out[DAE_ALG] = vertcat(eq);
F_out[DAE_QUAD] = qf;
F_ = MXFunction(F_in, F_out);
F_.init();
// Backwards dynamics
// NOTE: The following is derived so that it will give the exact adjoint
// sensitivities whenever g is the reverse mode derivative of f.
if (!g_.isNull()) {
// Symbolic inputs
MX rx0 = MX::sym("x0", g_.input(RDAE_RX).sparsity());
MX rp = MX::sym("p", g_.input(RDAE_RP).sparsity());
// Implicitly defined variables (rz and rx)
MX rv = MX::sym("v", deg_*(nrx_+nrz_));
vector<int> rv_offset(1, 0);
for (int d=0; d<deg_; ++d) {
rv_offset.push_back(rv_offset.back()+nrx_);
rv_offset.push_back(rv_offset.back()+nrz_);
}
vector<MX> rvv = vertsplit(rv, rv_offset);
vector<MX>::const_iterator rvv_it = rvv.begin();
// Collocated states
vector<MX> rx(deg_+1), rz(deg_+1);
for (int d=1; d<=deg_; ++d) {
rx[d] = reshape(*rvv_it++, this->rx0().shape());
rz[d] = reshape(*rvv_it++, this->rz0().shape());
}
casadi_assert(rvv_it==rvv.end());
// Equations that implicitly define v
eq.clear();
// Quadratures
MX rqf = MX::zeros(g_.output(RDAE_QUAD).sparsity());
// End state
MX rxf = D[0]*rx0;
// For all collocation points
for (int j=1; j<deg_+1; ++j) {
// Evaluate the backward DAE
vector<MX> g_arg(RDAE_NUM_IN);
g_arg[RDAE_T] = tt[j];
g_arg[RDAE_P] = p;
g_arg[RDAE_X] = x[j];
g_arg[RDAE_Z] = z[j];
g_arg[RDAE_RX] = rx[j];
g_arg[RDAE_RZ] = rz[j];
g_arg[RDAE_RP] = rp;
vector<MX> g_res = g_.call(g_arg);
// Get an expression for the state derivative at the collocation point
MX rxp_j = -D[j]*rx0;
for (int r=1; r<deg_+1; ++r) {
rxp_j += (B[r]*C[j][r]) * rx[r];
}
// Add collocation equation
eq.push_back(vec(h_*B[j]*g_res[RDAE_ODE] - rxp_j));
// Add the algebraic conditions
eq.push_back(vec(g_res[RDAE_ALG]));
// Add contribution to the final state
rxf += -B[j]*C[0][j]*rx[j];
// Add contribution to quadratures
rqf += h_*B[j]*g_res[RDAE_QUAD];
}
// Form backward discrete time dynamics
vector<MX> G_in(RDAE_NUM_IN);
G_in[RDAE_T] = t;
G_in[RDAE_X] = x0;
G_in[RDAE_P] = p;
G_in[RDAE_Z] = v;
G_in[RDAE_RX] = rx0;
G_in[RDAE_RP] = rp;
G_in[RDAE_RZ] = rv;
vector<MX> G_out(RDAE_NUM_OUT);
G_out[RDAE_ODE] = rxf;
G_out[RDAE_ALG] = vertcat(eq);
G_out[RDAE_QUAD] = rqf;
G_ = MXFunction(G_in, G_out);
G_.init();
}
}
Polynomial Polynomial::One(const Index numVars) {
MapPowers zeroPowers(numVars);
return Polynomial(zeroPowers);
}
