Eclipse SUMO - Simulation of Urban MObility
PolySolver.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2020 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials are made available under the
5 // terms of the Eclipse Public License 2.0 which is available at
6 // https://www.eclipse.org/legal/epl-2.0/
7 // This Source Code may also be made available under the following Secondary
8 // Licenses when the conditions for such availability set forth in the Eclipse
9 // Public License 2.0 are satisfied: GNU General Public License, version 2
10 // or later which is available at
11 // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 /****************************************************************************/
19 //
20 /****************************************************************************/
21 #include <math.h>
22 #include <cmath>
23 #include <limits>
24 #include <utils/geom/GeomHelper.h> // defines M_PI
25 #include "PolySolver.h"
26 
27 
28 // ===========================================================================
29 // static member variables
30 // ===========================================================================
31 // (none)
32 
33 // ===========================================================================
34 // member method definitions
35 // ===========================================================================
36 std::tuple<int, double, double>
37 PolySolver::quadraticSolve(double a, double b, double c) {
38  // ax^2 + bx + c = 0
39  // return only real roots
40  if (a == 0) {
41  //WRITE_WARNING("The coefficient of the quadrat of x is 0. Please use the utility for a FIRST degree linear. No further action taken.");
42  if (b == 0) {
43  if (c == 0) {
44  return std::make_tuple(2, std::numeric_limits<double>::infinity(), -std::numeric_limits<double>::infinity());
45  } else {
46  return std::make_tuple(0, NAN, NAN);
47  }
48  } else {
49  return std::make_tuple(1, -c / b, NAN);
50  }
51  }
52 
53  if (c == 0) {
54  //WRITE_WARNING("One root is 0. Now divide through by x and use the utility for a FIRST degree linear to solve the resulting equation for the other one root. No further action taken.");
55  return std::make_tuple(2, 0, -b / a);
56  }
57 
58  double disc, x1_real, x2_real ;
59  disc = b * b - 4 * a * c;
60 
61  if (disc > 0) {
62  // two different real roots
63  x1_real = (-b + sqrt(disc)) / (2 * a);
64  x2_real = (-b - sqrt(disc)) / (2 * a);
65  return std::make_tuple(2, x1_real, x2_real);
66  } else if (disc == 0) {
67  // a real root with multiplicity 2
68  x1_real = (-b + sqrt(disc)) / (2 * a);
69  return std::make_tuple(1, x1_real, NAN);
70  } else {
71  // all roots are complex
72  return std::make_tuple(0, NAN, NAN);
73  }
74 }
75 
76 std::tuple<int, double, double, double>
77 PolySolver::cubicSolve(double a, double b, double c, double d) {
78  // ax^3 + bx^2 + cx + d = 0
79  // return only real roots
80  if (a == 0) {
81  //WRITE_WARNING("The coefficient of the cube of x is 0. Please use the utility for a SECOND degree quadratic. No further action taken.");
82  int numX;
83  double x1, x2;
84  std::tie(numX, x1, x2) = quadraticSolve(b, c, d);
85  return std::make_tuple(numX, x1, x2, NAN);
86  }
87  if (d == 0) {
88  //WRITE_WARNING("One root is 0. Now divide through by x and use the utility for a SECOND degree quadratic to solve the resulting equation for the other two roots. No further action taken.");
89  int numX;
90  double x1, x2;
91  std::tie(numX, x1, x2) = quadraticSolve(a, b, c);
92  return std::make_tuple(numX + 1, 0, x1, x2);
93  }
94 
95  b /= a;
96  c /= a;
97  d /= a;
98 
99  double disc, q, r, dum1, s, t, term1, r13;
100  q = (3.0 * c - (b * b)) / 9.0;
101  r = -(27.0 * d) + b * (9.0 * c - 2.0 * (b * b));
102  r /= 54.0;
103  disc = q * q * q + r * r;
104  term1 = (b / 3.0);
105 
106  double x1_real, x2_real, x3_real;
107  if (disc > 0) { // One root real, two are complex
108  s = r + sqrt(disc);
109  s = s < 0 ? -cbrt(-s) : cbrt(s);
110  t = r - sqrt(disc);
111  t = t < 0 ? -cbrt(-t) : cbrt(t);
112  x1_real = -term1 + s + t;
113  term1 += (s + t) / 2.0;
114  x3_real = x2_real = -term1;
115  return std::make_tuple(1, x1_real, NAN, NAN);
116  }
117  // The remaining options are all real
118  else if (disc == 0) { // All roots real, at least two are equal.
119  r13 = r < 0 ? -cbrt(-r) : cbrt(r);
120  x1_real = -term1 + 2.0 * r13;
121  x3_real = x2_real = -(r13 + term1);
122  return std::make_tuple(2, x1_real, x2_real, NAN);
123  }
124  // Only option left is that all roots are real and unequal (to get here, q < 0)
125  else {
126  q = -q;
127  dum1 = q * q * q;
128  dum1 = acos(r / sqrt(dum1));
129  r13 = 2.0 * sqrt(q);
130  x1_real = -term1 + r13 * cos(dum1 / 3.0);
131  x2_real = -term1 + r13 * cos((dum1 + 2.0 * M_PI) / 3.0);
132  x3_real = -term1 + r13 * cos((dum1 + 4.0 * M_PI) / 3.0);
133  return std::make_tuple(3, x1_real, x2_real, x3_real);
134  }
135 }
136 
137 
138 /****************************************************************************/
static std::tuple< int, double, double, double > cubicSolve(double a, double b, double c, double d)
Solver of cubic equation ax^3 + bx^2 + cx + d = 0.
Definition: PolySolver.cpp:77
static std::tuple< int, double, double > quadraticSolve(double a, double b, double c)
Solver of quadratic equation ax^2 + bx + c = 0.
Definition: PolySolver.cpp:37
#define M_PI
Definition: odrSpiral.cpp:40