vector<double> Polynomial :: rational_roots(Polynomial q)
{
cout<<endl<<"będę zaokrąglał niecałkowite współczynniki"<<endl;
vector<double> r;
vector<int> z;
vector<int> o;
for(int i = 0; i < q.getA(0); i++)
{
if( (int)q.getA(0) % 1 == 0 && (int)q.getA(0) % i == 0)
{
z.push_back(i);
z.push_back(-i);
}
}
for(int i = 0; i < q.getA(q.deg() + 1); i++)
{
if((int)q.getA(q.deg() + 1) % 1 == 0 && (int) q.getA(q.deg() + 1) % i == 0)
{
o.push_back(i);
o.push_back(-i);
}
}
for(int i = 0; i < z.size(); i++)
{
for(int j = 0; j < o.size(); j++)
{
if(value(q, j/i) == 0)
{
r.push_back(j/i);
}
}
}
return r;
}
void Polynomial::doubleProduct(const Monomial & x,
const Polynomial & poly,const Monomial & y) {
if(&poly==this) DBG();
setToZero();
if(!poly.zero()) {
Polynomial temp(poly);
Term aterm(x);
aterm *= temp.tip();
aterm.MonomialPart() *= y;
Copy<Term> copyterm(aterm);
temp.Removetip();
PolynomialRep4 * p = new PolynomialRep4(copyterm,x,temp,y);
d_set.insert(p);
#ifdef POLYNOMIAL_USE_LIST
putAllInList();
#endif
};
};
bool Polynomial::multiImply(const Polynomial* e1, int e1_num, const Polynomial& e2) {
#if (linux || __MACH__)
#ifdef __PRT_QUERY
std::cout << "-------------Multi-Imply solving-------------\n";
#endif
z3::config cfg;
cfg.set("auto_config", true);
z3::context c(cfg);
z3::expr hypo = e1[0].toZ3expr(NULL, c);
for (int i = 1; i < e1_num; i++) {
hypo = hypo && e1[i].toZ3expr(NULL, c);;
}
z3::expr conc = e2.toZ3expr(NULL, c);
//std::cout << "hypo: " << hypo << std::endl;
//std::cout << "conc: " << conc << std::endl;
z3::expr query = implies(hypo, conc);
#ifdef __PRT_QUERY
std::cout << "Query : " << query << std::endl;
std::cout << "Answer: ";
#endif
z3::solver s(c);
s.add(!query);
z3::check_result ret = s.check();
if (ret == unsat) {
#ifdef __PRT_QUERY
std::cout << "True" << std::endl;
#endif
return true;
}
else {
#ifdef __PRT_QUERY
std::cout << "False" << std::endl;
#endif
return false;
}
#endif
return false;
}
void Polynomial<int>::euclidean_div(
const Polynomial<int>& f, const Polynomial<int>& g,
Polynomial<int>& q, Polynomial<int>& r)
{
r = f; r.copy_on_write();
int rd=r.degree(), gd=g.degree(), qd;
if ( rd < gd ) { q = Polynomial<int>(int(0)); }
else { qd = rd-gd+1; q = Polynomial<int>(std::size_t(qd)); }
while ( rd >= gd && !(r.is_zero())) {
int S = r[rd] / g[gd];
qd = rd-gd;
q.coeff(qd) += S;
r.minus_offsetmult(g,S,qd);
rd = r.degree();
}
CGAL_postcondition( f==q*g+r );
}
bool SimpleReductor::reduceStep (Polynomial& r, const Polynomial& p) {
Polynomial t;
r.copy(p);
Monomial lm = r.lm();
int i, i0 = -1;
for (i = 0; i < g.size(); ++i) {
Monomial m = g[i].lm();
if (MP.divides(lm, m)) {
t.mul(g[i], MP.div(lm, m));
Cf f;
Ring::neg(f, r.lc());
r.add(t, t.lc(), f);
return true;
}
}
return false;
}
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;
}
Polynomial mult(Polynomial q, Polynomial p)
{
Polynomial w;
int h=0; //chwilowa wartoϾ w[i]
for(int i = (q.deg()+p.deg()); i >= 0; i--)
{
for(int j = max(p.deg(), q.deg()); j >= 0; j--)
{
h=h+(wiekszy(p, q).getA(j)*mniejszy(p,q).getA(i-j));
}
w.setA(i, h);
h=0;
}
return w;
}
const Polynomial operator*(const Polynomial &q) const
{
Polynomial u;
int k=(deg()+q.deg());
u.setA(k,0);
double w=0;
int i=0;
int j=0;
for(i=0; i<=deg(); i++)
{
for( j=0; j<=q.deg(); j++) {
u.setA(i+j,u.getA(i+j)+getA(i)*q.getA(j)) ;
}
j=0;
}
return u;
}
void Polynomial::madd(const Term &m, const Polynomial &p)
{
if(p.terms.empty())return; //added May 7 2005
int sugar2=p.getSugar()+m.m.exponent.sum();
if(sugar2>sugar)sugar=sugar2;
TermMap::iterator i=terms.lower_bound(Monomial(theRing,p.terms.begin()->first.exponent+m.m.exponent));
for(TermMap::const_iterator j=p.terms.begin();j!=p.terms.end();j++)
{
while(i!=terms.end() && TermMapCompare()(i->first,Monomial(theRing,j->first.exponent+m.m.exponent)))i++;
if(i==terms.end())
{
terms.insert(i,TermMap::value_type(Monomial(theRing,j->first.exponent+m.m.exponent),j->second*m.c));
}
else
{
if(!TermMapCompare()(Monomial(theRing,j->first.exponent+m.m.exponent),i->first))
{ // they must be equal
FieldElement c=i->second+j->second*m.c;
if(c.isZero())
{
TermMap::iterator oldI=i;
i++;
terms.erase(oldI);
}
else
{
i->second=c;
}
}
else
{
terms.insert(i,TermMap::value_type(Monomial(theRing,j->first.exponent+m.m.exponent),j->second*m.c));
}
}
}
}
bool Polynomial::uniImply(const Polynomial& e2) {
#if (linux || __MACH__)
#ifdef __PRT_QUERY
std::cout << BLUE << "-------------uni-Imply solving-------------\n" << NORMAL;
std::cout << RED << *this << " ==> " << e2 << std::endl << NORMAL;
#endif
z3::config cfg;
cfg.set("auto_config", true);
z3::context c(cfg);
z3::expr hypo = this->toZ3expr(NULL, c);
z3::expr conc = e2.toZ3expr(NULL, c);
z3::expr query = implies(hypo, conc);
#ifdef __PRT_QUERY
std::cout << "\nhypo: " << hypo << std::endl;
std::cout << "conc: " << conc << std::endl;
std::cout << BLUE << "Query : " << query << std::endl << NORMAL;
#endif
z3::solver s(c);
s.add(!query);
z3::check_result ret = s.check();
if (ret == unsat) {
#ifdef __PRT_QUERY
std::cout << "Answer: UNSAT\n";
#endif
return true;
}
#ifdef __PRT_QUERY
std::cout << "Answer: SAT\n";
#endif
#endif
return false;
}
inline Polynomial<eT>
gcd(const Polynomial<eT>& x, const Polynomial<eT>& y)
{
Polynomial<eT> r;
Polynomial<eT> u;
Polynomial<eT> v;
const int xdeg = x.degree();
const int ydeg = y.degree();
if(xdeg < 0 || ydeg < 0)
return std::move(u);
if((xdeg == 0 && x[0] == 0) || (ydeg == 0 && y[0] == 0))
{
u.resize(1);
u[0] = eT(0);
return std::move(u);
}
const bool maxo = (xdeg >= ydeg);
u = maxo ? x : y;
v = maxo ? y : x;
while(1)
{
r = u % v;
u = v;
v = r;
if(v.degree() == 0 && v[0] == 0)
break;
}
if(u.degree() == 0) //x and y are co-primes
u[0] = eT(1);
return std::move(u);
}
int main()
{
Polynomial a ({2,1,0,1,0}) ;
Polynomial b ;
Polynomial c (a);
cout<<a<<endl;
cout<<a.deriv()<<endl;
cout<<endl;
cout<<endl;
cout<<endl;
cout<<a<<endl;
b.setcoefs({1,1,1,1});
cout<<b.getcoef(0)<<endl;
cout<<b<<endl;
cout<< b.eval(2)<<endl;
cout<<endl;
cout<<endl;
cout<<endl;
cout<<b<<endl;
cout<<b*3<<endl;
cout<<endl;
cout<<endl;
cout<<endl;
cout<<b<<endl;
cout<<b+3<<endl;
cout<<endl;
return 0;
}
int main()
{
Polynomial polyguy;
Polynomial polyguy2(123.4, 5);
cout << "Default constructor initial value:" << endl;
polyguy.Display();
polyguy.setCoefficient(4, 299.1);
cout << "Changed values using setCoefficient and set Power:" << endl;
polyguy.Display();
cout << "Default constructor with specified value:" << endl;
polyguy2.Display();
cout << "Test of getCoefficient: " << polyguy2.getCoefficient() << endl;
polyguy.addPolynomial(polyguy2);
cout << "Addition of the default and specified polynomials:" << endl;
polyguy.Display();
return 0;
}
int main(int argc, char const *argv[])
{
Term term1(3,4);
Term term2(3,2);
Term term3(3,3);
Term term4(3,7);
Polynomial poly;
poly.push_back(term1);
poly.push_back(term2);
poly.push_back(term3);
poly.push_back(term4);
poly.print();
poly.differentiate();
poly.print();
return 0;
}
// Overloaded operator +
Polynomial operator+(const Polynomial &pola, const Polynomial &polb)
{
cout << "operator + \n";
int degA = pola.get_degree();
int degB = polb.get_degree();
int max_degr;
if (degA > degB)
{
max_degr = degA;
} else
{
max_degr = degB;
}
Polynomial result(max_degr);
int i;
for (i = 0; i <= degA && i <= degB; i++)
{
double nc = pola.get_coeff(i) + polb.get_coeff(i);
result.set_coeff(i, nc);
}
// Finish the rest of the coefficients
for (; i <= max_degr; i++)
{
if (degA != max_degr)
{
result.set_coeff(i, polb.get_coeff(i));
} else
{
result.set_coeff(i, pola.get_coeff(i));
}
}
return result;
}
int main(){
float pSave[3];
float *a;
a = new float[10];
int nMin, nMax, nStep;
ifstream read("KHAOSAT.INP", ios::in);
while (!read.eof()) {
read >> pSave[0] >> pSave[1] >> pSave[2] >> nMin >> nMax >> nStep;
}
float pNumber[10];
int nCount = 0;
while (1){
pNumber[nCount] = nMin +nCount * nStep;
if ((pNumber[nCount++] + nStep) > nMax) break;
}
read.close();
float f[] = {-3, pSave[0]};
float g[] = {0, pSave[1], -3};
float h[] = {-5, 0 , pSave[2], 7};
Polynomial oF = Polynomial(1, f);
Polynomial oG = Polynomial(2, g);
Polynomial oH = Polynomial(3, h);
string sOut = "KHAOSAT.OUT";
void (*fPoint)(string ,float*,int ,Polynomial) = writef;
void (*gPoint)(string ,float*,int ,Polynomial) = writeg;
void (*hPoint)(string ,float*,int ,Polynomial) = writeh;
writeTitle(sOut, nCount, pNumber);
write(fPoint, sOut, pNumber, nCount, oF);
write(gPoint, sOut, pNumber, nCount, oG);
write(hPoint, sOut, pNumber, nCount, oH);
writeTitle(sOut, nCount, pNumber);
oF.writeDeri(sOut, pNumber, nCount);
oG.writeDeri(sOut, pNumber, nCount);
oH.writeDeri(sOut, pNumber, nCount);
system("pause");
return 0;
}
// doesn't return a remainder
// N.B. this function will break if there is a remainder
const Polynomial Polynomial::div( const Polynomial& rhs ) const {
Polynomial retVal;
if( degree() < rhs.degree() )
{
return retVal; // return 0
}
Polynomial rSide( *this );
int rDeg = rhs.degree();
double rCoeff = rhs._list.begin().getData().coeff;
itr_t it = rSide._list.begin();
while( 1 )
{
if( it == rSide._list.end() ) break;
int deg_diff = it.getData().degree() - rDeg;
if( deg_diff < 0 ) break; // TODO: check this condition, maybe need to put rest into remainder?
double coeff = it.getData().coeff / rCoeff;
Polynomial tmp;
Term multiplier( coeff, deg_diff );
retVal.addTerm( multiplier );
for( itr_t itt = rhs._list.begin(); itt != rhs._list.end(); ++itt )
{
Term res = itt.getData() * multiplier;
tmp.addTerm( res );
}
rSide = rSide.sub( tmp );
it = rSide._list.begin();
}
return retVal;
}
HexNodeBasis::HexNodeBasis(size_t order){
// Reference Space //
refSpace = new HexReferenceSpace;
nRefSpace = getReferenceSpace().getNReferenceSpace();
const vector<vector<vector<size_t> > >&
edgeIdx = refSpace->getEdgeNodeIndex();
const vector<vector<vector<size_t> > >&
faceIdx = refSpace->getFaceNodeIndex();
// Set Basis Type //
this->order = order;
type = 0;
dim = 3;
nVertex = 8;
nEdge = 12 * (order - 1);
nFace = 6 * (order - 1) * (order - 1);
nCell = (order - 1) * (order - 1) * (order - 1);
nFunction = nVertex + nEdge + nFace + nCell;
// Legendre Polynomial //
Polynomial* legendre = new Polynomial[order];
Legendre::integrated(legendre, order);
// Lagrange & Lifting //
const Polynomial lagrange[8] =
{
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) *
(Polynomial(1, 0, 1, 0)) *
(Polynomial(1, 0, 0, 1)))
};
const Polynomial lifting[8] =
{
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 0, 0) - Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 1))),
Polynomial((Polynomial(1, 0, 0, 0) - Polynomial(1, 1, 0, 0)) +
(Polynomial(1, 0, 1, 0)) +
(Polynomial(1, 0, 0, 1)))
};
// Basis //
basis = new Polynomial**[nRefSpace];
for(size_t s = 0; s < nRefSpace; s++)
basis[s] = new Polynomial*[nFunction];
// Vertex Based //
for(size_t s = 0; s < nRefSpace; s++){
basis[s][0] = new Polynomial(lagrange[0]);
basis[s][1] = new Polynomial(lagrange[1]);
basis[s][2] = new Polynomial(lagrange[2]);
basis[s][3] = new Polynomial(lagrange[3]);
basis[s][4] = new Polynomial(lagrange[4]);
basis[s][5] = new Polynomial(lagrange[5]);
basis[s][6] = new Polynomial(lagrange[6]);
basis[s][7] = new Polynomial(lagrange[7]);
}
// Edge Based //
for(size_t s = 0; s < nRefSpace; s++){
size_t i = nVertex;
for(size_t e = 0; e < 12; e++){
for(size_t l = 1; l < order; l++){
basis[s][i] =
new Polynomial(legendre[l].compose(lifting[edgeIdx[s][e][1]] -
lifting[edgeIdx[s][e][0]])
*
(lagrange[edgeIdx[s][e][0]] +
lagrange[edgeIdx[s][e][1]]));
i++;
}
}
}
// Face Based //
for(size_t s = 0; s < nRefSpace; s++){
size_t i = nVertex + nEdge;
for(size_t f = 0; f < 6; f++){
for(size_t l1 = 1; l1 < order; l1++){
for(size_t l2 = 1; l2 < order; l2++){
Polynomial sum =
lagrange[faceIdx[s][f][0]] +
lagrange[faceIdx[s][f][1]] +
lagrange[faceIdx[s][f][2]] +
lagrange[faceIdx[s][f][3]];
basis[s][i] =
new Polynomial(legendre[l1].compose(lifting[faceIdx[s][f][0]] -
lifting[faceIdx[s][f][1]]) *
legendre[l2].compose(lifting[faceIdx[s][f][0]] -
lifting[faceIdx[s][f][3]]) *
sum);
i++;
}
}
}
}
// Cell Based //
Polynomial px = Polynomial(2, 1, 0, 0);
Polynomial py = Polynomial(2, 0, 1, 0);
Polynomial pz = Polynomial(2, 0, 0, 1);
px = px - Polynomial(1, 0, 0, 0);
py = py - Polynomial(1, 0, 0, 0);
pz = pz - Polynomial(1, 0, 0, 0);
for(size_t s = 0; s < nRefSpace; s++){
size_t i = nVertex + nEdge + nFace;
for(size_t l1 = 1; l1 < order; l1++){
for(size_t l2 = 1; l2 < order; l2++){
for(size_t l3 = 1; l3 < order; l3++){
basis[s][i] =
new Polynomial(legendre[l1].compose(px) *
legendre[l2].compose(py) *
legendre[l3].compose(pz));
i++;
}
}
}
}
// Mapping to Gmsh Quad //
// x = (u + 1) / 2
// y = (v + 1) / 2
//
// (x, y) = Zaglmayr Ref Quad
// (u, v) = Gmsh Ref Quad
Polynomial mapX(Polynomial(0.5, 1, 0, 0) +
Polynomial(0.5, 0, 0, 0));
Polynomial mapY(Polynomial(0.5, 0, 1, 0) +
Polynomial(0.5, 0, 0, 0));
Polynomial mapZ(Polynomial(0.5, 0, 0, 1) +
Polynomial(0.5, 0, 0, 0));
for(size_t s = 0; s < nRefSpace; s++){
for(size_t i = 0; i < nFunction; i++){
Polynomial* tmp;
tmp = basis[s][i];
basis[s][i] = new Polynomial(tmp->compose(mapX, mapY, mapZ));
delete tmp;
}
}
// Free Temporary Sapce //
delete[] legendre;
}
int main() {
vector<int> a;
for (int i = 1; i < 5; ++i) {
a.push_back(i);
}
vector<int> b;
for (int i = 0; i < 7; ++i) {
int value = 4 - i;
b.push_back(value);
}
Polynomial<int> A(a);
Polynomial<int> B(b);
cout << "A = " << A << '\n' << "B = " << B << '\n';
Polynomial<int> C;
cout << "NULL Polynomial: " << C << " Power: " << C.GetPower() << '\n';
C = A + B;
cout << "A + B = " << C << '\n';
C = A - B;
cout << "A - B = " << C << '\n';
C = 0;
cout << "C = 0 <=> C: " << C << '\n';
C += A;
cout << "C += A: " << C << '\n';
C -= B;
cout << "C -= B: " << C << '\n';
C += 7;
cout << "C += 7: " << C << '\n';
C -= 4;
cout << "C -= 4: " << C << '\n';
C *= 5;
cout << "C *= 5: " << C << '\n';
C /= 5;
cout << "C /= 5: " << C << '\n';
C %= 3;
cout << "C %= 3: " << C << '\n';
C = A * 0;
cout << "C = A * 0: " << C << '\n';
try {
C = A / 0;
} catch (Polynomial<>::DivisionByZeroException e) {
cout << "C = A / 0: Div by Zero exception!\n";
}
C = A * B;
cout << "C = A * B: " << C << '\n';
cout << "\n\n NEW PolynomialS INCLUDED: \n";
vector<int> d;
d.push_back(1); d.push_back(2); d.push_back(2); d.push_back(1);
vector<int> f;
f.push_back(-2); f.push_back(-2); f.push_back(-1); f.push_back(1); f.push_back(1);
vector<int> g;
g.push_back(1); g.push_back(2); g.push_back(3); g.push_back(4); g.push_back(5); g.push_back(6);
vector<int> h;
h.push_back(-1); h.push_back(1); h.push_back(3); h.push_back(5); h.push_back(7); h.push_back(9);
vector<int> k;
k.push_back(-1); k.push_back(-1); k.push_back(1); k.push_back(1);
vector<int> z;
z.push_back(1); z.push_back(1);
vector<int> l;
l.push_back(-2); l.push_back(10); l.push_back(12); l.push_back(-5); l.push_back(1); l.push_back(2);
vector<int> m;
m.push_back(7); m.push_back(-2); m.push_back(0); m.push_back(1);
vector<int> n;
n.push_back(-8); n.push_back(-4); n.push_back(2); n.push_back(5); n.push_back(2); n.push_back(-4); n.push_back(1);
vector<int> p;
p.push_back(-4); p.push_back(-4); p.push_back(1); p.push_back(-1); p.push_back(-1); p.push_back(1);
Polynomial<int> D(d); cout << "D: " << D << '\n';
Polynomial<int> F(f); cout << "F: " << F << '\n';
Polynomial<int> G(g); cout << "G: " << G << '\n';
Polynomial<int> H(h); cout << "H: " << H << '\n';
Polynomial<int> K(k); cout << "K: " << K << '\n';
Polynomial<int> Z(z); cout << "Z: " << Z << '\n';
Polynomial<int> L(l); cout << "L: " << L << '\n';
Polynomial<int> M(m); cout << "M: " << M << '\n';
Polynomial<int> N(n); cout << "N: " << N << '\n';
Polynomial<int> P(p); cout << "P: " << P << '\n';
cout << "\n\n START CALCULATIONS\n";
C = (D, F);
cout << "C = GCD(D, F): " << C << '\n';
C = H / G;
cout << "C = H / G: " << C << '\n';
C = K / Z;
cout << "C = K / Z: " << C << '\n';
C = (L, M);
cout << "MUTUAL PRIME: C = GCD(L, M): " << C << '\n';
C = (P, N);
cout << "Inverse order: C = GCD(P, N): " << C << '\n';
C = F % D;
cout << "C = F % D: " << C << '\n';
C = N % P;
cout << "C = N % P: " << C << '\n';
C = P % (N % P);
cout << "C = P % (N % P): " << C << '\n';
C = 108;
C %= P;
cout << "108 % P: " << C << '\n';
C = 108;
C /= P;
cout << "108 / P: " << C << '\n';
C = P / 1;
cout << "C = P / 1: " << C << '\n';
C = P;
C /= 7;
cout << "C = P; C /= 7: " << C << '\n';
C = P;
C /= 4;
cout << "C = P; C /= 4: " << C << '\n';
C += Z;
cout << "C = P; C /= 4; C += Z: " << '\n';
C = P / 4 + 1;
cout << "C = P / 4 + 1: " << C << '\n';
C = P / 4 + 2;
C += Z;
cout << "C = P / 4 + 2; C += Z: " << C << '\n';
C = P = N;
cout << "C = P = N: " << C << '\n';
cout << "Get Power of C: " << C.GetPower() << '\n';
C = -F;
cout << "C = -F: " << C << '\n';
C *= L;
cout << "C = -F; C *= L: " << C << '\n';
cout << "C(3): " << C(3) << '\n';
cout << "C(0): " << C(0) << " C(-2): " << C(-2) << '\n';
cout << "C\' - derivative: " << C.Derivative() << " Power: " << C.Derivative().GetPower() << '\n';
C = 16;
cout << "C = 16 - scalar; C\': " << C.Derivative() << " Power: " << C.Derivative().GetPower() << '\n';
C = N;
cout << "\n\n Iterator from C.begin to C.end\nC = N: " << C << '\n';
for (Polynomial<int>::iterator it = C.begin(); it != C.end(); ++it) {
cout << *it << ' ';
}
cout << "\n\n Bool operations\nC = N\n";
if (C == N) {
cout << "True: C == N\n";
} else {
cout << "WRONG BOOL OPERATOR!\n";
}
if (C != L) {
cout << "True: C != L\n";
} else {
cout << "WRONG BOOL OPERATOR!\n";
}
cout << "\n\n Get C[ index ]: \n";
cout << "C[0]: " << C[0] << " C[3]: " << C[3] << '\n';
try {
cout << C[11] << '\n';
} catch (Polynomial<int>::ArrayOutOfBoundsException e) {
cout << "C[11] is out of bounds!\n";
}
cout << "\n\n SUPER Special Polynomials\n";
vector<int> x;
x.push_back(0); x.push_back(1);
vector<int> y;
y.push_back(1); y.push_back(2);
Polynomial<int> X(x); cout << "X: " << X << '\n';
Polynomial<int> Y(y); cout << "Y: " << Y << '\n';
C = (X, Y);
cout << "C = GCD(X, Y): " << C << '\n';
C = (Y, X);
cout << "C = GCD(Y, X): " << C << '\n';
C = X / Y;
cout << "C = X / Y: " << C << '\n';
C = Y / X;
cout << "C = Y / X: " << C << '\n';
C = Y % X;
cout << "C = Y % X: " << C << '\n';
C = X % Y;
cout << "C = X % Y: " << C << '\n';
vector<int> q;
q.push_back(0); q.push_back(1);
vector<int> t;
t.push_back(0); t.push_back(4);
Polynomial<int> Q(q); cout << "Q: " << Q << '\n';
Polynomial<int> T(t); cout << "T: " << T << '\n';
C = (Q, T);
cout << "C = GCD(Q, T): " << C << '\n';
C = (T, Q);
cout << "C = GCD(T, Q): " << C << '\n';
cout << "\n\n Double examples\n";
vector<double> d1;
d1.push_back(1.5); d1.push_back(2.3); d1.push_back(-3.4);
vector<double> d2;
d2.push_back(-0.6); d2.push_back(1.8);
Polynomial<double> D1(d1); cout << "D1: " << D1 << '\n';
Polynomial<double> D2(d2); cout << "D2: " << D2 << '\n';
Polynomial<double> Result;
Result = D1 / D2;
cout << "Result = D1 / D2: " << Result << '\n';
cout << "\n\n Monom Demo\n";
Polynomial<int>::Monom _x;
Polynomial<int> f_x = (_x^1)*4 + (_x^3) + (_x^5) + _x;
cout << "Polynomial from Monom: " << f_x << '\n';
Polynomial<double>::Monom _y;
Polynomial<double> f_y = (_y^2) + _y + (_y^7)*2.71 + 0.59 + _y*0.91;
cout << "Polynomial from Monom: " << f_y << '\n';
cout << "Thank You for attention!\n";
_getch();
return 0;
}
bool Polynomial<CoefficientType>::isApprox(const Polynomial& other,
const RealScalar& tol) const {
return getCoefficients().isApprox(other.getCoefficients(), tol);
}
// O(n^2 + m^2) from factor
// O(n log n) sort
// O(n * m) adding
Polynomial Polynomial::operator +(Polynomial& rhs)
{
/*
PRE-CONDITIONS:
The two polynomials have been entered in the correct format. Factoring called in-function.
POST-CONDITIONS:
The polynomial will combine all like terms from both of the polynomials and return the result as a new polynomial.
*/
Polynomial newPoly;
bool foundLikeTerm;
int newCoefficient;
// factor polynomials before adding them together
factor();
rhs.factor();
// begin comparing terms from each Polynomial and combining them
for (list<Term>::iterator iter = termList.begin(); iter != termList.end(); iter++)
{
foundLikeTerm = false;
// compare each term from poly1 with every term from poly2
for (list<Term>::iterator iter2 = rhs.termList.begin(); iter2 != rhs.termList.end();)
{
if (iter->getHasVariable() == true && iter->getExponent() == iter2->getExponent())
{
// If there is a like term, add the coefficients together and add to new poly list
foundLikeTerm = true;
newCoefficient = iter->getCoefficient() + iter2->getCoefficient();
// Advance iterator and delete current term
iter2 = rhs.termList.erase(iter2);
if (newCoefficient != 0)
// Add combined term to new polynomial
newPoly.addTermToList(Term(newCoefficient, iter->getExponent()));
}
else if (iter->getHasVariable() == false && iter2->getHasVariable() == false)
{
foundLikeTerm = true;
newCoefficient = iter->getCoefficient() + iter2->getCoefficient();
// Advance iterator and delete current term
iter2 = rhs.termList.erase(iter2);
// Add combined term to new polynomial if it is anything but zero.
if (newCoefficient != 0)
newPoly.addTermToList(Term(newCoefficient));
}
else{
iter2++;
}
}
// If there are no terms to be combined with, add term as it is.
// drop stand alone zeros from the polynomial
if (foundLikeTerm == false && iter->getCoefficient() != 0)
newPoly.addTermToList(*iter);
}
// add remaining terms from poly2 that had no matching terms in poly1
for (list<Term>::iterator iter2 = rhs.termList.begin(); iter2 != rhs.termList.end(); iter2++)
newPoly.addTermToList(*iter2);
// Sort (ascending order) and reverse to get it descending
newPoly.termList.sort(greater<Term>());
return newPoly;
}
int main(void)
{
Polynomial a, b, d;
a.readPoly("input1.txt");
cout << "a=";
a.print();
b.readPoly("input2.txt");
cout << "b=";
b.print();
d=a;
cout << "d=";
d.print();
Polynomial e(b);
cout << "e=";
e.print();
cout << "a(2)=" << a(2) << endl;
if (a==b) cout << "a equals to b." << endl;
else cout << "a does not equal to b." << endl;
cout << "a+b=";
(a+b).print();
cout << "a-b=";
(a-b).print();
cout << "a*b=";
(a*b).print();
cout << "d+=b; d=";
d+=b;
d.print();
cout << "e-=a; e=";
e-=a;
e.print();
cout << "b+=10; b=";
b+=10;
b.print();
cout << "10+b=";
Polynomial temp;
temp=10+b;
temp.print();
cout << "10*b=";
(10*b).print();
cout << "b*10=";
(b*10).print();
cout << "e*=10, e=";
e*=10;
e.print();
return (0);
}
void div(const Polynomial &W, const Polynomial &P, Polynomial &Q, Polynomial &R){
R = W;
for(int i=R.deg();i>=0 && R.deg()>=P.deg();i--){
Q.setA(i-P.deg(), R.getA(i)/P.getA(P.deg()));
for(int a=P.deg();a>=0;a--){
R.setA((i-P.deg())+a, R.getA((i-P.deg())+a)-( P.getA(a) * Q.getA(i-P.deg()) ) );
}
}
}
// O(n^2) factoring
void Polynomial::factor()
{
/*
PRE-CONDITIONS:
The polynomial is of the correct format.
POST-CONDITIONS:
The polynomial will combine all like terms in preparation for either output or addition.
*/
Polynomial newPoly;
Term currentTerm;
int newCoefficient;
// Loop while there is a term left to check
while (termList.size() > 0)
{
bool foundLikeTerm = false;
// If there are no terms in the new polynomial add the first term
if (newPoly.getNumTerms() == 0)
{
// If the exponent is anything but 0, add the term as is.
if (termList.front().getExponent() != 0)
{
newPoly.addTermToList(termList.front());
termList.pop_front();
}
else{
// If the exponent is zero, drop the variable and add the coefficient
newPoly.addTermToList(Term(termList.front().getCoefficient()));
termList.pop_front();
}
}
else
{
// Set new current term to be checked against the new polynomial
currentTerm = termList.front();
// If the exponent of the term is zero, drop the variable
if (currentTerm.getExponent() == 0)
currentTerm = Term(termList.front().getCoefficient());
// Iterate through the new polynomial to check for matches of exponents
// If the exponents match add the terms together, else add to the end of the new poly
for (list<Term>::iterator iter = newPoly.termList.begin(); iter != newPoly.termList.end();)
{
// If the exponents match or they are both numbers with no variables, add them together
if ((iter->getExponent() == currentTerm.getExponent()) || (iter->getHasVariable() == false && currentTerm.getHasVariable() == false))
{
newCoefficient = iter->getCoefficient() + currentTerm.getCoefficient();
iter->setCoefficient(newCoefficient);
foundLikeTerm = true;
break;
}
else{
// Advance iterator if no matches are found on the current term
iter++;
}
}
// If it gets through the new poly without a match, add it to the new poly
if(foundLikeTerm == false)
newPoly.addTermToList(currentTerm);
termList.pop_front();
}
}
// Clear everything from old polynomial, update with the new polynomial
termList = newPoly.termList;
}
int main(int argc, char** argv)
{
FileManager fManager;
char c;
bool read = false, save = false;
string fileToRead, fileToSave;
double startPoint = 1.0;
Polynomial *poly;
//parse program's arguments
while((c = getopt(argc, argv, "r:s:p:t")) != -1)
{
switch(c)
{
case 'r':
//read polynomial from specified file
read = true;
fileToRead = optarg;
break;
case 's':
//save polynomial to specified file
save = true;
fileToSave = optarg;
break;
case 'p':
//define starting point for Newton's method
startPoint = atof(optarg);
break;
case 't':
//run tests
testsRun();
return 0;
}
}
if(read)
{
//read specified file to load polynomial
poly = fManager.readPoly(fileToRead);
if(!poly)
{
cout << "Error while loading polynomial\n";
return 0;
}
cout << *poly << endl;
}
else
{
//get polynomial string from standard input
string polyString;
getline(cin, polyString);
//parse polynomial string
poly = fManager.polyGetFromString(polyString);
if(!poly)
{
cout << "Error while creating polynomial\n";
return 0;
}
}
//calculate root of polynomial using Newton's method
Root result = poly->calculateRoot(startPoint);
cout << "result: " << result.getX() << ", " << result.getY() << endl;
if(save)
{
//save polynomial to specified file
if(!fManager.savePoly(poly, fileToSave))
{
cout << "Failed to save polynomial\n";
}
}
}
bool Cylinder::intersect(const Point3D &eye, const Point3D &dst, HitReporter &hr) const
{
// Part 1: test for intersection with the non-planar surface using the
// cylinder equation.
Polynomial<2> x, y, z;
const Vector3D ray = dst - eye;
x[0] = eye[0];
x[1] = ray[0];
y[0] = eye[1];
y[1] = ray[1];
z[0] = eye[2];
z[1] = ray[2];
const Polynomial<2> eqn = x * x + y * y + (-1);
double ts[2];
auto nhits = eqn.solve(ts);
if(nhits > 0)
{
for(int i = 0; i < nhits; i++)
{
const double t = ts[i];
const Point3D pt = eye + t * ray;
if(!predicate(pt))
continue;
const Vector3D normal(pt - Point3D(0, 0, pt[2]));
// Figure out the uv.
Vector3D u, v;
Point2D uv;
get_uv(pt, normal, uv, u, v);
if(!hr.report(ts[i], normal, uv, u, v))
return false;
}
}
// Next, test intersection with the two planar circle surfaces.
const int circles[] = {AAFACE_PZ, AAFACE_NZ};
const double offsets[] = {1., -1.};
for(int i = 0; i < NUMELMS(circles); i++)
{
const int face = circles[i];
const int axis = face / 2;
const double offset = offsets[i];
const double t = axis_aligned_plane_intersect(eye, ray, axis, offset);
if(t < numeric_limits<double>::max())
{
const Point3D p = eye + t * ray;
if(axis_aligned_circle_contains(p, face, 1.))
{
Vector3D normal;
normal[axis] = offset;
const double _u = 0.25 * ((face == AAFACE_PZ ? 1 : 3) + p[0]);
const double _v = 0.25 * (3 + p[1]);
Point2D uv(_u, _v);
Vector3D u(1, 0, 0);
Vector3D v(0, 1, 0);
if(!hr.report(t, normal, uv, u, v))
return false;
}
}
}
return true;
}
void CH02Test::testPolynomial(void)
{
Polynomial a;
a.NewTerm(3, 2);
a.NewTerm(2, 1);
a.NewTerm(4, 0);
TEST_ASSERT(a.Eval(1) == 9);
TEST_ASSERT(a.Eval(2) == 20);
Polynomial b;
b.NewTerm(1, 4);
b.NewTerm(10, 3);
b.NewTerm(3, 2);
TEST_ASSERT(b.Eval(1) == 14);
TEST_ASSERT(b.Eval(2) == 108);
Polynomial c;
c = a + b;
TEST_ASSERT(c.Eval(1) == 23);
TEST_ASSERT(c.Eval(2) == 128);
Polynomial d;
d = b + a;
TEST_ASSERT(d.Eval(1) == 23);
TEST_ASSERT(d.Eval(2) == 128);
}
int main()
{
ifstream myFile("dataPoly.txt");
string currC;
string currE;
string currL;
double currCoeff;
int currExpon;
int x = 0,y,z,pNum;
string numPoly;
Polynomial* polyArray[12];
for(int i = 0; i < 12; i++)
{
Polynomial* polyList = new Polynomial;
polyArray[i] = polyList;
}
///////////////////////////////////////////////////////////////////////////////////////////
if (myFile.is_open())
{
myFile >> numPoly;
pNum = atoi(numPoly.c_str());
for(int i = 0; i < pNum;i++)
{
Polynomial* polyList = polyArray[i];
myFile >> currC;
myFile >> currE;
currCoeff = atof(currC.c_str());
currExpon = atoi(currE.c_str());
polyList->insert(currCoeff,currExpon,polyArray[10]);
while(currExpon != 0)
{
myFile >> currC;
myFile >> currE;
currCoeff = atof(currC.c_str());
currExpon = atoi(currE.c_str());
polyList->insert(currCoeff,currExpon,polyArray[10]);
}
polyArray[x] = polyList;
x++;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
while(currL.compare("Q") != 0 && myFile >> currL)
{
if (currL.compare("R") == 0)
{
myFile >> currL;
int x = atoi(currL.c_str()) - 1;
polyArray[10]->remove(polyArray[x]);
Polynomial* polyList = polyArray[x];
myFile >> currC;
myFile >> currE;
currCoeff = atof(currC.c_str());
currExpon = atoi(currE.c_str());
polyList->insert(currCoeff,currExpon,polyArray[10]);
while(currExpon != 0)
{
myFile >> currC;
myFile >> currE;
currCoeff = atof(currC.c_str());
currExpon = atoi(currE.c_str());
polyList->insert(currCoeff,currExpon,polyArray[10]);
}
}
else if(currL.compare("A") == 0)
bool GroundPolyFitter<degree>::findLanes(const cv::Mat_<float> &img, Polynomial<degree>& retp)
{
cv::Size size = img.size();
if(nrows != size.width || ncols != size.height)
return false;
// use RANSAC to fit a polynomial to the ground points
std::vector<Point2D> bestConsensusSet;
double bestNormalizedSqError;
Polynomial bestPolynomial;
// temporary variables for passing data to fitting algorithms
// nrows * ncols is the maximum number of points that could be fit
double rowColX[nrows * ncols];
double rowColY[nrows * ncols];
CumulativeImageSampler sampler(img);
std::vector<Point2D> inliers;
for(int iterations = 0; iterations < RANSAC_NUM_ITERATIONS; iterations++) {
// first, take an initial random sample to be the first inliers
RowCol rcs[RANSAC_MIN_SAMPLE];
sampler.multiSample(rcs, RANSAC_MIN_SAMPLE);
for(int i = 0; i < RANSAC_MIN_SAMPLE; i++) {
inliers.push_back(convertPixelToGround(rcs[i]));
}
// find a model for the inliers
rowColVectorToArrays(inliers, rowColX, rowColY);
Polynomial<degree> inlierPoly = Polynomial<degree>::fitRegressionPoly(
rowColX, rowColY, RANSAC_MIN_SAMPLE);
// check the model with unused points to see if those are inliers too
std::vector<Point2D> remainingPoints;
std::set_difference(allPixels.begin(), allPixels.end(),
inliers.begin(), inliers.end(),
remainingPoints.begin());
std::vector<Point2D>::iterator it;
for(it = remainingPoints.begin(); it < remainingPoints.end(); it++) {
if(inlierPoly.sqDistance(it->getX(), it->getY()) < RANSAC_SQ_DISTANCE_THRESHOLD)
inliers.push_back(*it);
}
// fit new model with larger inlier set and compare to previous best
rowColVectorToArrays(inliers, rowColX, rowColY);
Polynomial<degree> newInlierPoly = Polynomial<degree>::fitRegressionPoly(
rowColX, rowColY, inliers.size());
double sumSqError = 0.0;
for(it = inliers.begin(); it < inliers.end(); it++) {
sumSqError += newInlierPoly.sqDistance(it->getX(), it->getY());
}
double normalizedSqError = sumSqError / inliers.size();
if(normalizedSqError < bestNormalizedSqError) {
bestNormalizedSqError = normalizedSqError;
bestConsensusSet = inliers;
bestPolynomial = newInlierPoly;
}
}
}
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();
}
}
