Fixes unit conversion for polynomial coefficients

This commit is contained in:
Richard Brice
2024-12-02 14:44:46 -08:00
parent f425794182
commit eadf35f031
+7 -5
View File
@@ -766,15 +766,16 @@ class curve_segment_evaluator {
// Distance along the curve is Integral[0,x] (sqrt(f'(x)^2 + 1) dx // Distance along the curve is Integral[0,x] (sqrt(f'(x)^2 + 1) dx
// This functor is the derivative of y(x) => dy/dx = f'(x) // This functor is the derivative of y(x) => dy/dx = f'(x)
auto df = [coeffY](double x) -> double { auto df = [lu=length_unit_,coeffY](double x) -> double {
auto begin = std::next(coeffY.begin()); auto begin = std::next(coeffY.begin());
auto iter = begin; auto iter = begin;
auto end = coeffY.end(); auto end = coeffY.end();
double value = 0; double value = 0;
double lu_exp = 0.0;
for (; iter != end; iter++) { for (; iter != end; iter++) {
auto exp = std::distance(begin, iter); auto exp = std::distance(begin, iter);
auto coeff = (*iter); auto coeff = (*iter);
value += (double)exp * coeff * pow(x, exp); value += (double)exp * coeff * pow(lu, lu_exp--) * pow(x, exp - 1);
} }
return value; return value;
}; };
@@ -814,7 +815,7 @@ class curve_segment_evaluator {
} }
// This functor evaluates the polynomial at a distance u along the curve // This functor evaluates the polynomial at a distance u along the curve
parent_curve_fn_ = [start = start_, coeffX, coeffY, convert_u](double u) -> Eigen::Matrix4d { parent_curve_fn_ = [start = start_, lu = length_unit_, coeffX, coeffY, convert_u](double u) -> Eigen::Matrix4d {
auto x = convert_u(u + start); // find x for u auto x = convert_u(u + start); // find x for u
// evaluate the polynomial at x // evaluate the polynomial at x
std::array<const std::vector<double>*, 2> coefficients{&coeffX, &coeffY}; std::array<const std::vector<double>*, 2> coefficients{&coeffX, &coeffY};
@@ -823,13 +824,14 @@ class curve_segment_evaluator {
for (int i = 0; i < 2; i++) { // loop over X and Y for (int i = 0; i < 2; i++) { // loop over X and Y
auto begin = coefficients[i]->cbegin(); auto begin = coefficients[i]->cbegin();
auto end = coefficients[i]->cend(); auto end = coefficients[i]->cend();
double lu_exp = 1.0;
for (auto iter = begin; iter != end; iter++) { for (auto iter = begin; iter != end; iter++) {
auto exp = std::distance(begin, iter); auto exp = std::distance(begin, iter);
auto coeff = (*iter); auto coeff = (*iter);
position[i] += coeff * pow(x, exp); position[i] += coeff * pow(lu, lu_exp--) * pow(x, exp);
if (iter != begin) { if (iter != begin) {
slope[i] += coeff * exp * pow(x, exp - 1); slope[i] += coeff * pow(lu, lu_exp) * exp * pow(x, exp - 1);
} }
} }
} }