24 #include "Base/Common/W32Compat.hh"
25 #include <bias_config.h>
26 #include "PolynomialSolve.hh"
30 #include <gsl/gsl_poly.h>
31 #include <gsl/gsl_errno.h>
34 #include <Base/Common/BIASpragma.hh>
35 #include <Base/Common/CompareFloatingPoint.hh>
36 #include <MathAlgo/Laguerre.hh>
37 #include <MathAlgo/SVD.hh>
44 #define DOUBLE_MIN 1e-6
45 #define PS_DOUBLE_EPSILON DBL_EPSILON
48 vector<POLYNOMIALSOLVE_TYPE>& sol)
51 BIASASSERT(!ocoeff.empty());
53 vector<POLYNOMIALSOLVE_TYPE> coeff = ocoeff;
54 CheckCoefficients(coeff);
56 int deg=(int)coeff.size()-1;
63 case -1: sol.push_back(0);
67 num = Linear(coeff, sol);
70 num = Quadratic(coeff, sol);
73 num = Cubic(coeff, sol);
76 num = Quartic(coeff, sol);
79 return Numeric(ocoeff, sol);
82 NonLinearRefine(coeff,sol);
88 vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
90 int deg=(int)coeff.size()-1;
99 num = Linear(coeff, sol);
102 num = Quadratic(coeff, sol);
105 num = Cubic(coeff, sol);
108 num = Numeric(coeff, sol);
116 vector<POLYNOMIALSOLVE_TYPE>& sol)
118 vector<complex<POLYNOMIALSOLVE_TYPE> > csol;
119 int res=Numeric(coeff, csol), num=0;
121 sol.reserve(coeff.size()-1);
123 for (
int i=0; i<res; i++){
124 if (csol[i].imag()==0.0){
125 sol.push_back(csol[i].real());
136 vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
138 int deg=(int)coeff.size()-1;
140 if (coeff[deg]==0.0){
141 BIASERR(
"leading coefficient is zero, aborting"););
144 BIASASSERT(coeff[deg]!=0.0);
153 for (
unsigned i=0; i<coeff.size(); i++){
154 BIASCDOUT(D_PS_GSL, setw(3)<<i<<
"th coeff: "<<coeff[i]<<endl);
158 double *a=
new double[n];
159 double *z=
new double[2*deg];
160 gsl_poly_complex_workspace *w=gsl_poly_complex_workspace_alloc(n);
162 for (
unsigned i=0; i<n; i++) a[i]=coeff[i];
163 int res = gsl_poly_complex_solve(a, n, w, z);
164 gsl_poly_complex_workspace_free(w);
165 if (res==GSL_SUCCESS){
169 for (
unsigned i=0; i<n; i++){
170 sol.push_back(complex<double>(z[i<<1], z[(i<<1)+1]));
171 BIASCDOUT(D_PS_GSL, setw(3)<<i<<
"th solution : "<< *sol.rbegin()
172 <<
"\tpoly("<<z[i<<1]<<
") = "
173 <<gsl_poly_eval(a, n+1, z[i<<1]) << endl);
199 for (
unsigned int i=0; i<sol.size(); i++)
202 if (
Equal(sol[i].imag(),0.0,10e-11))
204 sol[i]=std::complex< double>(sol[i].real(),0.0);
225 vector<POLYNOMIALSOLVE_TYPE>& sol)
227 int deg=(int)coeff.size()-1;
236 num = Linear(coeff, sol);
239 num = Quadratic(coeff, sol);
242 num = Cubic(coeff, sol);
245 num = Quartic(coeff, sol);
248 BIASERR(
"something strange happened");
256 vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
258 int deg=(int)coeff.size()-1;
267 num = Linear(coeff, sol);
270 num = Quadratic(coeff, sol);
273 num = Cubic(coeff, sol);
276 BIASERR(
"something strange happened");
290 vector<POLYNOMIALSOLVE_TYPE>& sol)
292 int deg=(int)coeff.size()-1;
294 if(deg!=1 || coeff[deg]==0.0){
295 BIASERR(
"coeff is not a polynom of first order");
299 BIASASSERT(coeff[deg]!=0.0);
303 sol[0] = -coeff[0] / coeff[1];
309 vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
311 vector<POLYNOMIALSOLVE_TYPE> rsol;
312 int deg=Linear(coeff, rsol);
315 sol[0] = complex<POLYNOMIALSOLVE_TYPE>(rsol[0], 0.0);
323 vector<POLYNOMIALSOLVE_TYPE>& sol)
326 int deg=(int)coeff.size()-1;
328 if(deg!=2 || coeff[deg]==0.0){
329 BIASERR(
"coeff is not a polynom of second order");
333 BIASASSERT(coeff[deg]!=0.0);
339 POLYNOMIALSOLVE_TYPE s[2];
340 num=gsl_poly_solve_quadratic(coeff[2], coeff[1],coeff[0],&(s[0]),&(s[1]));
341 if (num==2 && s[0]==s[1]) num=1;
342 for (
int i=0; i<num; i++) sol.push_back(s[i]);
345 POLYNOMIALSOLVE_TYPE q1 = coeff[1] / (coeff[2] * 2.0);
346 POLYNOMIALSOLVE_TYPE q0 = coeff[0] / coeff[2];
349 POLYNOMIALSOLVE_TYPE discriminant = q1*q1 - q0;
350 POLYNOMIALSOLVE_TYPE epsilon_d =
351 (fabs(discriminant)+7.0*q1*q1+3.0*fabs(q0)) * DBL_EPSILON;
354 if (discriminant < -epsilon_d) {
356 }
else if (discriminant <= epsilon_d) {
363 POLYNOMIALSOLVE_TYPE FirstRoot =
364 ((q1>0.0)?-sqrt(discriminant):sqrt(discriminant)) - q1;
365 sol.push_back(FirstRoot);
368 sol.push_back(q0/FirstRoot);
376 vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
378 int deg=(int)coeff.size()-1;
380 if(deg!=2 || coeff[deg]==0.0){
381 BIASERR(
"coeff is not a polynom of second order");
385 BIASASSERT(coeff[deg]!=0.0);
391 num = gsl_poly_complex_solve_quadratic(coeff[2], coeff[1], coeff[0],
392 (gsl_complex*)&(z[0]),
393 (gsl_complex*)&(z[2]));
394 sol[0]=complex<POLYNOMIALSOLVE_TYPE>(z[0], z[1]);
395 sol[1]=complex<POLYNOMIALSOLVE_TYPE>(z[2], z[3]);
397 BIASERR(
"Quadratic(vector<double>, vbector<complex<double>>) only "
398 <<
"implemented with gsl");
409 std::vector<POLYNOMIALSOLVE_TYPE>& sol)
412 int deg=(int)coeff.size()-1;
413 if(deg!=3 || coeff[deg]==0.0){
414 BIASERR(
"coeff is not a polynom of third order");
418 BIASASSERT(coeff[deg]!=0.0);
423 POLYNOMIALSOLVE_TYPE msol[3];
424 num = gsl_poly_solve_cubic(coeff[2]/coeff[3], coeff[1]/coeff[3],
425 coeff[0]/coeff[3], &(msol[0]),
426 &(msol[1]), &(msol[2]));
427 for (
int i=0; i<num; i++)
428 sol.push_back(msol[i]);
431 POLYNOMIALSOLVE_TYPE a0 = coeff[0] / coeff[3];
432 POLYNOMIALSOLVE_TYPE a1 = coeff[1] / coeff[3];
436 POLYNOMIALSOLVE_TYPE a2 = coeff[2] / (coeff[3] * 3.0);
438 POLYNOMIALSOLVE_TYPE q = a1 / 3.0 - a2*a2;
439 POLYNOMIALSOLVE_TYPE r = ( a1*a2 - a0 ) * 0.5 - a2*a2*a2 ;
442 if (fabs(r)<PS_DOUBLE_EPSILON) {
445 if (q>-PS_DOUBLE_EPSILON) {
452 POLYNOMIALSOLVE_TYPE t = sqrt( -3.0 * q );
462 POLYNOMIALSOLVE_TYPE d = q*q*q + r*r;
465 if (fabs(d) < PS_DOUBLE_EPSILON) {
477 POLYNOMIALSOLVE_TYPE s1,s2;
484 sol.push_back(s1 + s2 - a2);
491 q = atan2(d,r) / 3.0;
492 r = cbrt(hypot(d,r));
494 POLYNOMIALSOLVE_TYPE s = 2.0 * r * cos(q);
495 POLYNOMIALSOLVE_TYPE t = fabs(2.0 * r * sin(q)) * sin(60.0*M_PI/180.0);
499 sol[1] = -0.5*s -a2 - t;
500 sol[2] = -0.5*s -a2 + t;
509 vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
512 int deg=(int)coeff.size()-1;
513 if(deg!=3 || coeff[deg]==0.0){
514 BIASERR(
"coeff is not a polynom of third order");
518 BIASASSERT(coeff[deg]!=0.0);
523 num = gsl_poly_complex_solve_cubic(coeff[2]/coeff[3], coeff[1]/coeff[3],
524 coeff[0]/coeff[3], (gsl_complex*)&(z[0]),
525 (gsl_complex*)&(z[2]),
526 (gsl_complex*)&(z[4]));
527 for (
int i=0; i<num; i++){
528 sol.push_back(complex<POLYNOMIALSOLVE_TYPE>(z[2*i], z[2*i+1]));
535 BIASERR(
"gsl needed for PolynomialSolve::Cubic(vector, vector<complex>)");
543 vector<POLYNOMIALSOLVE_TYPE>& sol)
546 BIASWARN(
"ATTENTION! there seems to be a bug in Quartic()!");
547 int deg=(int)coeff.size()-1;
549 if(deg!=4 || coeff[deg]==0.0){
550 BIASERR(
"coeff is not a polynom of fourth order");
553 BIASASSERT(coeff[deg]!=0.0);
564 vector<POLYNOMIALSOLVE_TYPE> CubicCoeffs, CubicSolutions;
565 CubicCoeffs.resize(4);
568 POLYNOMIALSOLVE_TYPE a0 = coeff[0] / coeff[4];
569 POLYNOMIALSOLVE_TYPE a1 = coeff[1] / coeff[4];
570 POLYNOMIALSOLVE_TYPE a2 = coeff[2] / coeff[4];
571 POLYNOMIALSOLVE_TYPE a3 = coeff[3] / coeff[4];
573 CubicCoeffs[0] = -( a1*a1 + a0 * a3 * a3 - 4.0 * a0 * a2 );
574 CubicCoeffs[1] = a1*a3 - 4.0 * a0;
575 CubicCoeffs[2] = -a2;
576 CubicCoeffs[3] = 1.0;
579 Cubic(CubicCoeffs, CubicSolutions);
582 for(
unsigned int i = 0 ; i < CubicSolutions.size() ; i++ ) {
583 POLYNOMIALSOLVE_TYPE u,s,t;
584 u = CubicSolutions[i];
586 s = a3*a3 / 4.0 + u - a2;
587 t = u * u / 4.0 - a0;
589 if ((s>-DOUBLE_MIN) && (t>-DOUBLE_MIN)) {
590 if (s<DOUBLE_MIN) s=0;
591 if (t<DOUBLE_MIN) t=0;
593 vector<POLYNOMIALSOLVE_TYPE> QuadraticCoeffs, QuadraticSolutions;
594 QuadraticCoeffs.resize(3);
600 if ((s*t>0.0)^(a3*u/2.0-a1>0.0)) t = -t;
602 QuadraticCoeffs[0] = u / 2.0 + t;
603 QuadraticCoeffs[1] = a3 / 2.0 + s;
604 QuadraticCoeffs[2] = 1.0;
605 Quadratic(QuadraticCoeffs, QuadraticSolutions);
607 QuadraticCoeffs[0] = u / 2.0 - t;
608 QuadraticCoeffs[1] = a3 / 2.0 - s;
609 Quadratic(QuadraticCoeffs, sol);
611 for (
unsigned int j=0; j<QuadraticSolutions.size(); j++)
612 sol.push_back(QuadraticSolutions[j]);
628 POLYNOMIALSOLVE_TYPE largestcoeff=0;
629 for (
unsigned int i=0; i<coeff.size(); i++) {
630 if (fabs(coeff[i])>largestcoeff) largestcoeff = fabs(coeff[i]);
632 if (largestcoeff>0.0) {
635 largestcoeff = exp2(-1.0*rint(log2(largestcoeff)));
636 for (
unsigned int i=0; i<coeff.size(); i++) {
637 coeff[i] *= largestcoeff;
644 while (fabs(*coeff.rbegin())<=eps && coeff.size()>0){
645 BIASCDOUT(D_PS_COEFF,
"removing leading zero: "<<*coeff.rbegin()<<endl);
651 vector<POLYNOMIALSOLVE_TYPE>& coeff)
653 int deg=(int)sol.size();
656 for (
int c=0; c<nc; c++){
657 BIASCDOUT(D_PS_COEFF,
"\ncoff["<<c<<
"] = ");
658 coeff[c]=_GetCoeff(c, 0, sol);
659 BIASCDOUT(D_PS_COEFF,
" = "<<coeff[c]<<endl);
665 const vector<POLYNOMIALSOLVE_TYPE>& sol)
667 POLYNOMIALSOLVE_TYPE sum=0.0;
668 int size=(int)sol.size();
671 BIASCDOUT(D_PS_COEFF, sum);
672 }
else if (order==0){
674 BIASCDOUT(D_PS_COEFF, -sol[0]);
675 for (
int i=1; i<size; i++) {
677 BIASCDOUT(D_PS_COEFF,
"*"<<-sol[i]);
681 vector<POLYNOMIALSOLVE_TYPE> msol=sol;
682 for (
int i=begin; i<size; i++){
685 msol.erase(msol.begin()+i);
687 if (order-1==0) { BIASCDOUT(D_PS_COEFF,
" + "); }
689 sum+=_GetCoeff(order-1, i, msol);
697 vector<POLYNOMIALSOLVE_TYPE> sol;
700 return (Solve(coeff, sol));
704 int PowellFuncPolyEval(
void *p,
int n,
const double *x,
double *fvec,
int iflag)
716 BIASERR(
"The Coefficients are NULL, something is wrong!");
720 BIASERR(
"GSL (Gnu Scientific Library) needed.");
726 vector<complex<POLYNOMIALSOLVE_TYPE> >& sol)
728 PowellCoeffNum_=coeff.size();
729 if (PowellCoeff_!=NULL){
delete[] PowellCoeff_; PowellCoeff_=NULL; };
730 PowellCoeff_=
new double[PowellCoeffNum_];
731 for (
int i=0; i<PowellCoeffNum_; i++) PowellCoeff_[i]=coeff[i];
732 for (
unsigned i=0; i<sol.size(); i++){
733 if (imag(sol[i])==0.0){
736 if (
Powell(PowellFuncPolyEval, (
void*)(
this), ig, re, 1e-10)==0){
738 GetDebugStream().precision(25);
739 BIASCDOUT(D_PS_NLREFINE,
"before refine: "<<ig[0]<<
"\tpoly: "
740 <<gsl_poly_eval(PowellCoeff_, PowellCoeffNum_, ig[0])<<endl);
742 sol[i]=(complex<double>(re[0], 0.0));
744 BIASCDOUT(D_PS_NLREFINE,
"after refine: "<<re[0]<<
"\tpoly: "
745 <<gsl_poly_eval(PowellCoeff_, PowellCoeffNum_, re[0])
746 <<
"\tsol: "<<sol[i]<<endl);
749 BIASERR(
"nonlinear optimization failed for "<<i);
752 BIASCDOUT(D_PS_NLREFINE,
"not refining "<<sol[i]
753 <<
", because solution is imaginary "<<endl);
756 delete[] PowellCoeff_; PowellCoeff_=NULL;
761 vector<POLYNOMIALSOLVE_TYPE>& sol)
763 PowellCoeffNum_=coeff.size();
764 if (PowellCoeff_!=NULL){
delete[] PowellCoeff_; PowellCoeff_=NULL; };
765 PowellCoeff_=
new double[PowellCoeffNum_];
766 for (
int i=0; i<PowellCoeffNum_; i++) PowellCoeff_[i]=coeff[i];
767 for (
unsigned i=0; i<sol.size(); i++){
769 initialvalue[0] = sol[i];
770 if (
Powell(PowellFuncPolyEval,(
void*)(
this),initialvalue, resultvalue, 1e-15)==0){
771 BIASCDOUT(D_PS_NLREFINE,
"residual before refine: "
773 <<
"\troot: "<<setprecision(19)<<sol[i]<<endl);
774 sol[i]=resultvalue[0];
775 BIASCDOUT(D_PS_NLREFINE,
"residual after refine: "
777 <<
"\troot: "<<setprecision(19)<<sol[i]<<endl);
783 delete[] PowellCoeff_; PowellCoeff_=NULL;
788 vector<double> values;
789 vector<double> locations;
791 int TheErrorFunction(
void *p,
int m,
int n,
const double *x,
792 double *fvec,
int iflag) {
793 vector<double> params;
794 for (
int i=0; i<n; i++) params.push_back(x[i]);
797 for (
int j=0; j<m; j++)
804 const std::vector<double>& x,
805 const std::vector<double>& y,
806 std::vector<double> &coefficients) {
807 BIASASSERT(x.size()==y.size());
808 coefficients.clear();
809 coefficients.resize(degree+1, 0.0);
812 for (
unsigned int row=0; row<x.size(); row++) {
813 const double & xc = x[row];
814 for (
unsigned int col=0; col<degree+1; col++) {
816 for (
unsigned int colpower=0; colpower<col; colpower++)
824 int result = mySVD.
Solve(A, B, X);
826 BIASERR(
"linear solution failed");
835 if ((result =
LevenbergMarquardt(&TheErrorFunction, (
void*)(
this), x.size(), degree+1,
837 MINPACK_DEF_TOLERANCE))!=0) {
838 BIASERR(
"Error in Levenberg-Marquardt-Optimization of Polynomial: "<<result);
840 for (
unsigned int col=0; col<degree+1; col++) coefficients[col] = X[col];
843 for (
unsigned int col=0; col<degree+1; col++) {
844 coefficients[col] = resultcoeff[col];
848 if (result!=0)
return 1;
int NonLinearRefine(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< std::complex< POLYNOMIALSOLVE_TYPE > > &sol)
does a non linear refinement using Powel from minpack for real roots (non imaginary) only ...
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
void CheckCoefficients(std::vector< POLYNOMIALSOLVE_TYPE > &coeff, double eps=DBL_EPSILON)
removes leading zeros in coefficients
Vector< double > Solve(const Vector< double > &y) const
int Numeric(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solves polynomial of arbitrary order n numerically coeff[n]x^n+...+coeff[2]x^2+coeff[1]x+coeff[0]=0 c...
int FitPolynomial(const unsigned int degree, const std::vector< double > &x, const std::vector< double > &y, std::vector< double > &coefficients)
given locations x and measurements y=f(x) approximate f by a polynomial of arbitrary degree and retur...
int Linear(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solve a polynomial of degree 1 coeff[1]*x+coeff[0]=0 coeff[1] != 0.0 is asserted returns the number o...
int GetNumberOfRealSolutions(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff)
determine number of real solutions of polynomial
int Quadratic(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solve a polynomial of degree 2 coeff[2]*x^2+coeff[1]*x+coeff[0]=0 coeff[2] != 0.0 is asserted returns...
POLYNOMIALSOLVE_TYPE _GetCoeff(int order, int b, const std::vector< POLYNOMIALSOLVE_TYPE > &sol)
helper function for CalCoefficients
int Solve(const std::vector< std::complex< double > > &coeffs, std::vector< std::complex< double > > &roots, const double eps=1.0e-14)
compute roots of a polynomial using the laguerre method
base class for solving polynomial equations
POLYNOMIALSOLVE_TYPE EvaluatePolynomial(const POLYNOMIALSOLVE_TYPE x, const std::vector< POLYNOMIALSOLVE_TYPE > &coeff) const
numerically robust way to evaluate a polynomial at position x, uses Horner scheme (same principle as ...
int Cubic(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solve a polynomial of degree 3 coeff[3]*x^3+coeff[2]*x^2+coeff[1]*x+coeff[0]=0 coeff[3] != 0...
void CalCoefficients(const std::vector< POLYNOMIALSOLVE_TYPE > &sol, std::vector< POLYNOMIALSOLVE_TYPE > &coeff)
inverse function, calculates coefficients from the solutions
class encapsulating a laguerre solver for polynomials
int Quartic(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solve a polynomial of degree 4 coeff[4]*x^4+coeff[3]*x^3+coeff[2]*x^2+coeff[1]*x+coeff[0]=0 coeff[4] ...
long int LevenbergMarquardt(minpack_funcder_mn fun, void *p, long int m, long int n, Vector< double > &InitialGuess, Vector< double > &Result, double Tolerance)
Solves an overdetermined system f(x) = 0 of m non-linear equations with n parameters using the Levenb...
int Analytic(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solves polynomial of arbitrary order n<=4 analytically coeff[n]x^n+...+coeff[2]x^2+coeff[1]x+coeff[0]...
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
int Solve(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solve polynomial of arbitrary order n coeff[n]x^n+...+coeff[2]x^2+coeff[1]x+coeff[0]=0 coeff[n]!=0...
long int Powell(minpack_func_nn fun, void *p, Vector< double > &InitialGuess, Vector< double > &Result, double Tolerance)
Solve a system f(x) = 0 of n non-linear equations with n unknowns using the Powell hybrid method...