Polynomials: fix CID 1363767 : uninit variable
[scilab.git] / scilab / modules / polynomials / src / cpp / find_polynomial_roots_jenkins_traub.cc
1 // Copyright (C) 2015 Chris Sweeney. All rights reserved.
2 //
3 // Redistribution and use in source and binary forms, with or without
4 // modification, are permitted provided that the following conditions are
5 // met:
6 //
7 //     * Redistributions of source code must retain the above copyright
8 //       notice, this list of conditions and the following disclaimer.
9 //
10 //     * Redistributions in binary form must reproduce the above
11 //       copyright notice, this list of conditions and the following
12 //       disclaimer in the documentation and/or other materials provided
13 //       with the distribution.
14 //
15 //     * Neither the name of Chris Sweeney nor the names of its contributors may
16 //       be used to endorse or promote products derived from this software
17 //       without specific prior written permission.
18 //
19 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
23 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 // POSSIBILITY OF SUCH DAMAGE.
30 //
31 // Please contact the author of this library if you have any questions.
32 // Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)
33
34 #include "find_polynomial_roots_jenkins_traub.h"
35
36 #include <Eigen/Core>
37
38 #include <cmath>
39 #include <complex>
40 #include <iostream>
41 #include <limits>
42 #include <vector>
43
44 #include "polynomial.h"
45
46 namespace rpoly_plus_plus {
47
48 using Eigen::MatrixXd;
49 using Eigen::Vector3d;
50 using Eigen::VectorXd;
51 using Eigen::Vector3cd;
52 using Eigen::VectorXcd;
53
54 namespace {
55
56 #ifndef M_PI
57 #define M_PI 3.14159265358979323846264338327950288
58 #endif
59
60 // Machine precision constants.
61 static const double mult_eps = std::numeric_limits<double>::epsilon();
62 static const double sum_eps = std::numeric_limits<double>::epsilon();
63
64 enum ConvergenceType{
65   NO_CONVERGENCE = 0,
66   LINEAR_CONVERGENCE = 1,
67   QUADRATIC_CONVERGENCE = 2
68 };
69
70 // Solves for the root of the equation ax + b = 0.
71 double FindLinearPolynomialRoots(const double a, const double b) {
72   return -b / a;
73 }
74
75 // Stable quadratic roots according to BKP Horn.
76 // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
77 void FindQuadraticPolynomialRoots(const double a,
78                                   const double b,
79                                   const double c,
80                                   std::complex<double>* roots) {
81   const double D = b * b - 4 * a * c;
82   const double sqrt_D = std::sqrt(std::abs(D));
83
84   // Real roots.
85   if (D >= 0) {
86     if (b >= 0) {
87       roots[0] = std::complex<double>((-b - sqrt_D) / (2.0 * a), 0);
88       roots[1] = std::complex<double>((2.0 * c) / (-b - sqrt_D), 0);
89     } else {
90       roots[0] = std::complex<double>((2.0 * c) / (-b + sqrt_D), 0);
91       roots[1] = std::complex<double>((-b + sqrt_D) / (2.0 * a), 0);
92     }
93     return;
94   }
95
96   // Use the normal quadratic formula for the complex case.
97   roots[0] = std::complex<double>(-b / (2.0 * a), sqrt_D / (2.0 * a));
98   roots[1] = std::complex<double>(-b / (2.0 * a), -sqrt_D / (2.0 * a));
99 }
100
101 // Perform division by a linear term of the form (z - x) and evaluate P at x.
102 void SyntheticDivisionAndEvaluate(const VectorXd& polynomial,
103                                   const double x,
104                                   VectorXd* quotient,
105                                   double* eval) {
106   quotient->setZero(polynomial.size() - 1);
107   (*quotient)(0) = polynomial(0);
108   for (int i = 1; i < polynomial.size() - 1; i++) {
109     (*quotient)(i) = polynomial(i) + (*quotient)(i - 1) * x;
110   }
111   const VectorXd::ReverseReturnType& creverse_quotient = quotient->reverse();
112   *eval = polynomial.reverse()(0) + creverse_quotient(0) * x;
113 }
114
115 // Perform division of a polynomial by a quadratic factor. The quadratic divisor
116 // should have leading 1s.
117 void QuadraticSyntheticDivision(const VectorXd& polynomial,
118                                 const VectorXd& quadratic_divisor,
119                                 VectorXd* quotient,
120                                 VectorXd* remainder) {
121   quotient->setZero(polynomial.size() - 2);
122   remainder->setZero(2);
123
124   (*quotient)(0) = polynomial(0);
125
126   // If the quotient is a constant then polynomial is degree 2 and the math is
127   // simple.
128   if (quotient->size() == 1) {
129     *remainder =
130         polynomial.tail<2>() - polynomial(0) * quadratic_divisor.tail<2>();
131     return;
132   }
133
134   (*quotient)(1) = polynomial(1) - polynomial(0) * quadratic_divisor(1);
135   for (int i = 2; i < polynomial.size() - 2; i++) {
136     (*quotient)(i) = polynomial(i) - (*quotient)(i - 2) * quadratic_divisor(2) -
137                      (*quotient)(i - 1) * quadratic_divisor(1);
138   }
139
140   const VectorXd::ReverseReturnType &creverse_quotient = quotient->reverse();
141   (*remainder)(0) = polynomial.reverse()(1) -
142                     quadratic_divisor(1) * creverse_quotient(0) -
143                     quadratic_divisor(2) * creverse_quotient(1);
144   (*remainder)(1) =
145       polynomial.reverse()(0) - quadratic_divisor(2) * creverse_quotient(0);
146 }
147
148 // Determines whether the iteration has converged by examining the three most
149 // recent values for convergence.
150 template<typename T>
151 bool HasConverged(const T& sequence) {
152   const bool convergence_condition_1 =
153       std::abs(sequence(1) - sequence(0)) < std::abs(sequence(0)) / 2.0;
154   const bool convergence_condition_2 =
155       std::abs(sequence(2) - sequence(1)) < std::abs(sequence(1)) / 2.0;
156
157   // If the sequence has converged then return true.
158   return convergence_condition_1 && convergence_condition_2;
159 }
160
161 // Determines if the root has converged by measuring the relative and absolute
162 // change in the root value. This stopping criterion is a simple measurement
163 // that proves to work well. It is referred to as "Ward's method" in the
164 // following reference:
165 //
166 // Nikolajsen, Jorgen L. "New stopping criteria for iterative root finding."
167 // Royal Society open science (2014)
168 template <typename T>
169 bool HasRootConverged(const std::vector<T>& roots) {
170   static const double kRootMagnitudeTolerance = 1e-8;
171   static const double kAbsoluteTolerance = 1e-14;
172   static const double kRelativeTolerance = 1e-10;
173
174   if (roots.size() != 3) {
175     return false;
176   }
177
178   const double e_i = std::abs(roots[2] - roots[1]);
179   const double e_i_minus_1 = std::abs(roots[1] - roots[0]);
180   const double mag_root = std::abs(roots[1]);
181   if (e_i <= e_i_minus_1) {
182     if (mag_root < kRootMagnitudeTolerance) {
183       return e_i < kAbsoluteTolerance;
184     } else {
185       return e_i / mag_root <= kRelativeTolerance;
186     }
187   }
188
189   return false;
190 }
191
192 // Implementation closely follows the three-stage algorithm for finding roots of
193 // polynomials with real coefficients as outlined in: "A Three-Stage Algorithm
194 // for Real Polynomaials Using Quadratic Iteration" by Jenkins and Traub, SIAM
195 // 1970. Please note that this variant is different than the complex-coefficient
196 // version, and is estimated to be up to 4 times faster.
197 class JenkinsTraubSolver {
198  public:
199   JenkinsTraubSolver(const VectorXd& coeffs,
200                      VectorXd* real_roots,
201                      VectorXd* complex_roots)
202       : polynomial_(coeffs),
203         real_roots_(real_roots),
204         complex_roots_(complex_roots),
205         num_solved_roots_(0) {}
206
207   // Extracts the roots using the Jenkins Traub method.
208   bool ExtractRoots();
209
210  private:
211   // Removes any zero roots and divides polynomial by z.
212   void RemoveZeroRoots();
213
214   // Computes the magnitude of the roots to provide and initial search radius
215   // for the iterative solver.
216   double ComputeRootRadius();
217
218   // Computes the zero-shift applied to the K-Polynomial.
219   void ComputeZeroShiftKPolynomial();
220
221   // Stage 1 of the Jenkins-Traub method. This stage is not technically
222   // necessary, but helps separate roots that are close to zero.
223   void ApplyZeroShiftToKPolynomial(const int num_iterations);
224
225   // Computes and returns the update of sigma(z) based on the current
226   // K-polynomial.
227   //
228   // NOTE: This function is used by the fixed shift iterations (which hold sigma
229   // constant) so sigma is *not* modified internally by this function. If you
230   // want to change sigma, simply call
231   //    sigma = ComputeNextSigma();
232   VectorXd ComputeNextSigma();
233
234   // Updates the K-polynomial based on the current value of sigma for the fixed
235   // or variable shift stage.
236   void UpdateKPolynomialWithQuadraticShift(
237       const VectorXd& polynomial_quotient,
238       const VectorXd& k_polynomial_quotient);
239
240   // Apply fixed-shift iterations to the K-polynomial to separate the
241   // roots. Based on the convergence of the K-polynomial, we apply a
242   // variable-shift linear or quadratic iteration to determine a real root or
243   // complex conjugate pair of roots respectively.
244   ConvergenceType ApplyFixedShiftToKPolynomial(const std::complex<double>& root,
245                                                const int max_iterations);
246
247   // Applies one of the variable shifts to the K-Polynomial. Returns true upon
248   // successful convergence to a good root, and false otherwise.
249   bool ApplyVariableShiftToKPolynomial(
250       const ConvergenceType& fixed_shift_convergence,
251       const std::complex<double>& root);
252
253   // Applies a quadratic shift to the K-polynomial to determine a pair of roots
254   // that are complex conjugates. Return true if a root was successfully found.
255   bool ApplyQuadraticShiftToKPolynomial(const std::complex<double>& root,
256                                         const int max_iterations);
257
258   // Applies a linear shift to the K-polynomial to determine a single real root.
259   // Return true if a root was successfully found.
260   bool ApplyLinearShiftToKPolynomial(const std::complex<double>& root,
261                                      const int max_iterations);
262
263   // Adds the root to the output variables.
264   void AddRootToOutput(const double real, const double imag);
265
266   // Solves polynomials of degree <= 2.
267   bool SolveClosedFormPolynomial();
268
269   // Helper variables to manage the polynomials as they are being manipulated
270   // and deflated.
271   VectorXd polynomial_;
272   VectorXd k_polynomial_;
273   // Sigma is the quadratic factor the divides the K-polynomial.
274   Vector3d sigma_;
275
276   // Let us define a, b, c, and d such that:
277   //   P(z) = Q_P * sigma(z) + b * (z + u) + a
278   //   K(z) = Q_K * sigma(z) + d * (z + u ) + c
279   //
280   // where Q_P and Q_K are the quotients from polynomial division of
281   // sigma(z). Note that this means for a given a root s of sigma:
282   //
283   //   P(s)      = a - b * s_conj
284   //   P(s_conj) = a - b * s
285   //   K(s)      = c - d * s_conj
286   //   K(s_conj) = c - d * s
287   double a_, b_, c_, d_;
288
289   // Output reference variables.
290   VectorXd* real_roots_;
291   VectorXd* complex_roots_;
292   int num_solved_roots_;
293
294   // Keeps track of whether the linear and quadratic shifts have been attempted
295   // yet so that we do not attempt the same shift twice.
296   bool attempted_linear_shift_;
297   bool attempted_quadratic_shift_;
298
299   // Number of zero-shift iterations to perform.
300   static const int kNumZeroShiftIterations = 20;
301
302   // The number of fixed shift iterations is computed as
303   //   # roots found * this multiplier.
304   static const int kFixedShiftIterationMultiplier = 20;
305
306   // If the fixed shift iterations fail to converge, we restart this many times
307   // before considering the solve attempt as a failure.
308   static const int kMaxFixedShiftRestarts = 20;
309
310   // The maximum number of linear shift iterations to perform before considering
311   // the shift as a failure.
312   static const int kMaxLinearShiftIterations = 20;
313
314   // The maximum number of quadratic shift iterations to perform before
315   // considering the shift as a failure.
316   static const int kMaxQuadraticShiftIterations = 20;
317
318   // When quadratic shift iterations are stalling, we attempt a few fixed shift
319   // iterations to help convergence.
320   static const int kInnerFixedShiftIterations = 5;
321
322   // During quadratic iterations, the real values of the root pairs should be
323   // nearly equal since the root pairs are complex conjugates. This tolerance
324   // measures how much the real values may diverge before consider the quadratic
325   // shift to be failed.
326   static const double kRootPairTolerance;;
327 };
328
329 const double JenkinsTraubSolver::kRootPairTolerance = 0.01;
330
331 bool JenkinsTraubSolver::ExtractRoots() {
332   if (polynomial_.size() == 0) {
333     // std::cout << "Invalid polynomial of size 0 passed to "
334     //              "FindPolynomialRootsJenkinsTraub" << std::endl;
335     return false;
336   }
337
338   // Remove any leading zeros of the polynomial.
339   polynomial_ = RemoveLeadingZeros(polynomial_);
340   // Normalize the polynomial.
341   polynomial_ /= polynomial_(0);
342   const int degree = polynomial_.size() - 1;
343
344   // Allocate the output roots.
345   if (real_roots_ != NULL) {
346     real_roots_->setZero(degree);
347   }
348   if (complex_roots_ != NULL) {
349     complex_roots_->setZero(degree);
350   }
351
352   // Remove any zero roots.
353   RemoveZeroRoots();
354
355   // Choose the initial starting value for the root-finding on the complex
356   // plane.
357   const double kDegToRad = M_PI / 180.0;
358   double phi = 49.0 * kDegToRad;
359
360   // Iterate until the polynomial has been completely deflated.
361   for (int i = 0; i < degree; i++) {
362     // Compute the root radius.
363     const double root_radius = ComputeRootRadius();
364
365     // Solve in closed form if the polynomial is small enough.
366     if (polynomial_.size() <= 3) {
367       break;
368     }
369
370     // Stage 1: Apply zero-shifts to the K-polynomial to separate the small
371     // zeros of the polynomial.
372     ApplyZeroShiftToKPolynomial(kNumZeroShiftIterations);
373
374     // Stage 2: Apply fixed shift iterations to the K-polynomial to separate the
375     // roots further.
376     std::complex<double> root;
377     ConvergenceType convergence = NO_CONVERGENCE;
378     for (int j = 0; j < kMaxFixedShiftRestarts; j++) {
379       root = root_radius * std::complex<double>(std::cos(phi), std::sin(phi));
380       convergence = ApplyFixedShiftToKPolynomial(
381           root, kFixedShiftIterationMultiplier * (i + 1));
382       if (convergence != NO_CONVERGENCE) {
383         break;
384       }
385
386       // Rotate the initial root value on the complex plane and try again.
387       phi += 94.0 * kDegToRad;
388     }
389
390     // Stage 3: Find the root(s) with variable shift iterations on the
391     // K-polynomial. If this stage was not successful then we return a failure.
392     if (!ApplyVariableShiftToKPolynomial(convergence, root)) {
393       return false;
394     }
395   }
396   return SolveClosedFormPolynomial();
397 }
398
399 // Stage 1: Generate K-polynomials with no shifts (i.e. zero-shifts).
400 void JenkinsTraubSolver::ApplyZeroShiftToKPolynomial(
401     const int num_iterations) {
402   // K0 is the first order derivative of polynomial.
403   k_polynomial_ = DifferentiatePolynomial(polynomial_) / polynomial_.size();
404   for (int i = 1; i < num_iterations; i++) {
405     ComputeZeroShiftKPolynomial();
406   }
407 }
408
409 ConvergenceType JenkinsTraubSolver::ApplyFixedShiftToKPolynomial(
410     const std::complex<double>& root, const int max_iterations) {
411   // Compute the fixed-shift quadratic:
412   // sigma(z) = (x - m - n * i) * (x - m + n * i) = x^2 - 2 * m + m^2 + n^2.
413   sigma_(0) = 1.0;
414   sigma_(1) = -2.0 * root.real();
415   sigma_(2) = root.real() * root.real() + root.imag() * root.imag();
416
417   // Compute the quotient and remainder for divinding P by the quadratic
418   // divisor. Since this iteration involves a fixed-shift sigma these may be
419   // computed once prior to any iterations.
420   VectorXd polynomial_quotient, polynomial_remainder;
421   QuadraticSyntheticDivision(
422       polynomial_, sigma_, &polynomial_quotient, &polynomial_remainder);
423
424   // Compute a and b from the above equations.
425   b_ = polynomial_remainder(0);
426   a_ = polynomial_remainder(1) - b_ * sigma_(1);
427
428   // Precompute P(s) for later using the equation above.
429   const std::complex<double> p_at_root = a_ - b_ * std::conj(root);
430
431   // These two containers hold values that we test for convergence such that the
432   // zero index is the convergence value from 2 iterations ago, the first
433   // index is from one iteration ago, and the second index is the current value.
434   Vector3cd t_lambda = Vector3cd::Zero();
435   Vector3d sigma_lambda = Vector3d::Zero();
436   VectorXd k_polynomial_quotient, k_polynomial_remainder;
437   for (int i = 0; i < max_iterations; i++) {
438     k_polynomial_ /= k_polynomial_(0);
439
440     // Divide the shifted polynomial by the quadratic polynomial.
441     QuadraticSyntheticDivision(
442         k_polynomial_, sigma_, &k_polynomial_quotient, &k_polynomial_remainder);
443     d_ = k_polynomial_remainder(0);
444     c_ = k_polynomial_remainder(1) - d_ * sigma_(1);
445
446     // Test for convergence.
447     const VectorXd variable_shift_sigma = ComputeNextSigma();
448     const std::complex<double> k_at_root = c_ - d_ * std::conj(root);
449
450     t_lambda.head<2>() = t_lambda.tail<2>().eval();
451     sigma_lambda.head<2>() = sigma_lambda.tail<2>().eval();
452     t_lambda(2) = root - p_at_root / k_at_root;
453     sigma_lambda(2) = variable_shift_sigma(2);
454
455     // Return with the convergence code if the sequence has converged.
456     if (HasConverged(sigma_lambda)) {
457       return QUADRATIC_CONVERGENCE;
458     } else if (HasConverged(t_lambda)) {
459       return LINEAR_CONVERGENCE;
460     }
461
462     // Compute K_next using the formula above.
463     UpdateKPolynomialWithQuadraticShift(polynomial_quotient,
464                                         k_polynomial_quotient);
465   }
466
467   return NO_CONVERGENCE;
468 }
469
470 bool JenkinsTraubSolver::ApplyVariableShiftToKPolynomial(
471     const ConvergenceType& fixed_shift_convergence,
472     const std::complex<double>& root) {
473   attempted_linear_shift_ = false;
474   attempted_quadratic_shift_ = false;
475
476   if (fixed_shift_convergence == LINEAR_CONVERGENCE) {
477     return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
478   } else if (fixed_shift_convergence == QUADRATIC_CONVERGENCE) {
479     return ApplyQuadraticShiftToKPolynomial(root, kMaxQuadraticShiftIterations);
480   }
481   return false;
482 }
483
484 // Generate K-polynomials with variable-shifts. During variable shifts, the
485 // quadratic shift is computed as:
486 //                | K0(s1)  K0(s2)  z^2 |
487 //                | K1(s1)  K1(s2)    z |
488 //                | K2(s1)  K2(s2)    1 |
489 //    sigma(z) = __________________________
490 //                  | K1(s1)  K2(s1) |
491 //                  | K2(s1)  K2(s2) |
492 // Where K0, K1, and K2 are successive zero-shifts of the K-polynomial.
493 //
494 // The K-polynomial shifts are otherwise exactly the same as Stage 2 after
495 // accounting for a variable-shift sigma.
496 bool JenkinsTraubSolver::ApplyQuadraticShiftToKPolynomial(
497     const std::complex<double>& root, const int max_iterations) {
498   // Only proceed if we have not already tried a quadratic shift.
499   if (attempted_quadratic_shift_) {
500     return false;
501   }
502
503   const double kTinyRelativeStep = 0.01;
504
505   // Compute the fixed-shift quadratic:
506   // sigma(z) = (x - m - n * i) * (x - m + n * i) = x^2 - 2 * m + m^2 + n^2.
507   sigma_(0) = 1.0;
508   sigma_(1) = -2.0 * root.real();
509   sigma_(2) = root.real() * root.real() + root.imag() * root.imag();
510
511   // These two containers hold values that we test for convergence such that the
512   // zero index is the convergence value from 2 iterations ago, the first
513   // index is from one iteration ago, and the second index is the current value.
514   VectorXd polynomial_quotient, polynomial_remainder, k_polynomial_quotient,
515       k_polynomial_remainder;
516   double poly_at_root(0), prev_poly_at_root(0), prev_v(0);
517   bool tried_fixed_shifts = false;
518
519   // These containers maintain a history of the predicted roots. The convergence
520   // of the algorithm is determined by the convergence of the root value.
521   std::vector<std::complex<double> > roots1, roots2;
522   roots1.push_back(root);
523   roots2.push_back(std::conj(root));
524   for (int i = 0; i < max_iterations; i++) {
525     // Terminate if the root evaluation is within our tolerance. This will
526     // return false if we do not have enough samples.
527     if (HasRootConverged(roots1) && HasRootConverged(roots2)) {
528       AddRootToOutput(roots1[1].real(), roots1[1].imag());
529       AddRootToOutput(roots2[1].real(), roots2[1].imag());
530       polynomial_ = polynomial_quotient;
531       return true;
532     }
533
534     QuadraticSyntheticDivision(
535         polynomial_, sigma_, &polynomial_quotient, &polynomial_remainder);
536
537     // Compute a and b from the above equations.
538     b_ = polynomial_remainder(0);
539     a_ = polynomial_remainder(1) - b_ * sigma_(1);
540
541     std::complex<double> roots[2];
542     FindQuadraticPolynomialRoots(sigma_(0), sigma_(1), sigma_(2), roots);
543
544     // Check that the roots are close. If not, then try a linear shift.
545     if (std::abs(std::abs(roots[0].real()) - std::abs(roots[1].real())) >
546         kRootPairTolerance * std::abs(roots[1].real())) {
547       return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
548     }
549
550     // If the iteration is stalling at a root pair then apply a few fixed shift
551     // iterations to help convergence.
552     poly_at_root =
553         std::abs(a_ - roots[0].real() * b_) + std::abs(roots[0].imag() * b_);
554     const double rel_step = std::abs((sigma_(2) - prev_v) / sigma_(2));
555     if (!tried_fixed_shifts && rel_step < kTinyRelativeStep &&
556         prev_poly_at_root > poly_at_root) {
557       tried_fixed_shifts = true;
558       ApplyFixedShiftToKPolynomial(roots[0], kInnerFixedShiftIterations);
559     }
560
561     // Divide the shifted polynomial by the quadratic polynomial.
562     QuadraticSyntheticDivision(
563         k_polynomial_, sigma_, &k_polynomial_quotient, &k_polynomial_remainder);
564     d_ = k_polynomial_remainder(0);
565     c_ = k_polynomial_remainder(1) - d_ * sigma_(1);
566
567     prev_v = sigma_(2);
568     sigma_ = ComputeNextSigma();
569
570     // Compute K_next using the formula above.
571     UpdateKPolynomialWithQuadraticShift(polynomial_quotient,
572                                         k_polynomial_quotient);
573     k_polynomial_ /= k_polynomial_(0);
574     prev_poly_at_root = poly_at_root;
575
576     // Save the roots for convergence testing.
577     roots1.push_back(roots[0]);
578     roots2.push_back(roots[1]);
579     if (roots1.size() > 3) {
580       roots1.erase(roots1.begin());
581       roots2.erase(roots2.begin());
582     }
583   }
584
585   attempted_quadratic_shift_ = true;
586   return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
587 }
588
589 // Generate K-Polynomials with variable-shifts that are linear. The shift is
590 // computed as:
591 //   K_next(z) = 1 / (z - s) * (K(z) - K(s) / P(s) * P(z))
592 //   s_next = s - P(s) / K_next(s)
593 bool JenkinsTraubSolver::ApplyLinearShiftToKPolynomial(
594     const std::complex<double>& root, const int max_iterations) {
595   if (attempted_linear_shift_) {
596     return false;
597   }
598
599   // Compute an initial guess for the root.
600   double real_root = (root -
601                       EvaluatePolynomial(polynomial_, root) /
602                           EvaluatePolynomial(k_polynomial_, root)).real();
603
604   VectorXd deflated_polynomial, deflated_k_polynomial;
605   double polynomial_at_root = std::numeric_limits<double>::max();
606   double k_polynomial_at_root;
607
608   // This container maintains a history of the predicted roots. The convergence
609   // of the algorithm is determined by the convergence of the root value.
610   std::vector<double> roots;
611   roots.push_back(real_root);;
612   for (int i = 0; i < max_iterations; i++) {
613     // Terminate if the root evaluation is within our tolerance. This will
614     // return false if we do not have enough samples.
615     if (HasRootConverged(roots)) {
616       AddRootToOutput(roots[1], 0);
617       polynomial_ = deflated_polynomial;
618       return true;
619     }
620
621     const double prev_polynomial_at_root = polynomial_at_root;
622     SyntheticDivisionAndEvaluate(
623         polynomial_, real_root, &deflated_polynomial, &polynomial_at_root);
624
625     // If the root is exactly the root then end early. Otherwise, the k
626     // polynomial will be filled with inf or nans.
627     if (polynomial_at_root == 0) {
628       AddRootToOutput(real_root, 0);
629       polynomial_ = deflated_polynomial;
630       return true;
631     }
632
633     // Update the K-Polynomial.
634     SyntheticDivisionAndEvaluate(k_polynomial_, real_root,
635                                  &deflated_k_polynomial, &k_polynomial_at_root);
636     k_polynomial_ = AddPolynomials(
637         deflated_k_polynomial,
638         -k_polynomial_at_root / polynomial_at_root * deflated_polynomial);
639
640     k_polynomial_ /= k_polynomial_(0);
641
642     // Compute the update for the root estimation.
643     k_polynomial_at_root = EvaluatePolynomial(k_polynomial_, real_root);
644     const double delta_root = polynomial_at_root / k_polynomial_at_root;
645     real_root -= polynomial_at_root / k_polynomial_at_root;
646
647     // Save the root so that convergence can be measured. Only the 3 most
648     // recently root values are needed.
649     roots.push_back(real_root);
650     if (roots.size() > 3) {
651       roots.erase(roots.begin());
652     }
653
654     // If the linear iterations appear to be stalling then we may have found a
655     // double real root of the form (z - x^2). Attempt a quadratic variable
656     // shift from the current estimate of the root.
657     if (i >= 2 &&
658         std::abs(delta_root) < 0.001 * std::abs(real_root) &&
659         std::abs(prev_polynomial_at_root) < std::abs(polynomial_at_root)) {
660       const std::complex<double> new_root(real_root, 0);
661       return ApplyQuadraticShiftToKPolynomial(new_root,
662                                               kMaxQuadraticShiftIterations);
663     }
664   }
665
666   attempted_linear_shift_ = true;
667   return ApplyQuadraticShiftToKPolynomial(root, kMaxQuadraticShiftIterations);
668 }
669
670 void JenkinsTraubSolver::AddRootToOutput(const double real, const double imag) {
671   if (real_roots_ != NULL) {
672     (*real_roots_)(num_solved_roots_) = real;
673   }
674   if (complex_roots_ != NULL) {
675     (*complex_roots_)(num_solved_roots_) = imag;
676   }
677   ++num_solved_roots_;
678 }
679
680 void JenkinsTraubSolver::RemoveZeroRoots() {
681   int num_zero_roots = 0;
682
683   const VectorXd::ReverseReturnType& creverse_polynomial =
684       polynomial_.reverse();
685   while (creverse_polynomial(num_zero_roots) == 0) {
686     ++num_zero_roots;
687   }
688
689   // The output roots have 0 as the default value so there is no need to
690   // explicitly add the zero roots.
691   polynomial_ = polynomial_.head(polynomial_.size() - num_zero_roots).eval();
692 }
693
694 bool JenkinsTraubSolver::SolveClosedFormPolynomial() {
695   const int degree = polynomial_.size() - 1;
696
697   // Is the polynomial constant?
698   if (degree == 0) {
699     // std::cout << "Trying to extract roots from a constant "
700     //          << "polynomial in FindPolynomialRoots" << std::endl;
701     // We return true with no roots, not false, as if the polynomial is constant
702     // it is correct that there are no roots. It is not the case that they were
703     // there, but that we have failed to extract them.
704     return true;
705   }
706   
707   // Linear
708   if (degree == 1) {
709     AddRootToOutput(FindLinearPolynomialRoots(polynomial_(0), polynomial_(1)),
710                     0);
711     return true;
712   }
713
714   // Quadratic
715   if (degree == 2) {
716     std::complex<double> roots[2];
717     FindQuadraticPolynomialRoots(polynomial_(0), polynomial_(1), polynomial_(2),
718                                  roots);
719     AddRootToOutput(roots[0].real(), roots[0].imag());
720     AddRootToOutput(roots[1].real(), roots[1].imag());
721     return true;
722   }
723
724   return false;
725 }
726
727 // Computes a lower bound on the radius of the roots of polynomial by examining
728 // the Cauchy sequence:
729 //
730 //    z^n + |a_1| * z^{n - 1} + ... + |a_{n-1}| * z - |a_n|
731 //
732 // The unique positive zero of this polynomial is an approximate lower bound of
733 // the radius of zeros of the original polynomial.
734 double JenkinsTraubSolver::ComputeRootRadius() {
735   static const double kEpsilon = 1e-2;
736   static const int kMaxIterations = 100;
737
738   VectorXd poly = polynomial_;
739   // Take the absolute value of all coefficients.
740   poly = poly.array().abs();
741   // Negate the last coefficient.
742   poly.reverse()(0) *= -1.0;
743
744   // Find the unique positive zero using Newton-Raphson iterations.
745   double x0 = 1.0;
746   return FindRootIterativeNewton(poly, x0, kEpsilon, kMaxIterations);
747 }
748
749 // The k polynomial with a zero-shift is
750 //  (K(x) - K(0) / P(0) * P(x)) / x.
751 //
752 // This is equivalent to:
753 //    K(x) - K(0)      K(0)     P(x) - P(0)
754 //    ___________   -  ____  *  ___________
755 //         x           P(0)          x
756 //
757 // Note that removing the constant term and dividing by x is equivalent to
758 // shifting the polynomial to one degree lower in our representation.
759 void JenkinsTraubSolver::ComputeZeroShiftKPolynomial() {
760   // Evaluating the polynomial at zero is equivalent to the constant term
761   // (i.e. the last coefficient). Note that reverse() is an expression and does
762   // not actually reverse the vector elements.
763   const double polynomial_at_zero = polynomial_(polynomial_.size() - 1);
764   const double k_at_zero = k_polynomial_(k_polynomial_.size() - 1);
765
766   k_polynomial_ = AddPolynomials(k_polynomial_.head(k_polynomial_.size() - 1),
767                                  -k_at_zero / polynomial_at_zero *
768                                      polynomial_.head(polynomial_.size() - 1));
769 }
770
771 // The iterations are computed with the following equation:
772 //              a^2 + u * a * b + v * b^2
773 //   K_next =  ___________________________ * Q_K
774 //                    b * c - a * d
775 //
776 //                      a * c + u * a * d + v * b * d
777 //             +  (z - _______________________________) * Q_P + b.
778 //                              b * c - a * d
779 //
780 // This is done using *only* realy arithmetic so it can be done very fast!
781 void JenkinsTraubSolver::UpdateKPolynomialWithQuadraticShift(
782     const VectorXd& polynomial_quotient,
783     const VectorXd& k_polynomial_quotient) {
784   const double coefficient_q_k =
785       (a_ * a_ + sigma_(1) * a_ * b_ + sigma_(2) * b_ * b_) /
786       (b_ * c_ - a_ * d_);
787   VectorXd linear_polynomial(2);
788   linear_polynomial(0) = 1.0;
789   linear_polynomial(1) =
790       -(a_ * c_ + sigma_(1) * a_ * d_ + sigma_(2) * b_ * d_) /
791       (b_ * c_ - a_ * d_);
792   k_polynomial_ = AddPolynomials(
793       coefficient_q_k * k_polynomial_quotient,
794       MultiplyPolynomials(linear_polynomial, polynomial_quotient));
795   k_polynomial_(k_polynomial_.size() - 1) += b_;
796 }
797
798 // Using a bit of algebra, the update of sigma(z) can be computed from the
799 // previous value along with a, b, c, and d defined above. The details of this
800 // simplification can be found in "Three Stage Variable-Shift Iterations for the
801 // Solution of Polynomial Equations With a Posteriori Error Bounds for the
802 // Zeros" by M.A. Jenkins, Doctoral Thesis, Stanford Univeristy, 1969.
803 //
804 // NOTE: we assume the leading term of quadratic_sigma is 1.0.
805 VectorXd JenkinsTraubSolver::ComputeNextSigma() {
806   const double u = sigma_(1);
807   const double v = sigma_(2);
808
809   const VectorXd::ReverseReturnType& creverse_k_polynomial =
810       k_polynomial_.reverse();
811   const VectorXd::ReverseReturnType& creverse_polynomial =
812       polynomial_.reverse();
813
814   const double b1 = -creverse_k_polynomial(0) / creverse_polynomial(0);
815   const double b2 = -(creverse_k_polynomial(1) + b1 * creverse_polynomial(1)) /
816                     creverse_polynomial(0);
817
818   const double a1 = b_* c_ - a_ * d_;
819   const double a2 = a_ * c_ + u * a_ * d_ + v * b_* d_;
820   const double c2 = b1 * a2;
821   const double c3 = b1 * b1 * (a_ * a_ + u * a_ * b_ + v * b_ * b_);
822   const double c4 = v * b2 * a1 - c2 - c3;
823   const double c1 = c_ * c_ + u * c_ * d_ + v * d_ * d_ +
824                     b1 * (a_ * c_ + u * b_ * c_ + v * b_ * d_) - c4;
825   const double delta_u = -(u * (c2 + c3) + v * (b1 * a1 + b2 * a2)) / c1;
826   const double delta_v = v * c4 / c1;
827
828   // Update u and v in the quadratic sigma.
829   VectorXd new_quadratic_sigma(3);
830   new_quadratic_sigma(0) = 1.0;
831   new_quadratic_sigma(1) = u + delta_u;
832   new_quadratic_sigma(2) = v + delta_v;
833   return new_quadratic_sigma;
834 }
835
836 }  // namespace
837
838 bool FindPolynomialRootsJenkinsTraub(const VectorXd& polynomial,
839                                      VectorXd* real_roots,
840                                      VectorXd* complex_roots) {
841   JenkinsTraubSolver solver(polynomial, real_roots, complex_roots);
842   return solver.ExtractRoots();
843 }
844
845 }  // namespace rpoly_plus_plus