1 |
|
|
#include <sot/pattern-generator/step-checker.h> |
2 |
|
|
|
3 |
|
|
#include <cmath> |
4 |
|
|
|
5 |
|
|
namespace dynamicgraph { |
6 |
|
|
namespace sot { |
7 |
|
|
|
8 |
|
|
void StepChecker::clipStep(double x, double y, double &x_result, |
9 |
|
|
double &y_result) { |
10 |
|
|
// const double MIN_y = 0.16; |
11 |
|
|
// const double MAX_y = 0.40; |
12 |
|
|
// const double MAX_x = 0.25; |
13 |
|
|
// const double EDGE_POINT_x = 0.20; |
14 |
|
|
// const double EDGE_POINT_y = 0.30; |
15 |
|
|
|
16 |
|
|
const double MIN_y = 0.16; |
17 |
|
|
const double MAX_y = 0.30; |
18 |
|
|
const double MAX_x = 0.25; |
19 |
|
|
const double EDGE_POINT_x = 0.20; |
20 |
|
|
const double EDGE_POINT_y = 0.30; |
21 |
|
|
|
22 |
|
|
double x0 = std::abs(x); |
23 |
|
|
double y0 = std::abs(y); |
24 |
|
|
double alpha = y0 / x0; |
25 |
|
|
double y_inter = MIN_y; |
26 |
|
|
double x_inter = MIN_y / alpha; |
27 |
|
|
|
28 |
|
|
if (alpha < MIN_y / MAX_x) { |
29 |
|
|
x_result = MAX_x; |
30 |
|
|
y_result = MIN_y; |
31 |
|
|
} else if (alpha < EDGE_POINT_y / EDGE_POINT_x) { |
32 |
|
|
double delta1 = (MIN_y - EDGE_POINT_y) / (MAX_x - EDGE_POINT_x); |
33 |
|
|
double x_inter2 = (EDGE_POINT_y - delta1 * EDGE_POINT_x) / (alpha - delta1); |
34 |
|
|
double y_inter2 = alpha * x_inter2; |
35 |
|
|
if (x0 < x_inter) { |
36 |
|
|
x_result = x_inter; |
37 |
|
|
y_result = y_inter; |
38 |
|
|
} else if (x0 > x_inter2) { |
39 |
|
|
x_result = x_inter2; |
40 |
|
|
y_result = y_inter2; |
41 |
|
|
} else { |
42 |
|
|
x_result = x0; |
43 |
|
|
y_result = y0; |
44 |
|
|
} |
45 |
|
|
} else { |
46 |
|
|
double delta2 = (EDGE_POINT_y - MAX_y) / (EDGE_POINT_x); |
47 |
|
|
double x_inter2 = (MAX_y) / (alpha - delta2); |
48 |
|
|
double y_inter2 = alpha * x_inter2; |
49 |
|
|
if (x0 < x_inter) { |
50 |
|
|
x_result = x_inter; |
51 |
|
|
y_result = y_inter; |
52 |
|
|
} else if (x0 > x_inter2) { |
53 |
|
|
x_result = x_inter2; |
54 |
|
|
y_result = y_inter2; |
55 |
|
|
} else { |
56 |
|
|
x_result = x0; |
57 |
|
|
y_result = y0; |
58 |
|
|
} |
59 |
|
|
} |
60 |
|
|
|
61 |
|
|
if (x < 0) x_result = -x_result; |
62 |
|
|
if (y < 0) y_result = -y_result; |
63 |
|
|
} |
64 |
|
|
|
65 |
|
|
} // namespace sot |
66 |
|
|
} // namespace dynamicgraph |