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
|
/*************************************************************************
* Copyright (c) 2011 AT&T Intellectual Property
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* https://www.eclipse.org/legal/epl-v10.html
*
* Contributors: Details at https://graphviz.org
*************************************************************************/
#include <math.h>
#include <pathplan/solvers.h>
static int solve1(double *coeff, double *roots);
static int solve2(double *coeff, double *roots);
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#define EPS 1E-7
#define AEQ0(x) (((x) < EPS) && ((x) > -EPS))
int solve3(double *coeff, double *roots)
{
double a, b, c, d;
int rootn, i;
double p, q, disc, b_over_3a, c_over_a, d_over_a;
double r, theta, temp, alpha, beta;
a = coeff[3], b = coeff[2], c = coeff[1], d = coeff[0];
if (AEQ0(a))
return solve2(coeff, roots);
b_over_3a = b / (3 * a);
c_over_a = c / a;
d_over_a = d / a;
p = b_over_3a * b_over_3a;
q = 2 * b_over_3a * p - b_over_3a * c_over_a + d_over_a;
p = c_over_a / 3 - p;
disc = q * q + 4 * p * p * p;
if (disc < 0) {
r = .5 * sqrt(-disc + q * q);
theta = atan2(sqrt(-disc), -q);
temp = 2 * cbrt(r);
roots[0] = temp * cos(theta / 3);
roots[1] = temp * cos((theta + M_PI + M_PI) / 3);
roots[2] = temp * cos((theta - M_PI - M_PI) / 3);
rootn = 3;
} else {
alpha = .5 * (sqrt(disc) - q);
beta = -q - alpha;
roots[0] = cbrt(alpha) + cbrt(beta);
if (disc > 0)
rootn = 1;
else
roots[1] = roots[2] = -.5 * roots[0], rootn = 3;
}
for (i = 0; i < rootn; i++)
roots[i] -= b_over_3a;
return rootn;
}
static int solve2(double *coeff, double *roots)
{
double a, b, c;
double disc, b_over_2a, c_over_a;
a = coeff[2], b = coeff[1], c = coeff[0];
if (AEQ0(a))
return solve1(coeff, roots);
b_over_2a = b / (2 * a);
c_over_a = c / a;
disc = b_over_2a * b_over_2a - c_over_a;
if (disc < 0) {
return 0;
} else if (disc > 0) {
roots[0] = -b_over_2a + sqrt(disc);
roots[1] = -2 * b_over_2a - roots[0];
return 2;
}
roots[0] = -b_over_2a;
return 1;
}
static int solve1(double *coeff, double *roots)
{
double a, b;
a = coeff[1], b = coeff[0];
if (AEQ0(a)) {
if (AEQ0(b))
return 4;
else
return 0;
}
roots[0] = -b / a;
return 1;
}
|
