/** 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;
}
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);
}
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;
}
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;
}
//------------------------------------------------------------------------------
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;
}
/** 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();
}
}
}
//-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;
}
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) );
}
}
