Line |
Branch |
Exec |
Source |
1 |
|
|
// Copyright (c) 2008-2014, Andrew Walker |
2 |
|
|
// |
3 |
|
|
// Permission is hereby granted, free of charge, to any person obtaining a copy |
4 |
|
|
// of this software and associated documentation files (the "Software"), to deal |
5 |
|
|
// in the Software without restriction, including without limitation the rights |
6 |
|
|
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
7 |
|
|
// copies of the Software, and to permit persons to whom the Software is |
8 |
|
|
// furnished to do so, subject to the following conditions: |
9 |
|
|
// |
10 |
|
|
// The above copyright notice and this permission notice shall be included in |
11 |
|
|
// all copies or substantial portions of the Software. |
12 |
|
|
// |
13 |
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
14 |
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
15 |
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
16 |
|
|
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
17 |
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
18 |
|
|
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
19 |
|
|
// THE SOFTWARE. |
20 |
|
|
|
21 |
|
|
#include "dubins.hh" |
22 |
|
|
|
23 |
|
|
#include <assert.h> |
24 |
|
|
#include <math.h> |
25 |
|
|
|
26 |
|
|
DubinsWord dubins_words[] = { |
27 |
|
|
dubins_LSL, dubins_LSR, dubins_RSL, dubins_RSR, dubins_RLR, dubins_LRL, |
28 |
|
|
}; |
29 |
|
|
|
30 |
|
|
/** |
31 |
|
|
* Floating point modulus suitable for rings |
32 |
|
|
* |
33 |
|
|
* fmod doesn't behave correctly for angular quantities, this function does |
34 |
|
|
*/ |
35 |
|
✗ |
double fmodr(double x, double y) { return x - y * floor(x / y); } |
36 |
|
|
|
37 |
|
✗ |
double mod2pi(double theta) { return fmodr(theta, 2 * M_PI); } |
38 |
|
|
|
39 |
|
✗ |
int dubins_LSL(double alpha, double beta, double d, double* outputs) { |
40 |
|
✗ |
UNPACK_INPUTS(alpha, beta); |
41 |
|
✗ |
double tmp0 = d + sa - sb; |
42 |
|
✗ |
double p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb)); |
43 |
|
✗ |
if (p_squared < 0) { |
44 |
|
✗ |
return EDUBNOPATH; |
45 |
|
|
} |
46 |
|
✗ |
double tmp1 = atan2((cb - ca), tmp0); |
47 |
|
✗ |
double t = mod2pi(-alpha + tmp1); |
48 |
|
✗ |
double p = sqrt(p_squared); |
49 |
|
✗ |
double q = mod2pi(beta - tmp1); |
50 |
|
✗ |
PACK_OUTPUTS(outputs); |
51 |
|
✗ |
return EDUBOK; |
52 |
|
|
} |
53 |
|
|
|
54 |
|
✗ |
int dubins_RSR(double alpha, double beta, double d, double* outputs) { |
55 |
|
✗ |
UNPACK_INPUTS(alpha, beta); |
56 |
|
✗ |
double tmp0 = d - sa + sb; |
57 |
|
✗ |
double p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa)); |
58 |
|
✗ |
if (p_squared < 0) { |
59 |
|
✗ |
return EDUBNOPATH; |
60 |
|
|
} |
61 |
|
✗ |
double tmp1 = atan2((ca - cb), tmp0); |
62 |
|
✗ |
double t = mod2pi(alpha - tmp1); |
63 |
|
✗ |
double p = sqrt(p_squared); |
64 |
|
✗ |
double q = mod2pi(-beta + tmp1); |
65 |
|
✗ |
PACK_OUTPUTS(outputs); |
66 |
|
✗ |
return EDUBOK; |
67 |
|
|
} |
68 |
|
|
|
69 |
|
✗ |
int dubins_LSR(double alpha, double beta, double d, double* outputs) { |
70 |
|
✗ |
UNPACK_INPUTS(alpha, beta); |
71 |
|
✗ |
double p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb)); |
72 |
|
✗ |
if (p_squared < 0) { |
73 |
|
✗ |
return EDUBNOPATH; |
74 |
|
|
} |
75 |
|
✗ |
double p = sqrt(p_squared); |
76 |
|
✗ |
double tmp2 = atan2((-ca - cb), (d + sa + sb)) - atan2(-2.0, p); |
77 |
|
✗ |
double t = mod2pi(-alpha + tmp2); |
78 |
|
✗ |
double q = mod2pi(-mod2pi(beta) + tmp2); |
79 |
|
✗ |
PACK_OUTPUTS(outputs); |
80 |
|
✗ |
return EDUBOK; |
81 |
|
|
} |
82 |
|
|
|
83 |
|
✗ |
int dubins_RSL(double alpha, double beta, double d, double* outputs) { |
84 |
|
✗ |
UNPACK_INPUTS(alpha, beta); |
85 |
|
✗ |
double p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb)); |
86 |
|
✗ |
if (p_squared < 0) { |
87 |
|
✗ |
return EDUBNOPATH; |
88 |
|
|
} |
89 |
|
✗ |
double p = sqrt(p_squared); |
90 |
|
✗ |
double tmp2 = atan2((ca + cb), (d - sa - sb)) - atan2(2.0, p); |
91 |
|
✗ |
double t = mod2pi(alpha - tmp2); |
92 |
|
✗ |
double q = mod2pi(beta - tmp2); |
93 |
|
✗ |
PACK_OUTPUTS(outputs); |
94 |
|
✗ |
return EDUBOK; |
95 |
|
|
} |
96 |
|
|
|
97 |
|
✗ |
int dubins_RLR(double alpha, double beta, double d, double* outputs) { |
98 |
|
✗ |
UNPACK_INPUTS(alpha, beta); |
99 |
|
✗ |
double tmp_rlr = (6. - d * d + 2 * c_ab + 2 * d * (sa - sb)) / 8.; |
100 |
|
✗ |
if (fabs(tmp_rlr) > 1) { |
101 |
|
✗ |
return EDUBNOPATH; |
102 |
|
|
} |
103 |
|
✗ |
double p = mod2pi(2 * M_PI - acos(tmp_rlr)); |
104 |
|
✗ |
double t = mod2pi(alpha - atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.)); |
105 |
|
✗ |
double q = mod2pi(alpha - beta - t + mod2pi(p)); |
106 |
|
✗ |
PACK_OUTPUTS(outputs); |
107 |
|
✗ |
return EDUBOK; |
108 |
|
|
} |
109 |
|
|
|
110 |
|
✗ |
int dubins_LRL(double alpha, double beta, double d, double* outputs) { |
111 |
|
✗ |
UNPACK_INPUTS(alpha, beta); |
112 |
|
✗ |
double tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (-sa + sb)) / 8.; |
113 |
|
✗ |
if (fabs(tmp_lrl) > 1) { |
114 |
|
✗ |
return EDUBNOPATH; |
115 |
|
|
} |
116 |
|
✗ |
double p = mod2pi(2 * M_PI - acos(tmp_lrl)); |
117 |
|
✗ |
double t = mod2pi(-alpha - atan2(ca - cb, d + sa - sb) + p / 2.); |
118 |
|
✗ |
double q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p)); |
119 |
|
✗ |
PACK_OUTPUTS(outputs); |
120 |
|
✗ |
return EDUBOK; |
121 |
|
|
} |
122 |
|
|
|