/usr/include/SurgSim/Math/CubicSolver-inl.h is in libopensurgsim-dev 0.7.0-6ubuntu1.
This file is owned by root:root, with mode 0o644.
The actual contents of the file can be viewed below.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 | // 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
|