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