Method position

plugins/trajectory/filters/SplineFit.cpp:14–28

template <int N>
Eigen::Matrix<double, N, 1> SplineFit<N>::position(double t) const
{
    auto tcomb = tconvert(t);
    int i = tcomb.first;
    double tf = tcomb.second;

    assert(i < num);

    datum p;
    for (int j = 0; j < N; ++j)
        p(j) = SplineFitScalar::EndPointCubic(r[i  ](j), v[i  ](j),
                                              r[i+1](j), v[i+1](j),
                                              tf);
    return p;
}

template <int N>
Eigen::Matrix<double, N, 1> SplineFit<N>::position(double t, SplineFit<N>::datum& velocity) const