1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm): SimTKcommon *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2007-12 Stanford University and the Authors. *
10 * Authors: Peter Eastman *
11 * Contributors: *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 #include "SimTKcommon.h"
25
26 #include <iostream>
27
28 #define ASSERT(cond) {SimTK_ASSERT_ALWAYS(cond, "Assertion failed");}
29
30 using std::cout;
31 using std::endl;
32 using namespace SimTK;
33
34 /**
35 * Determine if two complex numbers are equal to within the specified tolerance.
36 */
37
equal(Complex expected,Complex found,Real tol)38 bool equal(Complex expected, Complex found, Real tol) {
39 Real diffr = std::abs(expected.real()-found.real());
40 Real diffi = std::abs(expected.imag()-found.imag());
41 Real scaler = std::max(std::abs(expected.real()), std::abs(found.real()));
42 Real scalei = std::max(std::abs(expected.imag()), std::abs(found.imag()));
43 if (expected.real() == 0.0)
44 scaler = 1.0;
45 if (expected.imag() == 0.0)
46 scalei = 1.0;
47 return (diffr < tol*scaler && diffi < tol*scalei);
48 }
49
50 /**
51 * Determine if two roots of a quadratic polynomial are equal. This is a more stringent test then the above routine,
52 * since the quadratic solver has very high accuracy.
53 */
54
equal2(Complex expected,Complex found)55 bool equal2(Complex expected, Complex found) {
56 if (expected.imag() == 0.0 && found.imag() != 0.0)
57 return false; // If we expect a real number, require the number found to be precisely real as well.
58 return equal(expected, found, std::sqrt(NTraits<Real>::getEps()));
59 }
60
61 /**
62 * Find the roots of the specified polynomial with real coefficients, and compare them to the expected ones.
63 */
64
testQuadratic(Vec3 coeff,Vec<2,Complex> expected)65 void testQuadratic(Vec3 coeff, Vec<2,Complex> expected) {
66 Vec<2,Complex> found;
67 PolynomialRootFinder::findRoots(coeff, found);
68 ASSERT((equal2(expected[0], found[0]) && equal2(expected[1], found[1])) || (equal2(expected[0], found[1]) && equal2(expected[1], found[0])))
69 }
70
71 /**
72 * Find the roots of the specified polynomial with complex coefficients, and compare them to the expected ones.
73 */
74
testQuadratic(Vec<3,Complex> coeff,Vec<2,Complex> expected)75 void testQuadratic(Vec<3,Complex> coeff, Vec<2,Complex> expected) {
76 Vec<2,Complex> found;
77 PolynomialRootFinder::findRoots(coeff, found);
78 ASSERT((equal2(expected[0], found[0]) && equal2(expected[1], found[1])) || (equal2(expected[0], found[1]) && equal2(expected[1], found[0])))
79 }
80
81 /**
82 * Generate a polynomial that has the specified roots, then solve it and compare.
83 */
84
testQuadraticRealRoots(Vec2 roots)85 void testQuadraticRealRoots(Vec2 roots) {
86 static Random::Gaussian random(0.0, 100.0);
87 Vec3 coeff(1.0, -roots[0]-roots[1], roots[0]*roots[1]);
88 coeff *= random.getValue();
89 testQuadratic(coeff, Vec<2,Complex>(roots[0], roots[1]));
90 }
91
92 /**
93 * Generate a polynomial whose roots are a complex conjugate pair, then solve it and compare.
94 */
95
testQuadraticConjugateRoots(Complex root)96 void testQuadraticConjugateRoots(Complex root) {
97 static Random::Gaussian random(0.0, 1000.0);
98 Vec3 coeff(1.0, -2.0*root.real(), norm(root));
99 coeff *= random.getValue();
100 testQuadratic(coeff, Vec<2,Complex>(root, conj(root)));
101 }
102
103 /**
104 * Generate a polynomial that has the specified roots, then solve it and compare.
105 */
106
testQuadraticComplexRoots(Vec<2,Complex> roots)107 void testQuadraticComplexRoots(Vec<2,Complex>roots) {
108 static Random::Gaussian random(0.0, 100.0);
109 Vec<3,Complex> coeff(1.0, -roots[0]-roots[1], roots[0]*roots[1]);
110 coeff *= Complex(random.getValue(), random.getValue());
111 testQuadratic(coeff, roots);
112 }
113
114 /**
115 * Find the roots of the specified polynomial with real coefficients, and compare them to the expected ones.
116 */
117
testCubic(Vec4 coeff,Vec<3,Complex> expected)118 void testCubic(Vec4 coeff, Vec<3,Complex> expected) {
119 Vec<3,Complex> found;
120 PolynomialRootFinder::findRoots(coeff, found);
121 Real tol = 1e-4;
122 if (equal(expected[0], found[0], tol))
123 ASSERT((equal(expected[1], found[1], tol) && equal(expected[2], found[2], tol)) || (equal(expected[1], found[2], tol) && equal(expected[2], found[1], tol)))
124 else if (equal(expected[0], found[1], tol))
125 ASSERT((equal(expected[1], found[2], tol) && equal(expected[2], found[0], tol)) || (equal(expected[1], found[0], tol) && equal(expected[2], found[2], tol)))
126 else if (equal(expected[0], found[2], tol))
127 ASSERT((equal(expected[1], found[0], tol) && equal(expected[2], found[1], tol)) || (equal(expected[1], found[1], tol) && equal(expected[2], found[0], tol)))
128 else
129 ASSERT(false)
130 }
131
132 /**
133 * Find the roots of the specified polynomial with complex coefficients, and compare them to the expected ones.
134 */
135
testCubic(Vec<4,Complex> coeff,Vec<3,Complex> expected)136 void testCubic(Vec<4,Complex> coeff, Vec<3,Complex> expected) {
137 Vec<3,Complex> found;
138 PolynomialRootFinder::findRoots(coeff, found);
139 Real tol = 1e-4;
140 if (equal(expected[0], found[0], tol))
141 ASSERT((equal(expected[1], found[1], tol) && equal(expected[2], found[2], tol)) || (equal(expected[1], found[2], tol) && equal(expected[2], found[1], tol)))
142 else if (equal(expected[0], found[1], tol))
143 ASSERT((equal(expected[1], found[2], tol) && equal(expected[2], found[0], tol)) || (equal(expected[1], found[0], tol) && equal(expected[2], found[2], tol)))
144 else if (equal(expected[0], found[2], tol))
145 ASSERT((equal(expected[1], found[0], tol) && equal(expected[2], found[1], tol)) || (equal(expected[1], found[1], tol) && equal(expected[2], found[0], tol)))
146 else
147 ASSERT(false)
148 }
149
150 /**
151 * Generate a polynomial that has the specified roots, then solve it and compare.
152 */
153
testCubicRealRoots(Vec3 roots)154 void testCubicRealRoots(Vec3 roots) {
155 static Random::Gaussian random(0.0, 100.0);
156 Vec4 coeff(1.0, -roots[0]-roots[1]-roots[2], roots[0]*roots[1]+roots[1]*roots[2]+roots[2]*roots[0], -roots[0]*roots[1]*roots[2]);
157 coeff *= random.getValue();
158 testCubic(coeff, Vec<3,Complex>(roots[0], roots[1], roots[2]));
159 }
160
161 /**
162 * Generate a polynomial whose roots are a complex conjugate pair plus a single real root, then solve it and compare.
163 */
164
testCubicConjugateRoots(Complex root1,Real root2)165 void testCubicConjugateRoots(Complex root1, Real root2) {
166 static Random::Gaussian random(0.0, 1000.0);
167 Vec4 coeff(1.0, -2.0*root1.real()-root2, norm(root1)+2.0*root1.real()*root2,-norm(root1)*root2);
168 coeff *= random.getValue();
169 testCubic(coeff, Vec<3,Complex>(root1, conj(root1), root2));
170 }
171
172 /**
173 * Generate a polynomial that has the specified roots, then solve it and compare.
174 */
175
testCubicComplexRoots(Vec<3,Complex> roots)176 void testCubicComplexRoots(Vec<3,Complex> roots) {
177 static Random::Gaussian random(0.0, 100.0);
178 Vec<4,Complex> coeff(1.0, -roots[0]-roots[1]-roots[2], roots[0]*roots[1]+roots[1]*roots[2]+roots[2]*roots[0], -roots[0]*roots[1]*roots[2]);
179 coeff *= random.getValue();
180 testCubic(coeff, Vec<3,Complex>(roots[0], roots[1], roots[2]));
181 }
182
183 // Evaluate a real-coefficient polynomial at the given complex value. Should
184 // evaluate to zero if this is a root.
evalPoly(Vector_<Real> & coefficients,const Complex & value)185 Complex evalPoly(Vector_<Real>& coefficients, const Complex& value) {
186 Complex sum = 0.0;
187 for (int j = 0; j < coefficients.size(); ++j)
188 sum = sum*value+coefficients[j];
189 return sum;
190 }
191
192 // Evaluate a complex-coefficient polynomial at the given complex value. Should
193 // evaluate to zero if this is a root.
evalPoly(Vector_<Complex> & coefficients,const Complex & value)194 Complex evalPoly(Vector_<Complex>& coefficients, const Complex& value) {
195 Complex sum = 0.0;
196 for (int j = 0; j < coefficients.size(); ++j)
197 sum = sum*value+coefficients[j];
198 return sum;
199 }
200
201 /**
202 * Given a polynomial and a list of roots, verify that they really are roots.
203 */
verifyRoots(Vector_<Real> & coefficients,Vector_<Complex> & roots,Real tol=1e-2)204 void verifyRoots(Vector_<Real>& coefficients, Vector_<Complex>& roots, Real tol=1e-2) {
205 for (int i = 0; i < roots.size(); ++i) {
206 Complex sum = evalPoly(coefficients, roots[i]);
207 ASSERT(equal(0.0, sum, tol))
208 }
209 }
210
211 /**
212 * Given a polynomial and a list of roots, verify that they really are roots.
213 */
214
verifyRoots(Vector_<Complex> & coefficients,Vector_<Complex> & roots,Real tol=1e-2)215 void verifyRoots(Vector_<Complex>& coefficients, Vector_<Complex>& roots, Real tol=1e-2) {
216 for (int i = 0; i < roots.size(); ++i) {
217 Complex sum = evalPoly(coefficients, roots[i]);
218 ASSERT(equal(0.0, sum, tol))
219 }
220 }
221
222 /**
223 * Perform a variety of tests solving quadratic polynomials.
224 */
225
testSolveQuadratic()226 void testSolveQuadratic() {
227
228 // Try a few fixed tests.
229
230 testQuadraticRealRoots(Vec2(0.0, 0.0));
231 testQuadraticRealRoots(Vec2(1.0, 1.0));
232 testQuadraticRealRoots(Vec2(0.0, 5.0));
233 testQuadraticRealRoots(Vec2(10.0, -5.0));
234 testQuadratic(Vec3(1.0, 0.0, 0.0), Vec<2,Complex>(0.0, 0.0));
235
236 // Try cases with a single, repeated real root.
237
238 static Random::Gaussian random(0.0, 100.0);
239 for (int i = 0; i < 1000; ++i) {
240 Real root = random.getValue();
241 testQuadraticRealRoots(Vec2(root, root));
242 }
243
244 // Try cases with two distinct real roots.
245
246 for (int i = 0; i < 1000; ++i)
247 testQuadraticRealRoots(Vec2(random.getValue(), random.getValue()));
248
249 // Try cases whose roots are complex conjugates.
250
251 for (int i = 0; i < 1000; ++i)
252 testQuadraticConjugateRoots(Complex(random.getValue(), random.getValue()));
253
254 // Try cases with two distinct complex roots.
255
256 for (int i = 0; i < 1000; ++i)
257 testQuadraticComplexRoots(Vec<2,Complex>(Complex(random.getValue(), random.getValue()), Complex(random.getValue(), random.getValue())));
258
259 // Verify that if the leading coefficient is zero, it throws an exception.
260
261 try {
262 testQuadratic(Vec3(0.0, 1.0, 1.0), Vec<2,Complex>(0.0, 0.0));
263 ASSERT(false)
264 }
265 catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
266 }
267 try {
268 testQuadratic(Vec<3,Complex>(0.0, 1.0, 1.0), Vec<2,Complex>(0.0, 0.0));
269 ASSERT(false)
270 }
271 catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
272 }
273 }
274
275 /**
276 * Perform a variety of tests solving cubic polynomials.
277 */
278
testSolveCubic()279 void testSolveCubic() {
280
281 // Try a few fixed tests.
282
283 testCubicRealRoots(Vec3(0.0, 0.0, 0.0));
284 testCubicRealRoots(Vec3(1.0, 1.0, 1.0));
285 testCubicRealRoots(Vec3(0.0, 5.0, 5.0));
286 testCubicRealRoots(Vec3(10.0, -5.0, 100.0));
287 testCubic(Vec4(1.0, 0.0, 0.0, 0.0), Vec<3,Complex>(0.0, 0.0, 0.0));
288
289 // Try cases with a single, repeated real root.
290
291 static Random::Gaussian random(0.0, 100.0);
292 for (int i = 0; i < 1000; ++i) {
293 Real root = random.getValue();
294 testCubicRealRoots(Vec3(root, root, root));
295 }
296
297 // Try cases with two distinct real roots.
298
299 for (int i = 0; i < 1000; ++i) {
300 Real root = random.getValue();
301 testCubicRealRoots(Vec3(root, root, random.getValue()));
302 }
303
304 // Try cases with three distinct real roots.
305
306 for (int i = 0; i < 1000; ++i)
307 testCubicRealRoots(Vec3(random.getValue(), random.getValue(), random.getValue()));
308
309 // Try cases whose roots include a complex conjugates pair.
310
311 for (int i = 0; i < 1000; ++i)
312 testCubicConjugateRoots(Complex(random.getValue(), random.getValue()), random.getValue());
313
314 // Try cases with three distinct complex roots.
315
316 for (int i = 0; i < 1000; ++i)
317 testCubicComplexRoots(Vec<3,Complex>(Complex(random.getValue(), random.getValue()), Complex(random.getValue(), random.getValue()), Complex(random.getValue(), random.getValue())));
318
319 // Verify that if the leading coefficient is zero, it throws an exception.
320
321 try {
322 testCubic(Vec4(0.0, 0.0, 0.0, 0.0), Vec<3,Complex>(0.0, 0.0, 0.0));
323 ASSERT(false)
324 }
325 catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
326 }
327 try {
328 testCubic(Vec<4,Complex>(0.0, 0.0, 0.0, 0.0), Vec<3,Complex>(0.0, 0.0, 0.0));
329 ASSERT(false)
330 }
331 catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
332 }
333 }
334
335 /**
336 * Test solving polynomials of arbitrary degree.
337 */
338
testSolveArbitrary()339 void testSolveArbitrary() {
340 static Random::Uniform uniform(2.0, 7.0);
341 static Random::Gaussian random(0.0, 100.0);
342
343 // Test random polynomials of various degrees with real coefficients.
344
345 Vector realCoeff;
346 Vector_<Complex> roots;
347 for (int i = 0; i < 1000; ++i) {
348 int length = uniform.getIntValue();
349 realCoeff.resize(length);
350 roots.resize(length-1);
351 for (int j = 0; j < length; ++j)
352 realCoeff[j] = random.getValue();
353 PolynomialRootFinder::findRoots(realCoeff, roots);
354 verifyRoots(realCoeff, roots);
355 }
356
357 // Test random polynomials of various degrees with complex coefficients.
358
359 Vector_<Complex> complexCoeff;
360 for (int i = 0; i < 1000; ++i) {
361 int length = uniform.getIntValue();
362 complexCoeff.resize(length);
363 roots.resize(length-1);
364 for (int j = 0; j < length; ++j)
365 complexCoeff[j] = Complex(random.getValue(), random.getValue());
366 PolynomialRootFinder::findRoots(complexCoeff, roots);
367 verifyRoots(complexCoeff, roots);
368 }
369
370 // Verify that if the leading coefficient is zero, it throws an exception.
371
372 try {
373 realCoeff[0] = 0.0;
374 PolynomialRootFinder::findRoots(realCoeff, roots);
375 ASSERT(false)
376 }
377 catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
378 }
379 try {
380 complexCoeff[0] = 0.0;
381 PolynomialRootFinder::findRoots(complexCoeff, roots);
382 ASSERT(false)
383 }
384 catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
385 }
386 }
387
388 // Sherm 20130410:
389 // This 6th order polynomial was generated by Ellipsoid's findNearestPoint()
390 // method and caused the Jenkins-Traub polynomial solver to fail
391 // catastrophically, returning no roots, while many nearly identical
392 // polynomials solved perfectly. I put in a fix (see rpoly.cpp) and to make
393 // sure it stays fixed I'm recording the bad polynomial here.
testReallyAwfulEllipsoidPolynomial()394 void testReallyAwfulEllipsoidPolynomial() {
395 Vector good1(7), good2(7), bad(7);
396 Vector_<Complex> roots(6);
397
398 good1[0] = 1.0000000000000000;
399 good1[1] = 0.093099999999999988;
400 good1[2] = 0.00057211940879762883;
401 good1[3] = 1.4343090324181468e-006;
402 good1[4] = 1.6097307763625053e-009;
403 good1[5] = 6.6189348690786845e-013;
404 good1[6] = -3.0418139616145048e-018;
405 PolynomialRootFinder::findRoots(good1, roots);
406 verifyRoots(good1, roots, 1e-10);
407
408 good2[0] = 1.0000000000000000;
409 good2[1] = 0.021700000000000004;
410 good2[2] = 0.00013355532616269355;
411 good2[3] = 1.8068473626980300e-007;
412 good2[4] = 6.2414644021223684e-011;
413 good2[5] = 6.4036661086410838e-015;
414 good2[6] = -6.8627441483352105e-022;
415 PolynomialRootFinder::findRoots(good2, roots);
416 verifyRoots(good2, roots, 1e-10);
417
418 // This one breaks the original Jenkins-Traub method. Sure doesn't look
419 // much different than the ones above.
420 bad[0] = 1.0000000000000000;
421 bad[1] = 0.021700000000000004;
422 bad[2] = 2.9889970904696875e-005;
423 bad[3] = 1.0901272298136685e-008;
424 bad[4] = -4.4822782160985054e-012;
425 bad[5] = -2.6193432740351220e-015;
426 bad[6] = -3.0900602527225053e-019;
427 PolynomialRootFinder::findRoots(bad, roots);
428 verifyRoots(bad, roots, 1e-10);
429
430 }
431
main()432 int main() {
433 try {
434 testSolveQuadratic();
435 testSolveCubic();
436 testSolveArbitrary();
437 testReallyAwfulEllipsoidPolynomial();
438 } catch(const std::exception& e) {
439 cout << "exception: " << e.what() << endl;
440 return 1;
441 }
442 return 0;
443 }
444
445
446