C++ Polynomial::derivative方法代码示例

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));
    };
    
}

int main(int argc, char *argv[]) {
  // End time
  double tf = 10.0;

  // Dimensions
  int nx = 3;
  int np = 1;

  // Declare variables
  SX x  = SX::sym("x",nx);  // state
  SX p  = SX::sym("u",np);  // control

  // ODE right hand side function
  SX ode = vertcat((1 - x(1)*x(1))*x(0) - x(1) + p,
                   x(0),
                   x(0)*x(0) + x(1)*x(1) + p*p);
  SXDict dae = {{"x", x}, {"p", p}, {"ode", ode}};

  // Number of finite elements
  int n = 100;

  // Size of the finite elements
  double h = tf/n;

  // Degree of interpolating polynomial
  int d = 4;

  // Choose collocation points
  vector<double> tau_root = collocation_points(d, "legendre");
  tau_root.insert(tau_root.begin(), 0);

  // Nonlinear solver to use
  string solver = "newton";
  if (argc>1) solver = argv[1]; // chose a different solver from command line

  // Coefficients of the collocation equation
  vector<vector<double> > C(d+1,vector<double>(d+1,0));

  // Coefficients of the continuity equation
  vector<double> D(d+1,0);

  // For all collocation points
  for(int j=0; j<d+1; ++j){

    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    Polynomial p = 1;
    for(int r=0; r<d+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] = p(1.0);

    // 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<d+1; ++r){
      C[j][r] = dp(tau_root[r]);
    }
  }

  // Total number of variables for one finite element
  MX X0 = MX::sym("X0",nx);
  MX P  = MX::sym("P",np);
  MX V = MX::sym("V",d*nx);

  // Get the state at each collocation point
  vector<MX> X(1,X0);
  for(int r=0; r<d; ++r){
    X.push_back(V.nz(Slice(r*nx,(r+1)*nx)));
  }

  // Get the collocation quations (that define V)
  Function f("f", {dae["x"], dae["p"]}, {dae["ode"]});
  vector<MX> V_eq;
  for(int j=1; j<d+1; ++j){
    // Expression for the state derivative at the collocation point
    MX xp_j = 0;
    for(int r=0; r<d+1; ++r){
      xp_j += C[r][j]*X[r];
    }

    // Append collocation equations
    vector<MX> v = {X[j], P};
    v = f(v);
    V_eq.push_back(h*v[0] - xp_j);
  }

  // Root-finding function, implicitly defines V as a function of X0 and P
  Function vfcn("vfcn", {V, X0, P}, {vertcat(V_eq)});

  // Convert to sxfunction to decrease overhead
  Function vfcn_sx = vfcn.expand("vfcn");

  // Create a implicit function instance to solve the system of equations
  Dict opts;
  if (solver=="ipopt") {
    // Use an NLP solver
    opts["nlpsol"] = "ipopt";
//.........这里部分代码省略.........

  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];
      }

//.........这里部分代码省略.........

int main(int argc, char* argv[])
{
//    quadraticFormulaTest();
//    return 0;
//
//    mullerTest2();
//    return 0;
//    
//    mullerTest();
//    return 0;
//    
//    deflationTest();
//    return 0;
//
//    deflationTest2();
//    return 0;

    if(argc < 2)
    {
        cerr << "No file specified... Aborting.\n";
        return 1;
    }
    
//    RealDouble::setEpsilon(0);
//    ifstream file(argv[1]);
//    Polynomial<type> origP;
//    file >> origP;
//
//    RealDouble::setEpsilon(.00000001);
//    const type scalar = 1000000000;
//    cout << "Read in Polynomial:\n" << origP << '\n';
//    cout << "p = " << scalar << " * origP.\n\n";
//    Polynomial<type> p(scalar * origP);

    RealDouble::setEpsilon(.00000001);
    ifstream file(argv[1]);
    Polynomial<type> p;
    file >> p;

    cout << "Original Polynomial:\n";
    polynomialRootFinderTest( p );
    cout << "First Derivative:\n";
    polynomialRootFinderTest( p.derivative() );
    cout << "Second Derivative:\n";
    polynomialRootFinderTest( p.derivative().derivative() );
    cout << "Third Derivative:\n";
    polynomialRootFinderTest( p.derivative().derivative().derivative() );
    cout << "Fourth Derivative:\n";
    polynomialRootFinderTest( p.derivative().derivative().derivative().derivative() );


//    Polynomial<type> firstDeriv = p.derivative();
//    cout << "First Derivative: " << firstDeriv << '\n';
//
//    Polynomial<type> secondDeriv = firstDeriv.derivative();
//    cout << "Second Derivative: " << secondDeriv << '\n';
//
//    Polynomial<type> thirdDeriv = secondDeriv.derivative();
//    cout << "Third Derivative: " << thirdDeriv << '\n';
////    cout << "Third Derivative(7.16098) = " << thirdDeriv(7.16098) << '\n';
////    cout << "Third Derivative(2.85193) = " << thirdDeriv(2.85193) << '\n';
//
//    cout << "Polynomial degree: " << p.degree() << '\n';
//    cout << "First Derivative degree: " << firstDeriv.degree() << '\n';
//    cout << "Second Derivative degree: " << secondDeriv.degree() << '\n';
//    cout << "Third Derivative degree: " << thirdDeriv.degree() << '\n';
}