151 Scalar max_step_size) {
153 VectorXs &coeffs = interpolant.coeffs;
155 assert(samples.size() >= 2);
156 const FunctionSample &lower_bound = samples[0];
157 const Scalar &phi0 = lower_bound.phi;
158 const Scalar &dphi0 = lower_bound.dphi;
160 if (samples.size() == 2) {
161 strat = LSInterpolation::QUADRATIC;
165 case LSInterpolation::QUADRATIC: {
166 assert(samples.size() >= 2);
167 const FunctionSample &cand0 = samples[1];
168 Scalar a = (cand0.phi - phi0 - cand0.alpha * dphi0) /
169 (cand0.alpha * cand0.alpha);
170 coeffs.conservativeResize(3);
171 coeffs << a, dphi0, phi0;
172 assert(interpolant.degree() == 2);
173 anext = -dphi0 / (2. * a);
176 case LSInterpolation::CUBIC: {
177 assert(samples.size() >= 3);
178 const FunctionSample &cand0 = samples[1];
179 const FunctionSample &cand1 = samples[2];
180 const Scalar &a0 = cand0.alpha;
181 const Scalar &a1 = cand1.alpha;
183 Vector2s coeffs_cubic_interpolant;
185 alph_mat(0, 0) = a0 * a0 * a0;
186 alph_mat(0, 1) = a0 * a0;
187 alph_mat(1, 0) = a1 * a1 * a1;
188 alph_mat(1, 1) = a1 * a1;
190 Vector2s alph_rhs{cand1.phi - phi0 - dphi0 * a1,
191 cand0.phi - phi0 - dphi0 * a0};
193 Eigen::HouseholderQR<Matrix2s> decomp(alph_mat);
194 coeffs_cubic_interpolant = decomp.solve(alph_rhs);
196 const Scalar c3 = coeffs_cubic_interpolant(0);
197 const Scalar c2 = coeffs_cubic_interpolant(1);
198 coeffs.conservativeResize(4);
199 coeffs << c3, c2, dphi0, phi0;
200 assert(interpolant.degree() == 3);
203 anext = (-c2 + std::sqrt(c2 * c2 - 3.0 * c3 * dphi0)) / (3.0 * c3);
210 if ((anext > max_step_size) || (anext < min_step_size)) {
212 Scalar pleft = interpolant.evaluate(min_step_size);
213 Scalar pright = interpolant.evaluate(max_step_size);
214 if (pleft < pright) {
215 anext = min_step_size;
217 anext = max_step_size;