77lagrange(
const int p,
const Scalar& t, SolutionState<Scalar>* state_out)
const
80 using Teuchos::is_null;
82 using Thyra::V_StVpStV;
85 int n = nodes_->size();
86 TEUCHOS_ASSERT(n >= p);
88 const Scalar t_begin = (*nodes_)[0]->getTime();
89 const Scalar t_final = (*nodes_)[n-1]->getTime();
96 else if (t >= t_final)
99 auto it = std::find_if(nodes_->begin(), nodes_->end(),
100 [=](
const RCP<SolutionState<Scalar> >& s)
102 return t <= s->getTime();
104 i = std::distance(nodes_->begin(), it)-1;
106 TEUCHOS_ASSERT(i >= 0 && i<=n-1);
110 state_out->copy((*nodes_)[i]);
114 state_out->copy((*nodes_)[i+1]);
124 TEUCHOS_ASSERT(k >= 0 && k+p+1 <= n);
126 RCP<VectorBase<Scalar> > x = state_out->getX();
127 RCP<VectorBase<Scalar> > xdot = state_out->getXDot();
128 RCP<VectorBase<Scalar> > xdotdot = state_out->getXDotDot();
130 Scalar zero = Teuchos::ScalarTraits<Scalar>::zero();
131 Thyra::assign(x.ptr(), zero);
133 Thyra::assign(xdot.ptr(), zero);
134 if (!is_null(xdotdot))
135 Thyra::assign(xdotdot.ptr(), zero);
137 for (
int j=0; j<=p; ++j) {
138 const Scalar tj = (*nodes_)[k+j]->getTime();
139 RCP<const VectorBase<Scalar> > xj = (*nodes_)[k+j]->getX();
140 RCP<const VectorBase<Scalar> > xdotj = (*nodes_)[k+j]->getXDot();
141 RCP<const VectorBase<Scalar> > xdotdotj = (*nodes_)[k+j]->getXDotDot();
145 for (
int l=0; l<=p; ++l) {
147 const Scalar tl = (*nodes_)[k+l]->getTime();
152 const Scalar a = num / den;
153 Thyra::Vp_StV(x.ptr(), a, *xj);
155 Thyra::Vp_StV(xdot.ptr(), a, *xdotj);
156 if (!is_null(xdotdot))
157 Thyra::Vp_StV(xdotdot.ptr(), a, *xdotdotj);
161 state_out->getMetaData()->copy((*nodes_)[i]->getMetaData());
162 state_out->getMetaData()->setTime(t);
163 state_out->getMetaData()->setDt(t-(*nodes_)[i]->getTime());
164 state_out->getMetaData()->setIsInterpolated(
true);