/usr/include/SurgSim/Math/PolynomialRoots-inl.h is in libopensurgsim-dev 0.7.0-5.
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 | // This file is a part of the OpenSurgSim project.
// Copyright 2013, 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_POLYNOMIALROOTS_INL_H
#define SURGSIM_MATH_POLYNOMIALROOTS_INL_H
namespace SurgSim
{
namespace Math
{
// PolynomialRootsCommon
template <typename T, int N>
bool PolynomialRootsCommon<T, N>::isDegenerate() const
{
return m_numRoots == DEGENERATE;
}
template <typename T, int N>
int PolynomialRootsCommon<T, N>::getNumRoots() const
{
return m_numRoots;
}
template <typename T, int N>
T PolynomialRootsCommon<T, N>::operator[](const int i) const
{
SURGSIM_ASSERT((m_numRoots > i) && (i >= 0)) <<
"Requesting a root beyond the number of roots available for this polynomial, " <<
"or a root with a negative index.";
return m_roots[i];
}
// roots of an degree-1 polynomial (linear)
template <typename T>
PolynomialRoots<T, 1>::PolynomialRoots(const Polynomial<T, 1>& p, const T& epsilon)
{
solve<T, 1>(p.getCoefficient(1), p.getCoefficient(0), static_cast<T>(epsilon),
&(this->m_numRoots), &(this->m_roots));
}
// roots of an degree-2 polynomial (quadratic)
template <typename T>
PolynomialRoots<T, 2>::PolynomialRoots(const Polynomial<T, 2>& p, const T& epsilon)
{
solve<T, 2>(p.getCoefficient(2), p.getCoefficient(1), p.getCoefficient(0), static_cast<T>(epsilon),
&(this->m_numRoots), &(this->m_roots));
}
// Utilities: Solve for roots of linear equation a * x + b = y
template <typename T, int N>
void solve(const T& a, const T& b, const T& epsilon, int* numRoots, std::array<T, N>* roots)
{
static_assert(N >= 1, "Root array is not large enough to hold the roots of the polynomial");
if (isNearZero(a, epsilon))
{
// The "1-st degree polynomial" is really close to a constant.
// If the constant is zero, there are infinitely many solutions; otherwise there are zero.
if (isNearZero(b, epsilon))
{
*numRoots = PolynomialRootsCommon<T, N>::DEGENERATE; // infinitely many solutions
}
else
{
*numRoots = 0;
}
}
else
{
*numRoots = 1;
(*roots)[0] = -b / a;
}
}
// Utilities: Solve for roots of quadratic equation a * x^2 + b * x + c = y
template <typename T, int N>
void solve(const T& a, const T& b, const T& c, const T& epsilon, int* numRoots, std::array<T, N>* roots)
{
static_assert(N >= 2, "Root array is not large enough to hold the roots of the polynomial");
if (isNearZero(a, epsilon))
{
// The "2nd degree polynomial" is really (close to) 1st degree or less.
// We can delegate the solution in this case.
solve<T, N>(b, c, epsilon, numRoots, roots);
return;
}
T discriminant = b * b - 4.0 * a * c;
if (discriminant > epsilon)
{
*numRoots = 2;
T sqrtDiscriminant = sqrt(discriminant);
(*roots)[0] = (-b - sqrtDiscriminant) / (2 * a);
(*roots)[1] = (-b + sqrtDiscriminant) / (2 * a);
if ((*roots)[0] > (*roots)[1])
{
std::swap((*roots)[0], (*roots)[1]);
}
}
else if (discriminant > -epsilon)
{
*numRoots = 1;
(*roots)[0] = -b / (2 * a);
}
else
{
*numRoots = 0;
}
}
}; // Math
}; // SurgSim
#endif // SURGSIM_MATH_POLYNOMIALROOTS_INL_H
|