static SBasis my_inverse(SBasis f, int order){
double a0 = f[0][0];
if(a0 != 0) {
f -= a0;
}
double a1 = f[0][1];
if(a1 == 0)
THROW_NOTINVERTIBLE();
//assert(a1 != 0);// not invertible.
if(a1 != 1) {
f /= a1;
}
SBasis g=SBasis(order, Linear());
g[0] = Linear(0,1);
double df0=derivative(f)(0);
double df1=derivative(f)(1);
SBasis r = Linear(0,1)-g(f);
for(int i=1; i<order; i++){
//std::cout<<"i: "<<i<<std::endl;
r=Linear(0,1)-g(f);
//std::cout<<"t-gof="<<r<<std::endl;
r.normalize();
if (r.size()==0) return(g);
double a=r[i][0]/pow(df0,i);
double b=r[i][1]/pow(df1,i);
g[i] = Linear(a,b);
}
return(g);
}
/** Compute the cosine of a function.
\param f function
\param tol maximum error
\param order maximum degree polynomial to use
*/
Piecewise<SBasis> cos( SBasis const &f, double tol, int order){
double alpha = (f.at0()+f.at1())/2.;
SBasis x = f-alpha;
double d = x.tailError(0),err=1;
//estimate cos(x)-sum_0^order (-1)^k x^2k/2k! by the first neglicted term
for (int i=1; i<=2*order; i++) err*=d/i;
if (err<tol){
SBasis xk=Linear(1), c=Linear(1), s=Linear(0);
for (int k=1; k<=2*order; k+=2){
xk*=x/k;
//take also truncature errors into account...
err+=xk.tailError(order);
xk.truncate(order);
s+=xk;
xk*=-x/(k+1);
//take also truncature errors into account...
err+=xk.tailError(order);
xk.truncate(order);
c+=xk;
}
if (err<tol){
return Piecewise<SBasis>(std::cos(alpha)*c-std::sin(alpha)*s);
}
}
Piecewise<SBasis> c0,c1;
c0 = cos(compose(f,Linear(0.,.5)),tol,order);
c1 = cos(compose(f,Linear(.5,1.)),tol,order);
c0.setDomain(Interval(0.,.5));
c1.setDomain(Interval(.5,1.));
c0.concat(c1);
return c0;
}
Piecewise<SBasis> convole(SBasisOf<double> const &f, Interval dom_f,
SBasisOf<double> const &g, Interval dom_g,
bool f_cst_ends = false){
if ( dom_f.extent() < dom_g.extent() ) return convole(g, dom_g, f, dom_f);
Piecewise<SBasis> result;
SBasisOf<SBasisOf<double> > u,v;
u.push_back(LinearOf<SBasisOf<double> >(SBasisOf<double>(LinearOf<double>(0,1))));
v.push_back(LinearOf<SBasisOf<double> >(SBasisOf<double>(LinearOf<double>(0,0)),
SBasisOf<double>(LinearOf<double>(1,1))));
SBasisOf<SBasisOf<double> > v_u = (v - u)*(dom_f.extent()/dom_g.extent());
v_u += SBasisOf<SBasisOf<double> >(SBasisOf<double>(-dom_g.min()/dom_g.extent()));
SBasisOf<SBasisOf<double> > gg = multi_compose(g,v_u);
SBasisOf<SBasisOf<double> > ff = SBasisOf<SBasisOf<double> >(f);
SBasisOf<SBasisOf<double> > hh = integral(ff*gg,0);
result.cuts.push_back(dom_f.min()+dom_g.min());
//Note: we know dom_f.extent() >= dom_g.extent()!!
//double rho = dom_f.extent()/dom_g.extent();
double t0 = dom_g.min()/dom_f.extent();
double t1 = dom_g.max()/dom_f.extent();
double t2 = t0+1;
double t3 = t1+1;
SBasisOf<double> a,b,t;
SBasis seg;
a = SBasisOf<double>(LinearOf<double>(0,0));
b = SBasisOf<double>(LinearOf<double>(0,t1-t0));
t = SBasisOf<double>(LinearOf<double>(t0,t1));
seg = toSBasis(compose(hh,b,t)-compose(hh,a,t));
result.push(seg,dom_f.min() + dom_g.max());
if (dom_f.extent() > dom_g.extent()){
a = SBasisOf<double>(LinearOf<double>(0,t2-t1));
b = SBasisOf<double>(LinearOf<double>(t1-t0,1));
t = SBasisOf<double>(LinearOf<double>(t1,t2));
seg = toSBasis(compose(hh,b,t)-compose(hh,a,t));
result.push(seg,dom_f.max() + dom_g.min());
}
a = SBasisOf<double>(LinearOf<double>(t2-t1,1.));
b = SBasisOf<double>(LinearOf<double>(1.,1.));
t = SBasisOf<double>(LinearOf<double>(t2,t3));
seg = toSBasis(compose(hh,b,t)-compose(hh,a,t));
result.push(seg,dom_f.max() + dom_g.max());
result*=dom_f.extent();
//--constant ends correction--------------
if (f_cst_ends){
SBasis ig = toSBasis(integraaal(g))*dom_g.extent();
ig -= ig.at0();
Piecewise<SBasis> cor;
cor.cuts.push_back(dom_f.min()+dom_g.min());
cor.push(reverse(ig)*f.at0(),dom_f.min()+dom_g.max());
cor.push(Linear(0),dom_f.max()+dom_g.min());
cor.push(ig*f.at1(),dom_f.max()+dom_g.max());
result+=cor;
}
//----------------------------------------
return result;
}
//see a sb2d as an sb of u with coef in sbasis of v.
void
u_coef(SBasis2d f, unsigned deg, SBasis &a, SBasis &b) {
a = SBasis();
b = SBasis();
for (unsigned v=0; v<f.vs; v++){
a.push_back(Linear(f.index(deg,v)[0], f.index(deg,v)[2]));
b.push_back(Linear(f.index(deg,v)[1], f.index(deg,v)[3]));
}
}
void
v_coef(SBasis2d f, unsigned deg, SBasis &a, SBasis &b) {
a = SBasis();
b = SBasis();
for (unsigned u=0; u<f.us; u++){
a.push_back(Linear(f.index(deg,u)[0], f.index(deg,u)[1]));
b.push_back(Linear(f.index(deg,u)[2], f.index(deg,u)[3]));
}
}
Interval bounds_exact(SBasis const &a) {
Interval result = Interval(a.at0(), a.at1());
SBasis df = derivative(a);
vector<double>extrema = roots(df);
for (unsigned i=0; i<extrema.size(); i++){
result.extendTo(a(extrema[i]));
}
return result;
}
SBasis dotp(Point const& p, D2<SBasis> const& c)
{
SBasis d;
d.resize(c[X].size());
for ( unsigned int i = 0; i < c[0].size(); ++i )
{
for( unsigned int j = 0; j < 2; ++j )
d[i][j] = p[X] * c[X][i][j] + p[Y] * c[Y][i][j];
}
return d;
}
/** Changes the basis of p to be bernstein.
\param p the Symmetric basis polynomial
\returns the Bernstein basis polynomial
if the degree is even q is the order in the symmetrical power basis,
if the degree is odd q is the order + 1
n is always the polynomial degree, i. e. the Bezier order
sz is the number of bezier handles.
*/
void sbasis_to_bezier (Bezier & bz, SBasis const& sb, size_t sz)
{
if (sb.size() == 0) {
THROW_RANGEERROR("size of sb is too small");
}
size_t q, n;
bool even;
if (sz == 0)
{
q = sb.size();
if (sb[q-1][0] == sb[q-1][1])
{
even = true;
--q;
n = 2*q;
}
else
{
even = false;
n = 2*q-1;
}
}
else
{
q = (sz > 2*sb.size()-1) ? sb.size() : (sz+1)/2;
n = sz-1;
even = false;
}
bz.clear();
bz.resize(n+1);
double Tjk;
for (size_t k = 0; k < q; ++k)
{
for (size_t j = k; j < n-k; ++j) // j <= n-k-1
{
Tjk = binomial(n-2*k-1, j-k);
bz[j] += (Tjk * sb[k][0]);
bz[n-j] += (Tjk * sb[k][1]); // n-k <-> [k][1]
}
}
if (even)
{
bz[q] += sb[q][0];
}
// the resulting coefficients are with respect to the scaled Bernstein
// basis so we need to divide them by (n, j) binomial coefficient
for (size_t j = 1; j < n; ++j)
{
bz[j] /= binomial(n, j);
}
bz[0] = sb[0][0];
bz[n] = sb[0][1];
}
double Laguerre_internal(SBasis const & p,
double x0,
double tol,
bool & quad_root) {
double a = 2*tol;
double xk = x0;
double n = p.size();
quad_root = false;
while(a > tol) {
//std::cout << "xk = " << xk << std::endl;
Linear b = p.back();
Linear d(0), f(0);
double err = fabs(b);
double abx = fabs(xk);
for(int j = p.size()-2; j >= 0; j--) {
f = xk*f + d;
d = xk*d + b;
b = xk*b + p[j];
err = fabs(b) + abx*err;
}
err *= 1e-7; // magic epsilon for convergence, should be computed from tol
double px = b;
if(fabs(b) < err)
return xk;
//if(std::norm(px) < tol*tol)
// return xk;
double G = d / px;
double H = G*G - f / px;
//std::cout << "G = " << G << "H = " << H;
double radicand = (n - 1)*(n*H-G*G);
//assert(radicand.real() > 0);
if(radicand < 0)
quad_root = true;
//std::cout << "radicand = " << radicand << std::endl;
if(G.real() < 0) // here we try to maximise the denominator avoiding cancellation
a = - std::sqrt(radicand);
else
a = std::sqrt(radicand);
//std::cout << "a = " << a << std::endl;
a = n / (a + G);
//std::cout << "a = " << a << std::endl;
xk -= a;
}
//std::cout << "xk = " << xk << std::endl;
return xk;
}
Interval bounds_local(const SBasis &sb, const Interval &i, int order) {
double t0=i.min(), t1=i.max(), lo=0., hi=0.;
for(int j = sb.size()-1; j>=order; j--) {
double a=sb[j][0];
double b=sb[j][1];
double t = 0;
if (lo<0) t = ((b-a)/lo+1)*0.5;
if (lo>=0 || t<t0 || t>t1) {
lo = std::min(a*(1-t0)+b*t0+lo*t0*(1-t0),a*(1-t1)+b*t1+lo*t1*(1-t1));
}else{
lo = lerp(t, a+lo*t, b);
}
if (hi>0) t = ((b-a)/hi+1)*0.5;
if (hi<=0 || t<t0 || t>t1) {
hi = std::max(a*(1-t0)+b*t0+hi*t0*(1-t0),a*(1-t1)+b*t1+hi*t1*(1-t1));
}else{
hi = lerp(t, a+hi*t, b);
}
}
Interval res = Interval(lo,hi);
if (order>0) res*=pow(.25,order);
return res;
}
Interval bounds_fast(const SBasis &sb, int order) {
Interval res;
for(int j = sb.size()-1; j>=order; j--) {
double a=sb[j][0];
double b=sb[j][1];
double v, t = 0;
v = res[0];
if (v<0) t = ((b-a)/v+1)*0.5;
if (v>=0 || t<0 || t>1) {
res[0] = std::min(a,b);
}else{
res[0]=lerp(t, a+v*t, b);
}
v = res[1];
if (v>0) t = ((b-a)/v+1)*0.5;
if (v<=0 || t<0 || t>1) {
res[1] = std::max(a,b);
}else{
res[1]=lerp(t, a+v*t, b);
}
}
if (order>0) res*=pow(.25,order);
return res;
}
SBasis extract_u(SBasis2d const &a, double u) {
SBasis sb;
double s = u*(1-u);
for(unsigned vi = 0; vi < a.vs; vi++) {
double sk = 1;
Linear bo(0,0);
for(unsigned ui = 0; ui < a.us; ui++) {
bo += (extract_u(a.index(ui, vi), u))*sk;
sk *= s;
}
sb.push_back(bo);
}
return sb;
}
SBasisOf<double> toSBasisOfDouble(SBasis const &f){
SBasisOf<double> result;
for (unsigned i=0; i<f.size(); i++){
result.push_back(LinearOf<double>(f[i][0],f[i][1]));
}
return result;
}
/** Find all t s.t s(t) = 0
\param a sbasis function
\returns vector of zeros (roots)
*/
std::vector<double> roots(SBasis const & s) {
switch(s.size()) {
case 0:
return std::vector<double>();
case 1:
return roots1(s);
default:
{
Bezier bz;
sbasis_to_bezier(bz, s);
return bz.roots();
}
}
}
void subdiv_sbasis(SBasis const & s,
std::vector<double> & roots,
double left, double right) {
Interval bs = bounds_fast(s);
if(bs.min() > 0 || bs.max() < 0)
return; // no roots here
if(s.tailError(1) < 1e-7) {
double t = s[0][0] / (s[0][0] - s[0][1]);
roots.push_back(left*(1-t) + t*right);
return;
}
double middle = (left + right)/2;
subdiv_sbasis(compose(s, Linear(0, 0.5)), roots, left, middle);
subdiv_sbasis(compose(s, Linear(0.5, 1.)), roots, middle, right);
}
/**
* Centroid using sbasis integration.
\param p the Element.
\param centroid on return contains the centroid of the shape
\param area on return contains the signed area of the shape.
\relates Piecewise
This approach uses green's theorem to compute the area and centroid using integrals. For curved shapes this is much faster than converting to polyline. Note that without an uncross operation the output is not the absolute area.
* Returned values:
0 for normal execution;
2 if area is zero, meaning centroid is meaningless.
*/
unsigned Geom::centroid(Piecewise<D2<SBasis> > const &p, Point& centroid, double &area) {
Point centroid_tmp(0,0);
double atmp = 0;
for(unsigned i = 0; i < p.size(); i++) {
SBasis curl = dot(p[i], rot90(derivative(p[i])));
SBasis A = integral(curl);
D2<SBasis> C = integral(multiply(curl, p[i]));
atmp += A.at1() - A.at0();
centroid_tmp += C.at1()- C.at0(); // first moment.
}
// join ends
centroid_tmp *= 2;
Point final = p[p.size()-1].at1(), initial = p[0].at0();
const double ai = cross(final, initial);
atmp += ai;
centroid_tmp += (final + initial)*ai; // first moment.
area = atmp / 2;
if (atmp != 0) {
centroid = centroid_tmp / (3 * atmp);
return 0;
}
return 2;
}
/** Changes the basis of p to be sbasis.
\param p the Bernstein basis polynomial
\returns the Symmetric basis polynomial
if the degree is even q is the order in the symmetrical power basis,
if the degree is odd q is the order + 1
n is always the polynomial degree, i. e. the Bezier order
*/
void bezier_to_sbasis (SBasis & sb, Bezier const& bz)
{
size_t n = bz.order();
size_t q = (n+1) / 2;
size_t even = (n & 1u) ? 0 : 1;
sb.clear();
sb.resize(q + even, Linear(0, 0));
double Tjk;
for (size_t k = 0; k < q; ++k)
{
for (size_t j = k; j < q; ++j)
{
Tjk = sgn(j, k) * binomial(n-j-k, j-k) * binomial(n, k);
sb[j][0] += (Tjk * bz[k]);
sb[j][1] += (Tjk * bz[n-k]); // n-j <-> [j][1]
}
for (size_t j = k+1; j < q; ++j)
{
Tjk = sgn(j, k) * binomial(n-j-k-1, j-k-1) * binomial(n, k);
sb[j][0] += (Tjk * bz[n-k]);
sb[j][1] += (Tjk * bz[k]); // n-j <-> [j][1]
}
}
if (even)
{
for (size_t k = 0; k < q; ++k)
{
Tjk = sgn(q,k) * binomial(n, k);
sb[q][0] += (Tjk * (bz[k] + bz[n-k]));
}
sb[q][0] += (binomial(n, q) * bz[q]);
sb[q][1] = sb[q][0];
}
sb[0][0] = bz[0];
sb[0][1] = bz[n];
}
//------------------------------------------------------------------------------
static SBasis divide_by_sk(SBasis const &a, int k) {
if ( k>=(int)a.size()){
//make sure a is 0?
return SBasis();
}
if(k < 0) return shift(a,-k);
SBasis c;
c.insert(c.begin(), a.begin()+k, a.end());
return c;
}
SBasis integral(SBasis const &c) {
SBasis a;
a.resize(c.size() + 1, Linear(0,0));
a[0] = Linear(0,0);
for(unsigned k = 1; k < c.size() + 1; k++) {
double ahat = -c[k-1].tri()/(2*k);
a[k][0] = a[k][1] = ahat;
}
double aTri = 0;
for(int k = c.size()-1; k >= 0; k--) {
aTri = (c[k].hat() + (k+1)*aTri/2)/(2*k+1);
a[k][0] -= aTri/2;
a[k][1] += aTri/2;
}
a.normalize();
return a;
}
std::vector<double> roots(SBasis const & s) {
if(s.size() == 0) return std::vector<double>();
return sbasis_to_bezier(s).roots();
}
static void multi_roots_internal(SBasis const &f,
SBasis const &df,
std::vector<double> const &levels,
std::vector<std::vector<double> > &roots,
double htol,
double vtol,
double a,
double fa,
double b,
double fb){
if (f.size()==0){
int idx;
idx=upper_level(levels,0,vtol);
if (idx<(int)levels.size()&&fabs(levels.at(idx))<=vtol){
roots[idx].push_back(a);
roots[idx].push_back(b);
}
return;
}
////usefull?
// if (f.size()==1){
// int idxa=upper_level(levels,fa);
// int idxb=upper_level(levels,fb);
// if (fa==fb){
// if (fa==levels[idxa]){
// roots[a]=idxa;
// roots[b]=idxa;
// }
// return;
// }
// int idx_min=std::min(idxa,idxb);
// int idx_max=std::max(idxa,idxb);
// if (idx_max==levels.size()) idx_max-=1;
// for(int i=idx_min;i<=idx_max; i++){
// double t=a+(b-a)*(levels[i]-fa)/(fb-fa);
// if(a<t&&t<b) roots[t]=i;
// }
// return;
// }
if ((b-a)<htol){
//TODO: use different tol for t and f ?
//TODO: unsigned idx ? (remove int casts when fixed)
int idx=std::min(upper_level(levels,fa,vtol),upper_level(levels,fb,vtol));
if (idx==(int)levels.size()) idx-=1;
double c=levels.at(idx);
if((fa-c)*(fb-c)<=0||fabs(fa-c)<vtol||fabs(fb-c)<vtol){
roots[idx].push_back((a+b)/2);
}
return;
}
int idxa=upper_level(levels,fa,vtol);
int idxb=upper_level(levels,fb,vtol);
Interval bs = bounds_local(df,Interval(a,b));
//first times when a level (higher or lower) can be reached from a or b.
double ta_hi,tb_hi,ta_lo,tb_lo;
ta_hi=ta_lo=b+1;//default values => no root there.
tb_hi=tb_lo=a-1;//default values => no root there.
if (idxa<(int)levels.size() && fabs(fa-levels.at(idxa))<vtol){//a can be considered a root.
//ta_hi=ta_lo=a;
roots[idxa].push_back(a);
ta_hi=ta_lo=a+htol;
}else{
if (bs.max()>0 && idxa<(int)levels.size())
ta_hi=a+(levels.at(idxa )-fa)/bs.max();
if (bs.min()<0 && idxa>0)
ta_lo=a+(levels.at(idxa-1)-fa)/bs.min();
}
if (idxb<(int)levels.size() && fabs(fb-levels.at(idxb))<vtol){//b can be considered a root.
//tb_hi=tb_lo=b;
roots[idxb].push_back(b);
tb_hi=tb_lo=b-htol;
}else{
if (bs.min()<0 && idxb<(int)levels.size())
tb_hi=b+(levels.at(idxb )-fb)/bs.min();
if (bs.max()>0 && idxb>0)
tb_lo=b+(levels.at(idxb-1)-fb)/bs.max();
}
double t0,t1;
t0=std::min(ta_hi,ta_lo);
t1=std::max(tb_hi,tb_lo);
//hum, rounding errors frighten me! so I add this +tol...
if (t0>t1+htol) return;//no root here.
if (fabs(t1-t0)<htol){
multi_roots_internal(f,df,levels,roots,htol,vtol,t0,f(t0),t1,f(t1));
}else{
double t,t_left,t_right,ft,ft_left,ft_right;
t_left =t_right =t =(t0+t1)/2;
ft_left=ft_right=ft=f(t);
int idx=upper_level(levels,ft,vtol);
if (idx<(int)levels.size() && fabs(ft-levels.at(idx))<vtol){//t can be considered a root.
roots[idx].push_back(t);
//we do not want to count it twice (from the left and from the right)
t_left =t-htol/2;
t_right=t+htol/2;
ft_left =f(t_left);
ft_right=f(t_right);
}
multi_roots_internal(f,df,levels,roots,htol,vtol,t0 ,f(t0) ,t_left,ft_left);
multi_roots_internal(f,df,levels,roots,htol,vtol,t_right,ft_right,t1 ,f(t1) );
}
}
//-Sqrt----------------------------------------------------------
static Piecewise<SBasis> sqrt_internal(SBasis const &f,
double tol,
int order){
SBasis sqrtf;
if(f.isZero() || order == 0){
return Piecewise<SBasis>(sqrtf);
}
if (f.at0()<-tol*tol && f.at1()<-tol*tol){
return sqrt_internal(-f,tol,order);
}else if (f.at0()>tol*tol && f.at1()>tol*tol){
sqrtf.resize(order+1, Linear(0,0));
sqrtf[0] = Linear(std::sqrt(f[0][0]), std::sqrt(f[0][1]));
SBasis r = f - multiply(sqrtf, sqrtf); // remainder
for(unsigned i = 1; int(i) <= order && i<r.size(); i++) {
Linear ci(r[i][0]/(2*sqrtf[0][0]), r[i][1]/(2*sqrtf[0][1]));
SBasis cisi = shift(ci, i);
r -= multiply(shift((sqrtf*2 + cisi), i), SBasis(ci));
r.truncate(order+1);
sqrtf[i] = ci;
if(r.tailError(i) == 0) // if exact
break;
}
}else{
sqrtf = Linear(std::sqrt(fabs(f.at0())), std::sqrt(fabs(f.at1())));
}
double err = (f - multiply(sqrtf, sqrtf)).tailError(0);
if (err<tol){
return Piecewise<SBasis>(sqrtf);
}
Piecewise<SBasis> sqrtf0,sqrtf1;
sqrtf0 = sqrt_internal(compose(f,Linear(0.,.5)),tol,order);
sqrtf1 = sqrt_internal(compose(f,Linear(.5,1.)),tol,order);
sqrtf0.setDomain(Interval(0.,.5));
sqrtf1.setDomain(Interval(.5,1.));
sqrtf0.concat(sqrtf1);
return sqrtf0;
}
