PolynomialRoots-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, 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_POLYNOMIALROOTS_INL_H
17 #define SURGSIM_MATH_POLYNOMIALROOTS_INL_H
18 
19 namespace SurgSim
20 {
21 namespace Math
22 {
23 
24 // PolynomialRootsCommon
25 template <typename T, int N>
27 {
28  return m_numRoots == DEGENERATE;
29 }
30 
31 template <typename T, int N>
33 {
34  return m_numRoots;
35 }
36 
37 template <typename T, int N>
39 {
40  SURGSIM_ASSERT((m_numRoots > i) && (i >= 0)) <<
41  "Requesting a root beyond the number of roots available for this polynomial, " <<
42  "or a root with a negative index.";
43  return m_roots[i];
44 }
45 
46 // roots of an degree-1 polynomial (linear)
47 template <typename T>
49 {
50  solve<T, 1>(p.getCoefficient(1), p.getCoefficient(0), static_cast<T>(epsilon),
51  &(this->m_numRoots), &(this->m_roots));
52 }
53 
54 // roots of an degree-2 polynomial (quadratic)
55 template <typename T>
57 {
58  solve<T, 2>(p.getCoefficient(2), p.getCoefficient(1), p.getCoefficient(0), static_cast<T>(epsilon),
59  &(this->m_numRoots), &(this->m_roots));
60 }
61 
62 // Utilities: Solve for roots of linear equation a * x + b = y
63 template <typename T, int N>
64 void solve(const T& a, const T& b, const T& epsilon, int* numRoots, std::array<T, N>* roots)
65 {
66  static_assert(N >= 1, "Root array is not large enough to hold the roots of the polynomial");
67  if (isNearZero(a, epsilon))
68  {
69  // The "1-st degree polynomial" is really close to a constant.
70  // If the constant is zero, there are infinitely many solutions; otherwise there are zero.
71  if (isNearZero(b, epsilon))
72  {
73  *numRoots = PolynomialRootsCommon<T, N>::DEGENERATE; // infinitely many solutions
74  }
75  else
76  {
77  *numRoots = 0;
78  }
79  }
80  else
81  {
82  *numRoots = 1;
83  (*roots)[0] = -b / a;
84  }
85 }
86 
87 // Utilities: Solve for roots of quadratic equation a * x^2 + b * x + c = y
88 template <typename T, int N>
89 void solve(const T& a, const T& b, const T& c, const T& epsilon, int* numRoots, std::array<T, N>* roots)
90 {
91  static_assert(N >= 2, "Root array is not large enough to hold the roots of the polynomial");
92 
93  if (isNearZero(a, epsilon))
94  {
95  // The "2nd degree polynomial" is really (close to) 1st degree or less.
96  // We can delegate the solution in this case.
97  solve<T, N>(b, c, epsilon, numRoots, roots);
98  return;
99  }
100 
101  T discriminant = b * b - 4.0 * a * c;
102  if (discriminant > epsilon)
103  {
104  *numRoots = 2;
105  T sqrtDiscriminant = sqrt(discriminant);
106  (*roots)[0] = (-b - sqrtDiscriminant) / (2 * a);
107  (*roots)[1] = (-b + sqrtDiscriminant) / (2 * a);
108  if ((*roots)[0] > (*roots)[1])
109  {
110  std::swap((*roots)[0], (*roots)[1]);
111  }
112  }
113  else if (discriminant > -epsilon)
114  {
115  *numRoots = 1;
116  (*roots)[0] = -b / (2 * a);
117  }
118  else
119  {
120  *numRoots = 0;
121  }
122 }
123 
124 }; // Math
125 }; // SurgSim
126 
127 #endif // SURGSIM_MATH_POLYNOMIALROOTS_INL_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::PolynomialRoots
The (algebraic) roots of a Polynomial<N,T>.
Definition: PolynomialRoots.h:36
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::PolynomialRootsCommon
The common base class for PolynomialRoots specializations for various N.
Definition: PolynomialRoots.h:44
SurgSim::Math::Polynomial< T, 1 >::getCoefficient
T getCoefficient(size_t i) const
Definition: Polynomial-inl.h:235
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::Polynomial< T, 2 >::getCoefficient
T getCoefficient(size_t i) const
Definition: Polynomial-inl.h:387
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::Polynomial< T, 1 >
Polynomial<T, 1> specializes the Polynomial class for degree 1 (linear polynomials)
Definition: Polynomial.h:117
SurgSim::Math::PolynomialRootsCommon::isDegenerate
bool isDegenerate() const
Definition: PolynomialRoots-inl.h:26
SurgSim::Math::PolynomialRootsCommon::getNumRoots
int getNumRoots() const
Definition: PolynomialRoots-inl.h:32
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, 2 >
Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials)
Definition: Polynomial.h:183