PolynomialRoots-inl.h
Go to the documentation of this file.
// 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
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::PolynomialRootsCommon::getNumRoots
int getNumRoots() const
Definition: PolynomialRoots-inl.h:32
SurgSim::Math::Polynomial< T, 2 >::getCoefficient
T getCoefficient(size_t i) const
Definition: Polynomial-inl.h:387
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
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, 1 >::getCoefficient
T getCoefficient(size_t i) const
Definition: Polynomial-inl.h:235
SurgSim::Math::Polynomial< T, 2 >
Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials) ...
Definition: Polynomial.h:183
SurgSim::Math::PolynomialRoots
The (algebraic) roots of a Polynomial<N,T>.
Definition: PolynomialRoots.h:36
SurgSim::Math::PolynomialRootsCommon::operator[]
T operator[](int i) const
Read only access to the roots of the polynomial.
Definition: PolynomialRoots-inl.h:38
SurgSim::Math::PolynomialRootsCommon::isDegenerate
bool isDegenerate() const
Definition: PolynomialRoots-inl.h:26
SurgSim::Math::solve
void solve(const T &a, const T &b, const T &epsilon, int *numRoots, std::array< T, N > *roots)
Specialized solve routine for linear polynomials (2 coefficients)
Definition: PolynomialRoots-inl.h:64
SurgSim::Math::Polynomial< T, 1 >
Polynomial<T, 1> specializes the Polynomial class for degree 1 (linear polynomials) ...
Definition: Polynomial.h:117
SurgSim::Math::PolynomialRootsCommon
The common base class for PolynomialRoots specializations for various N.
Definition: PolynomialRoots.h:44