63 Vec<CT>
HermiteInterpolateAndSolve(CT
const& target_time,
const unsigned int num_sample_points,
const TimeCont<CT> & times,
const SampCont<CT> & samples,
const SampCont<CT> & derivatives, ContStart shift_from = ContStart::Back)
65 assert((times.size() >= num_sample_points) &&
"must have sufficient number of sample times");
66 assert((samples.size() >= num_sample_points) &&
"must have sufficient number of sample points");
67 assert((derivatives.size() >= num_sample_points) &&
"must have sufficient number of derivatives");
70 unsigned num_t, num_s, num_d;
71 if (shift_from == ContStart::Back)
73 num_t = times.size()-1;
74 num_s = samples.size()-1;
75 num_d = derivatives.size()-1;
79 num_t = num_s = num_d = num_sample_points-1;
82 Mat< Vec<CT> > space_differences(2*num_sample_points,2*num_sample_points);
83 Vec<CT> time_differences(2*num_sample_points);
86 for(
unsigned int ii=0; ii<num_sample_points; ++ii)
88 space_differences(2*ii,0) = samples[ num_s-ii];
89 space_differences(2*ii+1,0) = samples[ num_s-ii];
90 space_differences(2*ii+1,1) = derivatives[num_d-ii];
91 time_differences(2*ii) = times[ num_t-ii];
92 time_differences(2*ii+1) = times[ num_t-ii];
96 for(
unsigned int ii=1; ii< num_sample_points; ++ii)
98 space_differences(2*ii,1) = (space_differences(2*ii,0) - space_differences(2*ii-1,0)) / (time_differences(2*ii) - time_differences(2*ii-1));
103 for(
unsigned int ii=2; ii < 2*num_sample_points; ++ii)
105 for(
unsigned int jj=2; jj <=ii; ++jj)
107 space_differences(ii,jj) =
108 (space_differences(ii,jj-1) - space_differences(ii-1,jj-1))
110 (time_differences(ii) - time_differences(ii-jj));
116 Vec<CT> Result = space_differences(2*num_sample_points - 1,2*num_sample_points - 1);
121 for (
unsigned ii=num_sample_points-1; ii >= 1; --ii)
123 Result = ((Result*(target_time - time_differences(ii)) + space_differences(2*ii, 2*ii)) * (target_time - time_differences(ii-1)) + space_differences(2*ii-1, 2*ii-1)).eval();
127 return (Result * (target_time - time_differences(0)) + space_differences(0,0)).eval();
Vec< CT > HermiteInterpolateAndSolve(CT const &target_time, const unsigned int num_sample_points, const TimeCont< CT > ×, const SampCont< CT > &samples, const SampCont< CT > &derivatives, ContStart shift_from=ContStart::Back)
Estimates the root to interpolating polynomial.
Definition interpolation.hpp:63