CubicSolver-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2016, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_MATH_CUBICSOLVER_INL_H
17 #define SURGSIM_MATH_CUBICSOLVER_INL_H
18 
19 #include <boost/math/tools/roots.hpp>
20 #include <limits>
21 #include <vector>
22 
25 
26 using boost::math::tools::bisect;
27 
28 
29 namespace SurgSim
30 {
31 namespace Math
32 {
33 
34 template <class T>
35 int findRootsInRange01(const Polynomial<T, 3>& p, std::array<T, 3>* roots)
36 {
37  int numberOfRoots = 0;
38  boost::math::tools::eps_tolerance<T> tolerance(std::numeric_limits<T>::digits - 3);
39  const T epsilon = 4 * std::numeric_limits<T>::epsilon();
40 
41  // Is degenerate?
42  if (isNearZero(p.getCoefficient(3), epsilon))
43  {
45  PolynomialRoots<T, 2> quadraticRoots(quadratic, std::numeric_limits<T>::epsilon());
46 
47  for (int i = 0; i < quadraticRoots.getNumRoots(); ++i)
48  {
49  if (quadraticRoots[i] >= 0.0 && quadraticRoots[i] <= 1.0)
50  {
51  (*roots)[numberOfRoots++] = quadraticRoots[i];
52  }
53  }
54 
55  return numberOfRoots;
56  }
57 
58  PolynomialRoots<T, 2> stationaryPoints(p.derivative(), std::numeric_limits<T>::epsilon());
59  if (stationaryPoints.getNumRoots() < 2 ||
60  !Interval<T>(0, 1).overlapsWith(Interval<T>(stationaryPoints[0], stationaryPoints[1])))
61  {
62  T p0 = p.getCoefficient(0); // p.evaluate(static_cast<T>(0));
63  if (isNearZero(p0, epsilon))
64  {
65  (*roots)[0] = 0.0;
66  return 1;
67  }
68 
69  T p1 = p.evaluate(static_cast<T>(1));
70  if (isNearZero(p1, epsilon))
71  {
72  (*roots)[0] = static_cast<T>(1);
73  return 1;
74  }
75  if (p0 * p1 < 0)
76  {
77  auto bracket = bisect(p, static_cast<T>(0), static_cast<T>(1), tolerance);
78  (*roots)[0] = (bracket.first + bracket.second) * 0.5;
79  return 1;
80  }
81 
82  }
83  else
84  {
85  // Build the monotonic intervals partitioning [0..1] to be analyzed one by one
86  // #performance HS-2016-feb-17 Test with boost::static_vector as this gets used by the CCD
87  std::vector<Interval<T>> intervals;
88 
89  T lastValue = static_cast<T>(0);
90  if (stationaryPoints[0] > static_cast<T>(0))
91  {
92  intervals.emplace_back(lastValue, stationaryPoints[0]);
93  lastValue = stationaryPoints[0];
94  }
95  if (stationaryPoints[1] < static_cast<T>(1))
96  {
97  intervals.emplace_back(lastValue, stationaryPoints[1]);
98  lastValue = stationaryPoints[1];
99  }
100  intervals.emplace_back(lastValue, static_cast<T>(1));
101 
102  for (auto interval : intervals)
103  {
104  // On each interval, only 1 root can be found
105  T pMin = p.evaluate(interval.getMin());
106  if (isNearZero(pMin, epsilon))
107  {
108  (*roots)[numberOfRoots++] = interval.getMin();
109  }
110  else
111  {
112  T pMax = p.evaluate(interval.getMax());
113  if (isNearZero(pMax, epsilon))
114  {
115  (*roots)[numberOfRoots++] = interval.getMax();
116  }
117  else if (pMin * pMax < 0)
118  {
119  auto bracket = bisect(p, interval.getMin(), interval.getMax(), tolerance);
120  (*roots)[numberOfRoots++] = (bracket.first + bracket.second) * 0.5;
121  }
122  }
123  }
124  }
125 
126  return numberOfRoots;
127 }
128 
129 }; // Math
130 }; // SurgSim
131 
132 #endif // SURGSIM_MATH_CUBICSOLVER_INL_H
PolynomialRoots.h
SurgSim::Math::findRootsInRange01
int findRootsInRange01(const Polynomial< T, 3 > &p, std::array< T, 3 > *roots)
Find all roots in range of a cubic equation.
Definition: CubicSolver-inl.h:35
SurgSim::Math::isNearZero
bool isNearZero(const T &value, const T &epsilon)
Define an utility function for comparing individual coefficients to 0.
Definition: Polynomial-inl.h:30
SurgSim::Math::Polynomial< T, 3 >::evaluate
T evaluate(const T &x) const
Evaluate the polynomial at a point.
Definition: Polynomial-inl.h:455
IntervalArithmetic.h
SurgSim::Math::Polynomial< T, 3 >::derivative
Polynomial< T, 2 > derivative() const
Definition: Polynomial-inl.h:536
SurgSim::Math::PolynomialRoots< T, 2 >
PolynomialRoots<T, 2> specializes the PolynomialRoots class for degree 2 (quadratic polynomials)
Definition: PolynomialRoots.h:97
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
SurgSim::Math::Polynomial< T, 3 >::getCoefficient
T getCoefficient(size_t i) const
Definition: Polynomial-inl.h:557
SurgSim::Math::Interval
Interval defines the concept of a mathematical interval and provides operations on it including arith...
Definition: IntervalArithmetic.h:35
SurgSim::Math::PolynomialRootsCommon::getNumRoots
int getNumRoots() const
Definition: PolynomialRoots-inl.h:32
SurgSim::Math::Interval::overlapsWith
bool overlapsWith(const Interval< T > &i) const
Definition: IntervalArithmetic-inl.h:86
SurgSim::Math::Polynomial< T, 3 >
Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials)
Definition: Polynomial.h:256
SurgSim::Math::Polynomial< T, 2 >
Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials)
Definition: Polynomial.h:184