From ded2c76ec8d5c155f1753e3f20f5c1179cb2d211 Mon Sep 17 00:00:00 2001 From: agennart Date: Tue, 5 Aug 2025 17:00:06 +0200 Subject: [PATCH] Define PI and other constant and constexpr instead of macro Otherwise it might interfer with another symbol defined in another library. This was the case for the symbol PI and was causing the build to fail with the following error: `error: expected unqualified-id before numeric constant` In order to ensure that there is no confusion possible, also prefix all the PI constants by STEERING. --- .../utilities/utilities.hpp | 12 +-- src/dubins_state_space/dubins_state_space.cpp | 4 +- src/filter/ekf.cpp | 16 ++-- .../cc00_dubins_state_space.cpp | 24 +++--- .../cc00_reeds_shepp_state_space.cpp | 80 +++++++++---------- .../cc0pm_dubins_state_space.cpp | 24 +++--- .../ccpm0_dubins_state_space.cpp | 24 +++--- .../ccpmpm_dubins_state_space.cpp | 24 +++--- .../hc00_reeds_shepp_state_space.cpp | 80 +++++++++---------- .../hc0pm_reeds_shepp_state_space.cpp | 80 +++++++++---------- src/hc_cc_state_space/hc_cc_circle.cpp | 26 +++--- .../hcpm0_reeds_shepp_state_space.cpp | 80 +++++++++---------- .../hcpmpm_reeds_shepp_state_space.cpp | 80 +++++++++---------- .../reeds_shepp_state_space.cpp | 66 +++++++-------- src/utilities/utilities.cpp | 30 +++---- 15 files changed, 325 insertions(+), 325 deletions(-) diff --git a/include/steering_functions/utilities/utilities.hpp b/include/steering_functions/utilities/utilities.hpp index a132a4a..c212f7d 100755 --- a/include/steering_functions/utilities/utilities.hpp +++ b/include/steering_functions/utilities/utilities.hpp @@ -34,12 +34,12 @@ using namespace std; -#define PI 3.1415926535897932384 -#define HALF_PI 1.5707963267948966192 -#define TWO_PI 6.2831853071795864770 -#define SQRT_PI 1.7724538509055160273 -#define SQRT_PI_INV 0.56418958354775628695 -#define SQRT_TWO_PI_INV 0.39894228040143267794 +constexpr double STEERING_PI = 3.1415926535897932384; +constexpr double STEERING_HALF_PI = 1.5707963267948966192; +constexpr double STEERING_TWO_PI = 6.2831853071795864770; +constexpr double STEERING_SQRT_PI = 1.7724538509055160273; +constexpr double STEERING_SQRT_PI_INV = 0.56418958354775628695; +constexpr double STEERING_SQRT_TWO_PI_INV = 0.39894228040143267794; const double epsilon = 1e-4; diff --git a/src/dubins_state_space/dubins_state_space.cpp b/src/dubins_state_space/dubins_state_space.cpp index 4f3265a..8b373de 100644 --- a/src/dubins_state_space/dubins_state_space.cpp +++ b/src/dubins_state_space/dubins_state_space.cpp @@ -142,7 +142,7 @@ Dubins_State_Space::Dubins_Path dubinsRLR(double d, double alpha, double beta) double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb))); if (fabs(tmp) < 1.) { - double p = TWO_PI - acos(tmp); + double p = STEERING_TWO_PI - acos(tmp); double theta = atan2(ca - cb, d - sa + sb); double t = twopify(alpha - theta + .5 * p); double q = twopify(alpha - beta - t + p); @@ -160,7 +160,7 @@ Dubins_State_Space::Dubins_Path dubinsLRL(double d, double alpha, double beta) double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb))); if (fabs(tmp) < 1.) { - double p = TWO_PI - acos(tmp); + double p = STEERING_TWO_PI - acos(tmp); double theta = atan2(-ca + cb, d + sa - sb); double t = twopify(-alpha + theta + .5 * p); double q = twopify(beta - alpha - t + p); diff --git a/src/filter/ekf.cpp b/src/filter/ekf.cpp index 5b6520e..5042221 100644 --- a/src/filter/ekf.cpp +++ b/src/filter/ekf.cpp @@ -65,10 +65,10 @@ void EKF::get_motion_jacobi(const State &state, const Control &control, double i double abs_sigma = fabs(control.sigma); double sqrt_sigma_inv = 1 / sqrt(abs_sigma); double k1 = state.theta - 0.5 * d * state.kappa * state.kappa / control.sigma; - double k2 = SQRT_PI_INV * sqrt_sigma_inv * (abs_sigma * integration_step + sgn_sigma * state.kappa); - double k3 = SQRT_PI_INV * sqrt_sigma_inv * sgn_sigma * state.kappa; - double k4 = k1 + d * sgn_sigma * HALF_PI * k2 * k2; - double k5 = k1 + d * sgn_sigma * HALF_PI * k3 * k3; + double k2 = STEERING_SQRT_PI_INV * sqrt_sigma_inv * (abs_sigma * integration_step + sgn_sigma * state.kappa); + double k3 = STEERING_SQRT_PI_INV * sqrt_sigma_inv * sgn_sigma * state.kappa; + double k4 = k1 + d * sgn_sigma * STEERING_HALF_PI * k2 * k2; + double k5 = k1 + d * sgn_sigma * STEERING_HALF_PI * k3 * k3; double cos_k1 = cos(k1); double sin_k1 = sin(k1); double cos_k4 = cos(k4); @@ -86,9 +86,9 @@ void EKF::get_motion_jacobi(const State &state, const Control &control, double i F_x(0, 0) = 1.0; F_x(1, 1) = 1.0; - F_x(0, 2) = SQRT_PI * sqrt_sigma_inv * + F_x(0, 2) = STEERING_SQRT_PI* sqrt_sigma_inv * (-d * sin_k1 * (fresnel_c_k2 - fresnel_c_k3) - sgn_sigma * cos_k1 * (fresnel_s_k2 - fresnel_s_k3)); - F_x(1, 2) = SQRT_PI * sqrt_sigma_inv * + F_x(1, 2) = STEERING_SQRT_PI* sqrt_sigma_inv * (d * cos_k1 * (fresnel_c_k2 - fresnel_c_k3) - sgn_sigma * sin_k1 * (fresnel_s_k2 - fresnel_s_k3)); F_x(2, 2) = 1.0; @@ -97,11 +97,11 @@ void EKF::get_motion_jacobi(const State &state, const Control &control, double i F_u(1, 0) = sin_k4; F_u(2, 0) = state.kappa + control.sigma * integration_step; - F_u(0, 1) = SQRT_PI * sqrt_sigma_inv * state.kappa * + F_u(0, 1) = STEERING_SQRT_PI* sqrt_sigma_inv * state.kappa * (sgn_sigma * sin_k1 * (fresnel_c_k2 - fresnel_c_k3) + d * cos_k1 * (fresnel_s_k2 - fresnel_s_k3)) / abs_sigma + d * (cos_k4 - cos_k5) / control.sigma; - F_u(1, 1) = SQRT_PI * sqrt_sigma_inv * state.kappa * + F_u(1, 1) = STEERING_SQRT_PI* sqrt_sigma_inv * state.kappa * (-sgn_sigma * cos_k1 * (fresnel_c_k2 - fresnel_c_k3) + d * sin_k1 * (fresnel_s_k2 - fresnel_s_k3)) / abs_sigma + d * (sin_k4 - sin_k5) / control.sigma; diff --git a/src/hc_cc_state_space/cc00_dubins_state_space.cpp b/src/hc_cc_state_space/cc00_dubins_state_space.cpp index f2e4d3a..42531e7 100644 --- a/src/hc_cc_state_space/cc00_dubins_state_space.cpp +++ b/src/hc_cc_state_space/cc00_dubins_state_space.cpp @@ -63,22 +63,22 @@ class CC00_Dubins_State_Space::CC00_Dubins { if (c1.forward) { - theta = angle + HALF_PI - c1.mu; + theta = angle + STEERING_HALF_PI - c1.mu; } else { - theta = angle + HALF_PI + c1.mu; + theta = angle + STEERING_HALF_PI + c1.mu; } } else { if (c1.forward) { - theta = angle - HALF_PI + c1.mu; + theta = angle - STEERING_HALF_PI + c1.mu; } else { - theta = angle - HALF_PI - c1.mu; + theta = angle - STEERING_HALF_PI - c1.mu; } } *q = new Configuration(x, y, theta, 0); @@ -145,9 +145,9 @@ class CC00_Dubins_State_Space::CC00_Dubins { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -161,9 +161,9 @@ class CC00_Dubins_State_Space::CC00_Dubins { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -184,9 +184,9 @@ class CC00_Dubins_State_Space::CC00_Dubins if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -198,9 +198,9 @@ class CC00_Dubins_State_Space::CC00_Dubins if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } diff --git a/src/hc_cc_state_space/cc00_reeds_shepp_state_space.cpp b/src/hc_cc_state_space/cc00_reeds_shepp_state_space.cpp index 6eaf151..3484268 100644 --- a/src/hc_cc_state_space/cc00_reeds_shepp_state_space.cpp +++ b/src/hc_cc_state_space/cc00_reeds_shepp_state_space.cpp @@ -65,22 +65,22 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI - c1.mu; + theta = angle + STEERING_HALF_PI - c1.mu; } else { - theta = angle + HALF_PI + c1.mu; + theta = angle + STEERING_HALF_PI + c1.mu; } } else { if (c1.forward) { - theta = angle - HALF_PI + c1.mu; + theta = angle - STEERING_HALF_PI + c1.mu; } else { - theta = angle - HALF_PI - c1.mu; + theta = angle - STEERING_HALF_PI - c1.mu; } } *q = new Configuration(x, y, theta, 0); @@ -117,12 +117,12 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } else { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } } @@ -130,12 +130,12 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { if (c1.forward) { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } else { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } } @@ -413,9 +413,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -429,9 +429,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -452,9 +452,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -466,9 +466,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -1067,9 +1067,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1083,9 +1083,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1107,9 +1107,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1121,9 +1121,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (!c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1198,9 +1198,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -1214,9 +1214,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2); } @@ -1237,9 +1237,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -1251,9 +1251,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } return c1.cc_turn_length(**q1) + configuration_distance(**q1, **q2) + c2.cc_turn_length(**q2); } @@ -1313,9 +1313,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1329,9 +1329,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1353,9 +1353,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1367,9 +1367,9 @@ class CC00_Reeds_Shepp_State_Space::CC00_Reeds_Shepp if (!c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { diff --git a/src/hc_cc_state_space/cc0pm_dubins_state_space.cpp b/src/hc_cc_state_space/cc0pm_dubins_state_space.cpp index 53d5d57..26b4573 100644 --- a/src/hc_cc_state_space/cc0pm_dubins_state_space.cpp +++ b/src/hc_cc_state_space/cc0pm_dubins_state_space.cpp @@ -63,22 +63,22 @@ class CC0pm_Dubins_State_Space::CC0pm_Dubins { if (c1.forward) { - theta = angle + HALF_PI - c1.mu; + theta = angle + STEERING_HALF_PI - c1.mu; } else { - theta = angle + HALF_PI + c1.mu; + theta = angle + STEERING_HALF_PI + c1.mu; } } else { if (c1.forward) { - theta = angle - HALF_PI + c1.mu; + theta = angle - STEERING_HALF_PI + c1.mu; } else { - theta = angle - HALF_PI - c1.mu; + theta = angle - STEERING_HALF_PI - c1.mu; } } *q = new Configuration(x, y, theta, 0); @@ -149,9 +149,9 @@ class CC0pm_Dubins_State_Space::CC0pm_Dubins { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -165,9 +165,9 @@ class CC0pm_Dubins_State_Space::CC0pm_Dubins { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -188,9 +188,9 @@ class CC0pm_Dubins_State_Space::CC0pm_Dubins if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -202,9 +202,9 @@ class CC0pm_Dubins_State_Space::CC0pm_Dubins if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } diff --git a/src/hc_cc_state_space/ccpm0_dubins_state_space.cpp b/src/hc_cc_state_space/ccpm0_dubins_state_space.cpp index 0653c12..4813308 100644 --- a/src/hc_cc_state_space/ccpm0_dubins_state_space.cpp +++ b/src/hc_cc_state_space/ccpm0_dubins_state_space.cpp @@ -63,22 +63,22 @@ class CCpm0_Dubins_State_Space::CCpm0_Dubins { if (c1.forward) { - theta = angle + HALF_PI - c2.mu; + theta = angle + STEERING_HALF_PI - c2.mu; } else { - theta = angle + HALF_PI + c2.mu; + theta = angle + STEERING_HALF_PI + c2.mu; } } else { if (c1.forward) { - theta = angle - HALF_PI + c2.mu; + theta = angle - STEERING_HALF_PI + c2.mu; } else { - theta = angle - HALF_PI - c2.mu; + theta = angle - STEERING_HALF_PI - c2.mu; } } *q = new Configuration(x, y, theta, 0); @@ -149,9 +149,9 @@ class CCpm0_Dubins_State_Space::CCpm0_Dubins { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -165,9 +165,9 @@ class CCpm0_Dubins_State_Space::CCpm0_Dubins { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -188,9 +188,9 @@ class CCpm0_Dubins_State_Space::CCpm0_Dubins if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -202,9 +202,9 @@ class CCpm0_Dubins_State_Space::CCpm0_Dubins if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } diff --git a/src/hc_cc_state_space/ccpmpm_dubins_state_space.cpp b/src/hc_cc_state_space/ccpmpm_dubins_state_space.cpp index 08e1115..679f1d8 100644 --- a/src/hc_cc_state_space/ccpmpm_dubins_state_space.cpp +++ b/src/hc_cc_state_space/ccpmpm_dubins_state_space.cpp @@ -63,22 +63,22 @@ class CCpmpm_Dubins_State_Space::CCpmpm_Dubins { if (c1.forward) { - theta = angle + HALF_PI - parent_->mu_; + theta = angle + STEERING_HALF_PI - parent_->mu_; } else { - theta = angle + HALF_PI + parent_->mu_; + theta = angle + STEERING_HALF_PI + parent_->mu_; } } else { if (c1.forward) { - theta = angle - HALF_PI + parent_->mu_; + theta = angle - STEERING_HALF_PI + parent_->mu_; } else { - theta = angle - HALF_PI - parent_->mu_; + theta = angle - STEERING_HALF_PI - parent_->mu_; } } *q = new Configuration(x, y, theta, 0); @@ -150,9 +150,9 @@ class CCpmpm_Dubins_State_Space::CCpmpm_Dubins { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -166,9 +166,9 @@ class CCpmpm_Dubins_State_Space::CCpmpm_Dubins { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -189,9 +189,9 @@ class CCpmpm_Dubins_State_Space::CCpmpm_Dubins if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -203,9 +203,9 @@ class CCpmpm_Dubins_State_Space::CCpmpm_Dubins if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } diff --git a/src/hc_cc_state_space/hc00_reeds_shepp_state_space.cpp b/src/hc_cc_state_space/hc00_reeds_shepp_state_space.cpp index 011bed5..e1a8ee5 100644 --- a/src/hc_cc_state_space/hc00_reeds_shepp_state_space.cpp +++ b/src/hc_cc_state_space/hc00_reeds_shepp_state_space.cpp @@ -65,22 +65,22 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI - c1.mu; + theta = angle + STEERING_HALF_PI - c1.mu; } else { - theta = angle + HALF_PI + c1.mu; + theta = angle + STEERING_HALF_PI + c1.mu; } } else { if (c1.forward) { - theta = angle - HALF_PI + c1.mu; + theta = angle - STEERING_HALF_PI + c1.mu; } else { - theta = angle - HALF_PI - c1.mu; + theta = angle - STEERING_HALF_PI - c1.mu; } } *q = new Configuration(x, y, theta, 0); @@ -120,12 +120,12 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } else { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } } @@ -133,12 +133,12 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { if (c1.forward) { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } else { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } } @@ -428,9 +428,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -444,9 +444,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -467,9 +467,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -481,9 +481,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -1117,9 +1117,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1133,9 +1133,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1163,9 +1163,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1179,9 +1179,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1263,9 +1263,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1279,9 +1279,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *cstart = new HC_CC_Circle(c1.start, c1.left, c1.forward, CC_REGULAR, parent_->hc_cc_circle_param_); *cend = new HC_CC_Circle(c2); @@ -1309,9 +1309,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1325,9 +1325,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *cstart = new HC_CC_Circle(c1.start, c1.left, c1.forward, CC_REGULAR, parent_->hc_cc_circle_param_); *cend = new HC_CC_Circle(c2); @@ -1391,9 +1391,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1407,9 +1407,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { @@ -1434,9 +1434,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp if (c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1448,9 +1448,9 @@ class HC00_Reeds_Shepp_State_Space::HC00_Reeds_Shepp if (!c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { diff --git a/src/hc_cc_state_space/hc0pm_reeds_shepp_state_space.cpp b/src/hc_cc_state_space/hc0pm_reeds_shepp_state_space.cpp index 9d56089..ed8e54e 100644 --- a/src/hc_cc_state_space/hc0pm_reeds_shepp_state_space.cpp +++ b/src/hc_cc_state_space/hc0pm_reeds_shepp_state_space.cpp @@ -66,22 +66,22 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI - c1.mu; + theta = angle + STEERING_HALF_PI - c1.mu; } else { - theta = angle + HALF_PI + c1.mu; + theta = angle + STEERING_HALF_PI + c1.mu; } } else { if (c1.forward) { - theta = angle - HALF_PI + c1.mu; + theta = angle - STEERING_HALF_PI + c1.mu; } else { - theta = angle - HALF_PI - c1.mu; + theta = angle - STEERING_HALF_PI - c1.mu; } } *q = new Configuration(x, y, theta, 0); @@ -122,12 +122,12 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } else { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } } @@ -135,12 +135,12 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { if (c1.forward) { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } else { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } } @@ -435,9 +435,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -451,9 +451,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -474,9 +474,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -488,9 +488,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -1143,9 +1143,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1159,9 +1159,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1190,9 +1190,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1206,9 +1206,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1291,9 +1291,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1307,9 +1307,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *cstart = new HC_CC_Circle(c1.start, c1.left, c1.forward, CC_REGULAR, parent_->hc_cc_circle_param_); *cend = new HC_CC_Circle(c2); @@ -1338,9 +1338,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1354,9 +1354,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *cstart = new HC_CC_Circle(c1.start, c1.left, c1.forward, CC_REGULAR, parent_->hc_cc_circle_param_); *cend = new HC_CC_Circle(c2); @@ -1420,9 +1420,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1436,9 +1436,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { @@ -1463,9 +1463,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp if (c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1477,9 +1477,9 @@ class HC0pm_Reeds_Shepp_State_Space::HC0pm_Reeds_Shepp if (!c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { diff --git a/src/hc_cc_state_space/hc_cc_circle.cpp b/src/hc_cc_state_space/hc_cc_circle.cpp index db49825..6557e07 100644 --- a/src/hc_cc_state_space/hc_cc_circle.cpp +++ b/src/hc_cc_state_space/hc_cc_circle.cpp @@ -119,7 +119,7 @@ double HC_CC_Circle::deflection(const Configuration &q) const double HC_CC_Circle::D1(double alpha) const { double fresnel_s, fresnel_c; - double s = sqrt(2 * alpha / PI); + double s = sqrt(2 * alpha / STEERING_PI); fresnel(s, fresnel_s, fresnel_c); return cos(alpha) * fresnel_c + sin(alpha) * fresnel_s; } @@ -131,7 +131,7 @@ double HC_CC_Circle::rs_circular_deflection(double delta) const return delta; // irregular rs-turn else - return (delta <= PI) ? delta : delta - TWO_PI; + return (delta <= STEERING_PI) ? delta : delta - STEERING_TWO_PI; } double HC_CC_Circle::rs_turn_length(const Configuration &q) const @@ -149,7 +149,7 @@ double HC_CC_Circle::hc_circular_deflection(double delta) const if (this->regular) { if (delta < delta_min_twopified) - return TWO_PI + delta - delta_min_twopified; + return STEERING_TWO_PI + delta - delta_min_twopified; else return delta - delta_min_twopified; } @@ -160,12 +160,12 @@ double HC_CC_Circle::hc_circular_deflection(double delta) const if (delta < delta_min_twopified) { delta_arc1 = delta - delta_min_twopified; // negative - delta_arc2 = delta_arc1 + TWO_PI; // positive + delta_arc2 = delta_arc1 + STEERING_TWO_PI; // positive } else { delta_arc1 = delta - delta_min_twopified; // positive - delta_arc2 = delta_arc1 - TWO_PI; // negative + delta_arc2 = delta_arc1 - STEERING_TWO_PI; // negative } return (fabs(delta_arc1) < fabs(delta_arc2)) ? delta_arc1 : delta_arc2; } @@ -185,7 +185,7 @@ bool HC_CC_Circle::cc_elementary_sharpness(const Configuration &q, double delta, // Planning for Car-Like Vehicles," in IEEE/RSJ International Conference on Intelligent Robots and Systems, 1997. if (distance > get_epsilon()) { - sigma0 = 4 * PI * pow(this->D1(0.5 * delta), 2) / pow(distance, 2); + sigma0 = 4 * STEERING_PI * pow(this->D1(0.5 * delta), 2) / pow(distance, 2); if (!this->left) { sigma0 = -sigma0; @@ -202,7 +202,7 @@ double HC_CC_Circle::cc_circular_deflection(double delta) const if (this->regular) { if (delta < two_delta_min_twopified) - return TWO_PI + delta - two_delta_min_twopified; + return STEERING_TWO_PI + delta - two_delta_min_twopified; else return delta - two_delta_min_twopified; } @@ -213,12 +213,12 @@ double HC_CC_Circle::cc_circular_deflection(double delta) const if (delta < two_delta_min_twopified) { delta_arc1 = delta - two_delta_min_twopified; // negative - delta_arc2 = delta_arc1 + TWO_PI; // positive + delta_arc2 = delta_arc1 + STEERING_TWO_PI; // positive } else { delta_arc1 = delta - two_delta_min_twopified; // positive - delta_arc2 = delta_arc1 - TWO_PI; // negative + delta_arc2 = delta_arc1 - STEERING_TWO_PI; // negative } return (fabs(delta_arc1) < fabs(delta_arc2)) ? delta_arc1 : delta_arc2; } @@ -308,19 +308,19 @@ bool configuration_on_hc_cc_circle(const HC_CC_Circle &c, const Configuration &q double angle = atan2(q.y - c.yc, q.x - c.xc); if (c.left && c.forward) { - angle = angle + HALF_PI - c.mu; + angle = angle + STEERING_HALF_PI - c.mu; } if (c.left && !c.forward) { - angle = angle + HALF_PI + c.mu; + angle = angle + STEERING_HALF_PI + c.mu; } if (!c.left && c.forward) { - angle = angle - HALF_PI + c.mu; + angle = angle - STEERING_HALF_PI + c.mu; } if (!c.left && !c.forward) { - angle = angle - HALF_PI - c.mu; + angle = angle - STEERING_HALF_PI - c.mu; } angle = twopify(angle); return fabs(q.theta - angle) < get_epsilon(); diff --git a/src/hc_cc_state_space/hcpm0_reeds_shepp_state_space.cpp b/src/hc_cc_state_space/hcpm0_reeds_shepp_state_space.cpp index 112b164..a226634 100644 --- a/src/hc_cc_state_space/hcpm0_reeds_shepp_state_space.cpp +++ b/src/hc_cc_state_space/hcpm0_reeds_shepp_state_space.cpp @@ -66,22 +66,22 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI - c2.mu; + theta = angle + STEERING_HALF_PI - c2.mu; } else { - theta = angle + HALF_PI + c2.mu; + theta = angle + STEERING_HALF_PI + c2.mu; } } else { if (c1.forward) { - theta = angle - HALF_PI + c2.mu; + theta = angle - STEERING_HALF_PI + c2.mu; } else { - theta = angle - HALF_PI - c2.mu; + theta = angle - STEERING_HALF_PI - c2.mu; } } *q = new Configuration(x, y, theta, 0); @@ -122,12 +122,12 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } else { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } } @@ -135,12 +135,12 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { if (c1.forward) { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } else { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } } @@ -435,9 +435,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -451,9 +451,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -474,9 +474,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -488,9 +488,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -1144,9 +1144,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1160,9 +1160,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1190,9 +1190,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1206,9 +1206,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1290,9 +1290,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1306,9 +1306,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_); *cend = new HC_CC_Circle(c2); @@ -1337,9 +1337,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1353,9 +1353,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa); *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_); @@ -1420,9 +1420,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1436,9 +1436,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { @@ -1463,9 +1463,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp if (c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1477,9 +1477,9 @@ class HCpm0_Reeds_Shepp_State_Space::HCpm0_Reeds_Shepp if (!c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { diff --git a/src/hc_cc_state_space/hcpmpm_reeds_shepp_state_space.cpp b/src/hc_cc_state_space/hcpmpm_reeds_shepp_state_space.cpp index 12f37b4..004c9ba 100644 --- a/src/hc_cc_state_space/hcpmpm_reeds_shepp_state_space.cpp +++ b/src/hc_cc_state_space/hcpmpm_reeds_shepp_state_space.cpp @@ -66,22 +66,22 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI - parent_->mu_; + theta = angle + STEERING_HALF_PI - parent_->mu_; } else { - theta = angle + HALF_PI + parent_->mu_; + theta = angle + STEERING_HALF_PI + parent_->mu_; } } else { if (c1.forward) { - theta = angle - HALF_PI + parent_->mu_; + theta = angle - STEERING_HALF_PI + parent_->mu_; } else { - theta = angle - HALF_PI - parent_->mu_; + theta = angle - STEERING_HALF_PI - parent_->mu_; } } *q = new Configuration(x, y, theta, 0); @@ -123,12 +123,12 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { if (c1.forward) { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } else { - theta = angle + HALF_PI; + theta = angle + STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } } @@ -136,12 +136,12 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { if (c1.forward) { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, -delta_y, &x, &y); } else { - theta = angle - HALF_PI; + theta = angle - STEERING_HALF_PI; global_frame_change(c1.xc, c1.yc, angle, delta_x, delta_y, &x, &y); } } @@ -444,9 +444,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -460,9 +460,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -483,9 +483,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp if (c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && c1.forward) { @@ -497,9 +497,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp if (!c1.left && !c1.forward) { global_frame_change(c1.xc, c1.yc, theta, delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, 0); + *q1 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, -delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } } @@ -1172,9 +1172,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1188,9 +1188,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1219,9 +1219,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (c1.left && !c1.forward) { @@ -1235,9 +1235,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x1, -delta_y1, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, -delta_x2, -delta_y2, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); } if (!c1.left && !c1.forward) { @@ -1322,9 +1322,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1338,9 +1338,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa); *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_); @@ -1369,9 +1369,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && c1.forward) { @@ -1385,9 +1385,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, delta_x1, -delta_y1, &x, &y); - *q2 = new Configuration(x, y, theta + PI, 0); + *q2 = new Configuration(x, y, theta + STEERING_PI, 0); global_frame_change(c2.xc, c2.yc, theta, delta_x2, -delta_y2, &x, &y); - *q3 = new Configuration(x, y, theta + PI, c2.kappa); + *q3 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } *q1 = new Configuration(c1.start.x, c1.start.y, c1.start.theta, c1.kappa); *cstart = new HC_CC_Circle(**q2, c1.left, !c1.forward, HC_REGULAR, parent_->hc_cc_circle_param_); @@ -1452,9 +1452,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle - alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1468,9 +1468,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp { theta = angle + alpha; global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { @@ -1495,9 +1495,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp if (c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (c1.left && !c1.forward) { @@ -1509,9 +1509,9 @@ class HCpmpm_Reeds_Shepp_State_Space::HCpmpm_Reeds_Shepp if (!c1.left && c1.forward) { global_frame_change(c1.xc, c1.yc, theta, -delta_x, -delta_y, &x, &y); - *q1 = new Configuration(x, y, theta + PI, c1.kappa); + *q1 = new Configuration(x, y, theta + STEERING_PI, c1.kappa); global_frame_change(c2.xc, c2.yc, theta, delta_x, -delta_y, &x, &y); - *q2 = new Configuration(x, y, theta + PI, c2.kappa); + *q2 = new Configuration(x, y, theta + STEERING_PI, c2.kappa); } if (!c1.left && !c1.forward) { diff --git a/src/reeds_shepp_state_space/reeds_shepp_state_space.cpp b/src/reeds_shepp_state_space/reeds_shepp_state_space.cpp index c4c2083..24ebae4 100644 --- a/src/reeds_shepp_state_space/reeds_shepp_state_space.cpp +++ b/src/reeds_shepp_state_space/reeds_shepp_state_space.cpp @@ -69,7 +69,7 @@ inline void tauOmega(double u, double v, double xi, double eta, double phi, doub { double delta = pify(u - v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.; double t1 = atan2(eta * A - xi * B, xi * A + eta * B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3; - tau = (t2 < 0) ? pify(t1 + PI) : pify(t1); + tau = (t2 < 0) ? pify(t1 + STEERING_PI) : pify(t1); omega = pify(tau - u + v - phi); } @@ -159,7 +159,7 @@ inline bool LpRmL(double x, double y, double phi, double &t, double &u, double & if (u1 <= 4.) { u = -2. * asin(.25 * u1); - t = pify(theta + .5 * u + PI); + t = pify(theta + .5 * u + STEERING_PI); v = pify(phi - t + u); assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS); assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS); @@ -234,7 +234,7 @@ inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, dou if (rho >= 0 && rho <= 1) { u = -acos(rho); - if (u >= -.5 * PI) + if (u >= -.5 * STEERING_PI) { tauOmega(u, u, xi, eta, phi, t, v); assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS); @@ -297,10 +297,10 @@ inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, doubl double r = sqrt(rho * rho - 4.); u = 2. - r; t = pify(theta + atan2(r, -2.)); - v = pify(phi - .5 * PI - t); + v = pify(phi - .5 * STEERING_PI - t); assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS); assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS); - assert(fabs(pify(t + PI / 2 + v - phi)) < RS_EPS); + assert(fabs(pify(t + STEERING_PI / 2 + v - phi)) < RS_EPS); return t >= -RS_ZERO && u <= RS_ZERO && v <= RS_ZERO; } return false; @@ -314,38 +314,38 @@ inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, doubl { t = theta; u = 2. - rho; - v = pify(t + .5 * PI - phi); + v = pify(t + .5 * STEERING_PI - phi); assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS); assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS); - assert(fabs(pify(t + PI / 2 - v - phi)) < RS_EPS); + assert(fabs(pify(t + STEERING_PI / 2 - v - phi)) < RS_EPS); return t >= -RS_ZERO && u <= RS_ZERO && v <= RS_ZERO; } return false; } void CCSC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_Path &path) { - double t, u, v, Lmin = path.length() - .5 * PI, L; + double t, u, v, Lmin = path.length() - .5 * STEERING_PI, L; if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) { path = - Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[4], t, -.5 * PI, u, v); + Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[4], t, -.5 * STEERING_PI, u, v); Lmin = L; } if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[4], -t, .5 * PI, -u, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[4], -t, .5 * STEERING_PI, -u, -v); Lmin = L; } if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect { path = - Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[5], t, -.5 * PI, u, v); + Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[5], t, -.5 * STEERING_PI, u, v); Lmin = L; } if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[5], -t, .5 * PI, -u, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[5], -t, .5 * STEERING_PI, -u, -v); Lmin = L; } @@ -353,24 +353,24 @@ void CCSC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_P if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) { path = - Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[8], t, -.5 * PI, u, v); + Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[8], t, -.5 * STEERING_PI, u, v); Lmin = L; } if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[8], -t, .5 * PI, -u, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[8], -t, .5 * STEERING_PI, -u, -v); Lmin = L; } if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect { path = - Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[9], t, -.5 * PI, u, v); + Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[9], t, -.5 * STEERING_PI, u, v); Lmin = L; } if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[9], -t, .5 * PI, -u, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[9], -t, .5 * STEERING_PI, -u, -v); Lmin = L; } @@ -380,49 +380,49 @@ void CCSC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_P if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) { path = - Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[6], v, u, -.5 * PI, t); + Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[6], v, u, -.5 * STEERING_PI, t); Lmin = L; } if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[6], -v, -u, .5 * PI, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[6], -v, -u, .5 * STEERING_PI, -t); Lmin = L; } if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect { path = - Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[7], v, u, -.5 * PI, t); + Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[7], v, u, -.5 * STEERING_PI, t); Lmin = L; } if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[7], -v, -u, .5 * PI, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[7], -v, -u, .5 * STEERING_PI, -t); Lmin = L; } if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[10], v, u, -.5 * PI, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[10], v, u, -.5 * STEERING_PI, t); Lmin = L; } if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip { path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[10], -v, -u, - .5 * PI, -t); + .5 * STEERING_PI, -t); Lmin = L; } if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[11], v, u, -.5 * PI, + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[11], v, u, -.5 * STEERING_PI, t); Lmin = L; } if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[11], -v, -u, - .5 * PI, -t); + .5 * STEERING_PI, -t); } // formula 8.11 *** TYPO IN PAPER *** inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v) @@ -446,28 +446,28 @@ inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, doub } void CCSCC(double x, double y, double phi, Reeds_Shepp_State_Space::Reeds_Shepp_Path &path) { - double t, u, v, Lmin = path.length() - PI, L; + double t, u, v, Lmin = path.length() - STEERING_PI, L; if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[16], t, -.5 * PI, u, - -.5 * PI, v); + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[16], t, -.5 * STEERING_PI, u, + -.5 * STEERING_PI, v); Lmin = L; } if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[16], -t, .5 * PI, - -u, .5 * PI, -v); + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[16], -t, .5 * STEERING_PI, + -u, .5 * STEERING_PI, -v); Lmin = L; } if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect { - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[17], t, -.5 * PI, u, - -.5 * PI, v); + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[17], t, -.5 * STEERING_PI, u, + -.5 * STEERING_PI, v); Lmin = L; } if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect - path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[17], -t, .5 * PI, - -u, .5 * PI, -v); + path = Reeds_Shepp_State_Space::Reeds_Shepp_Path(Reeds_Shepp_State_Space::reeds_shepp_path_type[17], -t, .5 * STEERING_PI, + -u, .5 * STEERING_PI, -v); } Reeds_Shepp_State_Space::Reeds_Shepp_Path reeds_shepp(double x, double y, double phi) diff --git a/src/utilities/utilities.cpp b/src/utilities/utilities.cpp index 5af4f7f..a423e21 100755 --- a/src/utilities/utilities.cpp +++ b/src/utilities/utilities.cpp @@ -48,16 +48,16 @@ void polar(double x, double y, double &r, double &theta) double twopify(double alpha) { - return alpha - TWO_PI * floor(alpha / TWO_PI); + return alpha - STEERING_TWO_PI * floor(alpha / STEERING_TWO_PI); } double pify(double alpha) { - double v = fmod(alpha, TWO_PI); - if (v < -PI) - v += TWO_PI; - else if (v > PI) - v -= TWO_PI; + double v = fmod(alpha, STEERING_TWO_PI); + if (v < -STEERING_PI) + v += STEERING_TWO_PI; + else if (v > STEERING_PI) + v -= STEERING_TWO_PI; return v; } @@ -90,8 +90,8 @@ void fresnel_0_8(double x, double &S_f, double &C_f) A += chebev_a[17] * T34; double sqrt_x = sqrt(x); - C_f = SQRT_TWO_PI_INV * sqrt_x * A; - S_f = SQRT_TWO_PI_INV * sqrt_x * B; + C_f = STEERING_SQRT_TWO_PI_INV * sqrt_x * A; + S_f = STEERING_SQRT_TWO_PI_INV * sqrt_x * B; return; } @@ -124,14 +124,14 @@ void fresnel_8_inf(double x, double &S_f, double &C_f) double sin_x = sin(x); double cos_x = cos(x); double sqrt_x = sqrt(x); - C_f = 0.5 - SQRT_TWO_PI_INV * (E * cos_x / (2 * x) - F * sin_x) / sqrt_x; - S_f = 0.5 - SQRT_TWO_PI_INV * (E * sin_x / (2 * x) + F * cos_x) / sqrt_x; + C_f = 0.5 - STEERING_SQRT_TWO_PI_INV * (E * cos_x / (2 * x) - F * sin_x) / sqrt_x; + S_f = 0.5 - STEERING_SQRT_TWO_PI_INV * (E * sin_x / (2 * x) + F * cos_x) / sqrt_x; return; } void fresnel(double s, double &S_f, double &C_f) { - double x = HALF_PI * s * s; + double x = STEERING_HALF_PI * s * s; if (x <= 8.0) fresnel_0_8(x, S_f, C_f); else @@ -153,8 +153,8 @@ void end_of_clothoid(double x_i, double y_i, double theta_i, double kappa_i, dou double abs_sigma = fabs(sigma); double sqrt_sigma_inv = 1 / sqrt(abs_sigma); double k1 = theta_i - 0.5 * direction * kappa_i * kappa_i / sigma; - double k2 = SQRT_PI_INV * sqrt_sigma_inv * (abs_sigma * length + sgn_sigma * kappa_i); - double k3 = SQRT_PI_INV * sqrt_sigma_inv * sgn_sigma * kappa_i; + double k2 = STEERING_SQRT_PI_INV * sqrt_sigma_inv * (abs_sigma * length + sgn_sigma * kappa_i); + double k3 = STEERING_SQRT_PI_INV * sqrt_sigma_inv * sgn_sigma * kappa_i; double cos_k1 = cos(k1); double sin_k1 = sin(k1); double fresnel_s_k2; @@ -164,10 +164,10 @@ void end_of_clothoid(double x_i, double y_i, double theta_i, double kappa_i, dou fresnel(k2, fresnel_s_k2, fresnel_c_k2); fresnel(k3, fresnel_s_k3, fresnel_c_k3); *x_f = x_i + - SQRT_PI * sqrt_sigma_inv * + STEERING_SQRT_PI* sqrt_sigma_inv * (direction * cos_k1 * (fresnel_c_k2 - fresnel_c_k3) - sgn_sigma * sin_k1 * (fresnel_s_k2 - fresnel_s_k3)); *y_f = y_i + - SQRT_PI * sqrt_sigma_inv * + STEERING_SQRT_PI* sqrt_sigma_inv * (direction * sin_k1 * (fresnel_c_k2 - fresnel_c_k3) + sgn_sigma * cos_k1 * (fresnel_s_k2 - fresnel_s_k3)); // theta_f = theta_i + kappa_i*length + 0.5*sigma*length^2