CubicSolver-inl.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_CUBICSOLVER_INL_H
#define SURGSIM_MATH_CUBICSOLVER_INL_H
#include <boost/math/tools/roots.hpp>
#include <limits>
#include <vector>
#include "SurgSim/Math/IntervalArithmetic.h"
#include "SurgSim/Math/PolynomialRoots.h"
using boost::math::tools::bisect;
namespace SurgSim
{
namespace Math
{
template <class T>
int findRootsInRange01(const Polynomial<T, 3>& p, std::array<T, 3>* roots)
{
int numberOfRoots = 0;
boost::math::tools::eps_tolerance<T> tolerance(std::numeric_limits<T>::digits - 3);
const T epsilon = 4 * std::numeric_limits<T>::epsilon();
// Is degenerate?
if (isNearZero(p.getCoefficient(3), epsilon))
{
Polynomial<T, 2> quadratic(p.getCoefficient(0), p.getCoefficient(1), p.getCoefficient(2));
PolynomialRoots<T, 2> quadraticRoots(quadratic, std::numeric_limits<T>::epsilon());
for (int i = 0; i < quadraticRoots.getNumRoots(); ++i)
{
if (quadraticRoots[i] >= 0.0 && quadraticRoots[i] <= 1.0)
{
(*roots)[numberOfRoots++] = quadraticRoots[i];
}
}
return numberOfRoots;
}
PolynomialRoots<T, 2> stationaryPoints(p.derivative(), std::numeric_limits<T>::epsilon());
if (stationaryPoints.getNumRoots() < 2 ||
!Interval<T>(0, 1).overlapsWith(Interval<T>(stationaryPoints[0], stationaryPoints[1])))
{
T p0 = p.getCoefficient(0); // p.evaluate(static_cast<T>(0));
if (isNearZero(p0, epsilon))
{
(*roots)[0] = 0.0;
return 1;
}
T p1 = p.evaluate(static_cast<T>(1));
if (isNearZero(p1, epsilon))
{
(*roots)[0] = static_cast<T>(1);
return 1;
}
if (p0 * p1 < 0)
{
auto bracket = bisect(p, static_cast<T>(0), static_cast<T>(1), tolerance);
(*roots)[0] = (bracket.first + bracket.second) * 0.5;
return 1;
}
}
else
{
// Build the monotonic intervals partitioning [0..1] to be analyzed one by one
// #performance HS-2016-feb-17 Test with boost::static_vector as this gets used by the CCD
std::vector<Interval<T>> intervals;
T lastValue = static_cast<T>(0);
if (stationaryPoints[0] > static_cast<T>(0))
{
intervals.emplace_back(lastValue, stationaryPoints[0]);
lastValue = stationaryPoints[0];
}
if (stationaryPoints[1] < static_cast<T>(1))
{
intervals.emplace_back(lastValue, stationaryPoints[1]);
lastValue = stationaryPoints[1];
}
intervals.emplace_back(lastValue, static_cast<T>(1));
for (auto interval : intervals)
{
// On each interval, only 1 root can be found
T pMin = p.evaluate(interval.getMin());
if (isNearZero(pMin, epsilon))
{
(*roots)[numberOfRoots++] = interval.getMin();
}
else
{
T pMax = p.evaluate(interval.getMax());
if (isNearZero(pMax, epsilon))
{
(*roots)[numberOfRoots++] = interval.getMax();
}
else if (pMin * pMax < 0)
{
auto bracket = bisect(p, interval.getMin(), interval.getMax(), tolerance);
(*roots)[numberOfRoots++] = (bracket.first + bracket.second) * 0.5;
}
}
}
}
return numberOfRoots;
}
}; // Math
}; // SurgSim
#endif // SURGSIM_MATH_CUBICSOLVER_INL_H
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
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::PolynomialRoots< T, 2 >
PolynomialRoots<T, 2> specializes the PolynomialRoots class for degree 2 (quadratic polynomials) ...
Definition: PolynomialRoots.h:96
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 >
Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials) ...
Definition: Polynomial.h:255
PolynomialRoots.h
SurgSim::Math::Polynomial< T, 2 >
Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials) ...
Definition: Polynomial.h:183
SurgSim::Math::Polynomial< T, 3 >::getCoefficient
T getCoefficient(size_t i) const
Definition: Polynomial-inl.h:557
SurgSim::Math::Polynomial< T, 3 >::derivative
Polynomial< T, 2 > derivative() const
Definition: Polynomial-inl.h:536
SurgSim::Math::Interval
Interval defines the concept of a mathematical interval and provides operations on it including arith...
Definition: IntervalArithmetic.h:34
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