#include <cfloat>
#include <Rcpp.h>
namespace rstpm2 {
using namespace R;
using namespace Rcpp;
// List vunirootRcpp(Function f, NumericVector lower, NumericVector upper, int numiter, double tol) {
RcppExport SEXP vunirootRcpp(SEXP __f, SEXP __lower, SEXP __upper, SEXP __fa, SEXP __fb, SEXP __numiter, SEXP __tol) {
Rcpp::Function f = as<Rcpp::Function>(__f);
NumericVector lower = clone(as<NumericVector>(__lower));
NumericVector upper = clone(as<NumericVector>(__upper));
NumericVector fa = clone(as<NumericVector>(__fa));
NumericVector fb = clone(as<NumericVector>(__fb));
int numiter = as<int>(__numiter);
double tol = as<double>(__tol);
int size = lower.size();
NumericVector a(clone(lower)), b(clone(upper)), c(clone(a)), Tol(size,0.0);
NumericVector fc(clone(fa));
LogicalVector converged(size,false);
IntegerVector ns(size,-1);
int i;
/* First test if we have found a root at an endpoint */
for(i=0; i<size; i++) {
if (fa[i]==0.0) {
converged[i]=true;
b[i]=a[i];
}
if (fb[i]==0.0) {
converged[i]=true;
}
}
for (int n = 1; n<=numiter; n++) {
for(i=0; i<size; i++) {
if (!converged[i]) {
double prev_step = b[i]-a[i]; /* Distance from the last but one
to the last approximation */
double tol_act; /* Actual tolerance */
double p; /* Interpolation step is calcu- */
double q; /* lated in the form p/q; divi-
* sion operations is delayed
* until the last moment */
double new_step; /* Step at this iteration */
if( fabs(fc[i]) < fabs(fb[i]) )
{ /* Swap data for b to be the */
a[i] = b[i]; b[i] = c[i]; c[i] = a[i]; /* best approximation */
fa[i]=fb[i]; fb[i]=fc[i]; fc[i]=fa[i];
}
tol_act = 2*DBL_EPSILON*fabs(b[i]) + tol/2;
new_step = (c[i]-b[i])/2.0;
if( fabs(new_step) <= tol_act || fb[i] == (double)0 )
{
// *Maxit -= maxit;
Tol[i] = fabs(c[i]-b[i]);
converged[i] = true; /* Acceptable approx. is found */
ns[i] = n;
} else {
/* Decide if the interpolation can be tried */
if( fabs(prev_step) >= tol_act /* If prev_step was large enough*/
&& fabs(fa[i]) > fabs(fb[i]) ) { /* and was in true direction,
* Interpolation may be tried */
double t1,cb,t2;
cb = c[i]-b[i];
if( a[i]==c[i] ) { /* If we have only two distinct */
/* points linear interpolation */
t1 = fb[i]/fa[i]; /* can only be applied */
p = cb*t1;
q = 1.0 - t1;
}
else { /* Quadric inverse interpolation*/
q = fa[i]/fc[i]; t1 = fb[i]/fc[i]; t2 = fb[i]/fa[i];
p = t2 * ( cb*q*(q-t1) - (b[i]-a[i])*(t1-1.0) );
q = (q-1.0) * (t1-1.0) * (t2-1.0);
}
if( p>(double)0 ) /* p was calculated with the */
q = -q; /* opposite sign; make p positive */
else /* and assign possible minus to */
p = -p; /* q */
if( p < (0.75*cb*q-fabs(tol_act*q)/2) /* If b+p/q falls in [b,c]*/
&& p < fabs(prev_step*q/2) ) /* and isnt too large */
new_step = p/q; /* it is accepted
* If p/q is too large then the
* bisection procedure can
* reduce [b,c] range to more
* extent */
}
if( fabs(new_step) < tol_act) { /* Adjust the step to be not less*/
if( new_step > (double)0 ) /* than tolerance */
new_step = tol_act;
else
new_step = -tol_act;
}
a[i] = b[i]; fa[i] = fb[i]; /* Save the previous approx. */
b[i] += new_step; // fb = (*f)(b, info); /* Do step to a new approxim. */
}
}
}
if (is_true(all(converged))) break;
fb = f(b);
for(i=0; i<size; i++) {
if( (fb[i] > 0 && fc[i] > 0) || (fb[i] < 0 && fc[i] < 0) ) {
/* Adjust c for it to have a sign opposite to that of b */
c[i] = a[i]; fc[i] = fa[i];
}
}
}
if (is_false(all(converged)))
for (i=0; i<size; i++)
if (!converged[i]) {
Tol[i]=fabs(c[i]-b[i]);
ns[i] = -1;
}
return wrap(List::create(_("root") = b, _("iter") = ns, _("tol")=Tol));
}
} // namespace