Skip to content

Commit

Permalink
fix underscore_names for methods
Browse files Browse the repository at this point in the history
  • Loading branch information
@pantor
pantor committed Sep 30, 2023
1 parent 38aa11f commit 386e56b
Show file tree
Hide file tree
Showing 4 changed files with 57 additions and 57 deletions.
2 changes: 1 addition & 1 deletion include/ruckig/profile.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -488,7 +488,7 @@ class Profile {
continue;
}

for (const double _t: roots::solveCub(j[i]/6, a[i]/2, v[i], p[i]-pt)) {
for (const double _t: roots::solve_cubic(j[i]/6, a[i]/2, v[i], p[i]-pt)) {
if (0 < _t && _t <= t[i]) {
time = offset + _t + ((i > 0) ? t_sum[i-1] : 0.0);
std::tie(std::ignore, vt, at) = integrate(_t, p[i], v[i], a[i], j[i]);
Expand Down
34 changes: 17 additions & 17 deletions include/ruckig/roots.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ class PositiveSet: public Set<T, N> {


//! Calculate all roots of a*x^3 + b*x^2 + c*x + d = 0
inline PositiveSet<double, 3> solveCub(double a, double b, double c, double d) {
inline PositiveSet<double, 3> solve_cubic(double a, double b, double c, double d) {
PositiveSet<double, 3> roots;

if (std::abs(d) < DBL_EPSILON) {
Expand Down Expand Up @@ -151,7 +151,7 @@ inline PositiveSet<double, 3> solveCub(double a, double b, double c, double d) {
// Solve resolvent eqaution of corresponding Quartic equation
// The input x must be of length 3
// Number of zeros are returned
inline int solveResolvent(std::array<double, 3>& x, double a, double b, double c) {
inline int solve_resolvent(std::array<double, 3>& x, double a, double b, double c) {
constexpr double cos120 = -0.50;
constexpr double sin120 = 0.866025403784438646764;

Expand Down Expand Up @@ -195,7 +195,7 @@ inline int solveResolvent(std::array<double, 3>& x, double a, double b, double c
}

//! Calculate all roots of the monic quartic equation: x^4 + a*x^3 + b*x^2 + c*x + d = 0
inline PositiveSet<double, 4> solveQuartMonic(double a, double b, double c, double d) {
inline PositiveSet<double, 4> solve_quart_monic(double a, double b, double c, double d) {
PositiveSet<double, 4> roots;

if (std::abs(d) < DBL_EPSILON) {
Expand Down Expand Up @@ -225,7 +225,7 @@ inline PositiveSet<double, 4> solveQuartMonic(double a, double b, double c, doub
const double c3 = -a * a * d - c * c + 4 * b * d;

std::array<double, 3> x3;
const int number_zeroes = solveResolvent(x3, a3, b3, c3);
const int number_zeroes = solve_resolvent(x3, a3, b3, c3);

double y = x3[0];
// Choosing Y with maximal absolute value.
Expand Down Expand Up @@ -283,14 +283,14 @@ inline PositiveSet<double, 4> solveQuartMonic(double a, double b, double c, doub
}

//! Calculate the quartic equation: x^4 + b*x^3 + c*x^2 + d*x + e = 0
inline PositiveSet<double, 4> solveQuartMonic(const std::array<double, 4>& polynom) {
return solveQuartMonic(polynom[0], polynom[1], polynom[2], polynom[3]);
inline PositiveSet<double, 4> solve_quart_monic(const std::array<double, 4>& polynom) {
return solve_quart_monic(polynom[0], polynom[1], polynom[2], polynom[3]);
}


//! Evaluate a polynomial of order N at x
template<size_t N>
inline double polyEval(const std::array<double, N>& p, double x) {
inline double poly_eval(const std::array<double, N>& p, double x) {
double retVal = 0.0;
if constexpr (N == 0) {
return retVal;
Expand All @@ -316,7 +316,7 @@ inline double polyEval(const std::array<double, N>& p, double x) {

// Calculate the derivative poly coefficients of a given poly
template<size_t N>
inline std::array<double, N-1> polyDeri(const std::array<double, N>& coeffs) {
inline std::array<double, N-1> poly_derivative(const std::array<double, N>& coeffs) {
std::array<double, N-1> deriv;
for (size_t i = 0; i < N - 1; ++i) {
deriv[i] = (N - 1 - i) * coeffs[i];
Expand All @@ -325,7 +325,7 @@ inline std::array<double, N-1> polyDeri(const std::array<double, N>& coeffs) {
}

template<size_t N>
inline std::array<double, N-1> polyMonicDeri(const std::array<double, N>& monic_coeffs) {
inline std::array<double, N-1> poly_monic_derivative(const std::array<double, N>& monic_coeffs) {
std::array<double, N-1> deriv;
deriv[0] = 1.0;
for (size_t i = 1; i < N - 1; ++i) {
Expand All @@ -340,9 +340,9 @@ constexpr double tolerance {1e-14};
// Calculate a single zero of polynom p(x) inside [lbound, ubound]
// Requirements: p(lbound)*p(ubound) < 0, lbound < ubound
template<size_t N, size_t maxIts = 128>
inline double shrinkInterval(const std::array<double, N>& p, double l, double h) {
const double fl = polyEval(p, l);
const double fh = polyEval(p, h);
inline double shrink_interval(const std::array<double, N>& p, double l, double h) {
const double fl = poly_eval(p, l);
const double fh = poly_eval(p, h);
if (fl == 0.0) {
return l;
}
Expand All @@ -356,9 +356,9 @@ inline double shrinkInterval(const std::array<double, N>& p, double l, double h)
double rts = (l + h) / 2;
double dxold = std::abs(h - l);
double dx = dxold;
const auto deriv = polyDeri(p);
double f = polyEval(p, rts);
double df = polyEval(deriv, rts);
const auto deriv = poly_derivative(p);
double f = poly_eval(p, rts);
double df = poly_eval(deriv, rts);
double temp;
for (size_t j = 0; j < maxIts; j++) {
if ((((rts - h) * df - f) * ((rts - l) * df - f) > 0.0) || (std::abs(2 * f) > std::abs(dxold * df))) {
Expand All @@ -382,8 +382,8 @@ inline double shrinkInterval(const std::array<double, N>& p, double l, double h)
break;
}

f = polyEval(p, rts);
df = polyEval(deriv, rts);
f = poly_eval(p, rts);
df = poly_eval(deriv, rts);
if (f < 0.0) {
l = rts;
} else {
Expand Down
6 changes: 3 additions & 3 deletions src/ruckig/position_third_step1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,9 +191,9 @@ void PositionThirdOrderStep1::time_all_none_acc0_acc1(ProfileIter& profile, doub
const bool polynom_acc0_has_solution = (polynom_acc0_min[0] < 0.0) || (polynom_acc0_min[1] < 0.0) || (polynom_acc0_min[2] < 0.0) || (polynom_acc0_min[3] <= 0.0);
const bool polynom_acc1_has_solution = (polynom_acc1[0] < 0.0) || (polynom_acc1[1] < 0.0) || (polynom_acc1[2] < 0.0) || (polynom_acc1[3] <= 0.0);

auto roots_none = roots::solveQuartMonic(polynom_none);
auto roots_acc0 = polynom_acc0_has_solution ? roots::solveQuartMonic(polynom_acc0) : roots::PositiveSet<double, 4>{};
auto roots_acc1 = polynom_acc1_has_solution ? roots::solveQuartMonic(polynom_acc1) : roots::PositiveSet<double, 4>{};
auto roots_none = roots::solve_quart_monic(polynom_none);
auto roots_acc0 = polynom_acc0_has_solution ? roots::solve_quart_monic(polynom_acc0) : roots::PositiveSet<double, 4>{};
auto roots_acc1 = polynom_acc1_has_solution ? roots::solve_quart_monic(polynom_acc1) : roots::PositiveSet<double, 4>{};


for (double t: roots_none) {
Expand Down
72 changes: 36 additions & 36 deletions src/ruckig/position_third_step2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ bool PositionThirdOrderStep2::time_acc1_vel(Profile& profile, double vMax, doubl
const double t_min = -a0/jMax;
const double t_max = std::min((tf + 2*aMin/jMax - (a0 + af)/jMax)/2, (aMax - a0)/jMax);

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t < t_min || t > t_max) {
continue;
Expand Down Expand Up @@ -137,7 +137,7 @@ bool PositionThirdOrderStep2::time_acc1_vel(Profile& profile, double vMax, doubl
const double t_min = -a0/jMax;
const double t_max = std::min((tf + ad/jMax - 2*aMax/jMax)/2, (aMax - a0)/jMax);

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t > t_max || t < t_min) {
continue;
Expand Down Expand Up @@ -180,7 +180,7 @@ bool PositionThirdOrderStep2::time_acc0_vel(Profile& profile, double vMax, doubl
const double t_min = -af/jMax;
const double t_max = std::min(tf - (2*aMax - a0)/jMax, -aMin/jMax);

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t < t_min || t > t_max) {
continue;
Expand Down Expand Up @@ -222,7 +222,7 @@ bool PositionThirdOrderStep2::time_acc0_vel(Profile& profile, double vMax, doubl
const double t_min = af/jMax;
const double t_max = std::min(tf - aMax/jMax, aMax/jMax);

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t < t_min || t > t_max) {
continue;
Expand Down Expand Up @@ -268,7 +268,7 @@ bool PositionThirdOrderStep2::time_vel(Profile& profile, double vMax, double vMi
polynom[2] = 0;
polynom[3] = pd/(2*jMax);

auto roots = roots::solveCub(polynom[0], polynom[1], polynom[2], polynom[3]);
auto roots = roots::solve_cubic(polynom[0], polynom[1], polynom[2], polynom[3]);
for (double t: roots) {
if (t > tf/4) {
continue;
Expand Down Expand Up @@ -310,11 +310,11 @@ bool PositionThirdOrderStep2::time_vel(Profile& profile, double vMax, double vMi
polynom[4] = (a0*(7*a0_p4 - 10*a0_a0*ph3 - 4*a0*ph1 + 6*a0_a0*p1 + ph2))/(12*jMax_jMax*jMax*ph4);
polynom[5] = (7*a0_p6 + af_p6 - 12*a0_p4*ph3 + 48*af_p3*jMax_jMax*g1 - 8*a0_p3*ph1 - 72*jMax_jMax*jMax*(jMax*g1*g1 + vd_vd*vd + 2*af*g1*vd) - 6*af_p4*jMax*vd + 36*af_af*jMax_jMax*vd_vd + 9*a0_p4*p1 + 3*a0_a0*ph2)/(144*jMax_jMax*jMax_jMax*ph4);

auto deriv = roots::polyMonicDeri(polynom);
auto dderiv = roots::polyDeri(deriv);
auto deriv = roots::poly_monic_derivative(polynom);
auto dderiv = roots::poly_derivative(deriv);

// Solve 4th order derivative analytically
auto d_extremas = roots::solveQuartMonic(deriv[1], deriv[2], deriv[3], deriv[4]);
auto d_extremas = roots::solve_quart_monic(deriv[1], deriv[2], deriv[3], deriv[4]);

double tz_current {tz_min};

Expand Down Expand Up @@ -351,26 +351,26 @@ bool PositionThirdOrderStep2::time_vel(Profile& profile, double vMax, double vMi
continue;
}

const double orig = roots::polyEval(deriv, tz);
const double orig = roots::poly_eval(deriv, tz);
if (std::abs(orig) > roots::tolerance) {
tz -= orig / roots::polyEval(dderiv, tz);
tz -= orig / roots::poly_eval(dderiv, tz);
}

const double val_new = roots::polyEval(polynom, tz);
if (std::abs(val_new) < 64 * std::abs(roots::polyEval(dderiv, tz)) * roots::tolerance) {
const double val_new = roots::poly_eval(polynom, tz);
if (std::abs(val_new) < 64 * std::abs(roots::poly_eval(dderiv, tz)) * roots::tolerance) {
if (check_root(tz)) {
return true;
}
} else if (roots::polyEval(polynom, tz_current) * val_new < 0) {
if (check_root(roots::shrinkInterval(polynom, tz_current, tz))) {
} else if (roots::poly_eval(polynom, tz_current) * val_new < 0) {
if (check_root(roots::shrink_interval(polynom, tz_current, tz))) {
return true;
}
}
tz_current = tz;
}
const double val_max = roots::polyEval(polynom, tz_max);
if (roots::polyEval(polynom, tz_current) * val_max < 0) {
if (check_root(roots::shrinkInterval(polynom, tz_current, tz_max))) {
const double val_max = roots::poly_eval(polynom, tz_max);
if (roots::poly_eval(polynom, tz_current) * val_max < 0) {
if (check_root(roots::shrink_interval(polynom, tz_current, tz_max))) {
return true;
}
} else if (std::abs(val_max) < 8 * DBL_EPSILON) {
Expand Down Expand Up @@ -398,29 +398,29 @@ bool PositionThirdOrderStep2::time_vel(Profile& profile, double vMax, double vMi
polynom[5] = (a0*(11*a0_p4 + ph4 - 10*a0_p3*ph5 - 6*a0_a0*ph1 + 4*a0*ph2))/(12*jMax_jMax*jMax_jMax*jMax);
polynom[6] = (11*a0_p6 - af_p6 - 12*a0_p5*ph5 - 48*af_p3*jMax_jMax*g1 - 9*a0_p4*ph1 + 72*jMax_jMax*jMax*(jMax*g1*g1 - vd_vd*vd - 2*af*g1*vd) - 6*af_p4*jMax*vd - 36*af_af*jMax_jMax*vd_vd + 8*a0_p3*ph2 + 3*a0_a0*ph4)/(144*jMax_jMax*jMax_jMax*jMax_jMax);

std::array<double, 6> deriv = roots::polyMonicDeri(polynom);
std::array<double, 5> dderiv = roots::polyMonicDeri(deriv);
std::array<double, 6> deriv = roots::poly_monic_derivative(polynom);
std::array<double, 5> dderiv = roots::poly_monic_derivative(deriv);

double dd_tz_current {tz_min};
roots::Set<std::pair<double, double>, 6> dd_tz_intervals;

auto dd_extremas = roots::solveQuartMonic(dderiv[1], dderiv[2], dderiv[3], dderiv[4]);
auto dd_extremas = roots::solve_quart_monic(dderiv[1], dderiv[2], dderiv[3], dderiv[4]);
for (double tz: dd_extremas) {
if (tz >= tz_max) {
continue;
}

const double orig = roots::polyEval(dderiv, tz);
const double orig = roots::poly_eval(dderiv, tz);
if (std::abs(orig) > roots::tolerance) {
tz -= orig / roots::polyEval(roots::polyDeri(dderiv), tz);
tz -= orig / roots::poly_eval(roots::poly_derivative(dderiv), tz);
}

if (roots::polyEval(deriv, dd_tz_current) * roots::polyEval(deriv, tz) < 0) {
if (roots::poly_eval(deriv, dd_tz_current) * roots::poly_eval(deriv, tz) < 0) {
dd_tz_intervals.insert({dd_tz_current, tz});
}
dd_tz_current = tz;
}
if (roots::polyEval(deriv, dd_tz_current) * roots::polyEval(deriv, tz_max) < 0) {
if (roots::poly_eval(deriv, dd_tz_current) * roots::poly_eval(deriv, tz_max) < 0) {
dd_tz_intervals.insert({dd_tz_current, tz_max});
}

Expand Down Expand Up @@ -458,27 +458,27 @@ bool PositionThirdOrderStep2::time_vel(Profile& profile, double vMax, double vMi
};

for (auto interval: dd_tz_intervals) {
const double tz = roots::shrinkInterval(deriv, interval.first, interval.second);
const double tz = roots::shrink_interval(deriv, interval.first, interval.second);

if (tz >= tz_max) {
continue;
}

const double p_val = roots::polyEval(polynom, tz);
if (std::abs(p_val) < 64 * std::abs(roots::polyEval(dderiv, tz)) * roots::tolerance) {
const double p_val = roots::poly_eval(polynom, tz);
if (std::abs(p_val) < 64 * std::abs(roots::poly_eval(dderiv, tz)) * roots::tolerance) {
if (check_root(tz)) {
return true;
}

} else if (roots::polyEval(polynom, tz_current) * p_val < 0) {
if (check_root(roots::shrinkInterval(polynom, tz_current, tz))) {
} else if (roots::poly_eval(polynom, tz_current) * p_val < 0) {
if (check_root(roots::shrink_interval(polynom, tz_current, tz))) {
return true;
}
}
tz_current = tz;
}
if (roots::polyEval(polynom, tz_current) * roots::polyEval(polynom, tz_max) < 0) {
if (check_root(roots::shrinkInterval(polynom, tz_current, tz_max))) {
if (roots::poly_eval(polynom, tz_current) * roots::poly_eval(polynom, tz_max) < 0) {
if (check_root(roots::shrink_interval(polynom, tz_current, tz_max))) {
return true;
}
}
Expand Down Expand Up @@ -735,7 +735,7 @@ bool PositionThirdOrderStep2::time_none(Profile& profile, double vMax, double vM
polynom[2] = 4*(pd - tf*vf)/jMax;
polynom[3] = (vd_vd + jMax*tf*g2)/(jMax_jMax);

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t > tf/2 || t > (aMax - a0)/jMax) {
continue;
Expand Down Expand Up @@ -802,7 +802,7 @@ bool PositionThirdOrderStep2::time_none(Profile& profile, double vMax, double vM
const double t_min = ad/jMax;
const double t_max = std::min((aMax - a0)/jMax, (ad/jMax + tf) / 2);

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t < t_min || t > t_max) {
continue;
Expand Down Expand Up @@ -869,7 +869,7 @@ bool PositionThirdOrderStep2::time_none(Profile& profile, double vMax, double vM

const double t_max = (a0 - aMin)/jMax;

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t > t_max) {
continue;
Expand Down Expand Up @@ -923,7 +923,7 @@ bool PositionThirdOrderStep2::time_none(Profile& profile, double vMax, double vM
polynom[2] = (-a0_p5 + af_p5 - af_p4*jMax*tf + 5*a0_p4*(af - jMax*tf) - 2*a0_p3*ph3 - 4*af_p3*jMax*(jMax*tf_tf + vd) + 12*af_af*jMax_jMax*g2 - 12*af*jMax_jMax*ph6 + 2*a0_a0*(5*af_p3 - 9*af_af*jMax*tf - 6*af*jMax*vd + 6*jMax_jMax*ph0) + 12*jMax_jMax*jMax*ph2 + a0*(-5*af_p4 + 8*af_p3*jMax*tf + 12*af_af*jMax*(jMax*tf_tf + vd) - 24*af*jMax_jMax*(-2*pd + jMax*tf_p3 + 2*tf*vf) + 6*jMax_jMax*ph4))/(jMax*ph7);
polynom[3] = -(a0_p6 + af_p6 - 6*a0_p5*(af - jMax*tf) + 48*af_p3*jMax_jMax*g1 - 72*jMax_jMax*jMax*(jMax*g1*g1 + vd_vd*vd + 2*af*g1*vd) + 3*a0_p4*ph3 - 6*af_p4*jMax*vd + 36*af_af*jMax_jMax*vd_vd - 4*a0_p3*(5*af_p3 - 9*af_af*jMax*tf - 6*af*jMax*vd + 6*jMax_jMax*ph0) + 3*a0_a0*ph5 - 6*a0*(af_p5 - af_p4*jMax*tf - 4*af_p3*jMax*(jMax*tf_tf + vd) + 12*jMax_jMax*(af_af*g2 - af*ph6 + jMax*ph2)))/(6*jMax_jMax*ph7);

auto roots = roots::solveQuartMonic(polynom);
auto roots = roots::solve_quart_monic(polynom);
for (double t: roots) {
if (t > tf || t > (aMax - a0)/jMax) {
continue;
Expand Down Expand Up @@ -971,7 +971,7 @@ bool PositionThirdOrderStep2::time_none(Profile& profile, double vMax, double vM
polynom[2] = (a0_a0 + af_af + 10*a0*af)*tf_tf + 24*(tf*(af*v0 - a0*vf) - pd*ad) + 12*vd_vd;
polynom[3] = -3*tf*((a0_a0 + af_af + 2*a0*af)*tf_tf - 4*vd*(a0 + af)*tf + 4*vd_vd);

auto roots = roots::solveCub(polynom[0], polynom[1], polynom[2], polynom[3]);
auto roots = roots::solve_cubic(polynom[0], polynom[1], polynom[2], polynom[3]);
for (double t: roots) {
if (t > tf) {
continue;
Expand Down

0 comments on commit 386e56b

Please sign in to comment.