void dataStoreVectorToVector(
const typename DataStore<Scalar>::DataStoreVector_t &ds
,Array<Scalar> *time_vec
,Array<Teuchos::RCP<const Thyra::VectorBase<Scalar> > > *x_vec
,Array<Teuchos::RCP<const Thyra::VectorBase<Scalar> > > *xdot_vec
,Array<typename Teuchos::ScalarTraits<Scalar>::magnitudeType> *accuracy_vec)
{
if(time_vec)
time_vec->clear();
if(x_vec)
x_vec->clear();
if(xdot_vec)
xdot_vec->clear();
if(accuracy_vec)
accuracy_vec->clear();
int N = ds.size();
for (int i=0; i<N ; ++i) {
if(time_vec)
time_vec->push_back(ds[i].time);
if(x_vec)
x_vec->push_back(ds[i].x);
if(xdot_vec)
xdot_vec->push_back(ds[i].xdot);
if(accuracy_vec)
accuracy_vec->push_back(ds[i].accuracy);
}
}
void assertBaseInterpolatePreconditions(
const typename DataStore<Scalar>::DataStoreVector_t &data_in,
const Array<Scalar> &t_values,
typename DataStore<Scalar>::DataStoreVector_t *data_out
)
{
TEUCHOS_TEST_FOR_EXCEPTION(
data_in.size()==0, std::logic_error,
"Error, data_in.size() == 0!\n"
);
Array<Scalar> time_vec;
dataStoreVectorToVector<Scalar>(data_in, &time_vec, 0, 0, 0);
assertTimePointsAreSorted<Scalar>(time_vec);
assertTimePointsAreSorted<Scalar>(t_values);
if (data_in.size() == 1) {
TEUCHOS_TEST_FOR_EXCEPTION(
t_values.size()>1, std::logic_error,
"Error, data_in.size() == 1, but t_values.size() > 1!\n"
);
TEUCHOS_TEST_FOR_EXCEPTION(
compareTimeValues(t_values[0],data_in[0].time)!=0, std::logic_error,
"Error, data_in.size) == 1, but t_values[0] = " <<
t_values[0] << " != " << data_in[0].time << " = data_in[0].time!\n"
);
}
TimeRange<Scalar> range(data_in.front().time,data_in.back().time);
for (int i=0; i<Teuchos::as<int>(t_values.size()) ; ++i) {
TEUCHOS_TEST_FOR_EXCEPTION(
!range.isInRange(t_values[i]), std::out_of_range,
"Error, t_values[" << i << "] = " << t_values[i] <<
" is not in range of data_in = " << range << "!\n"
);
}
TEUCHOS_TEST_FOR_EXCEPTION(
data_out == 0, std::logic_error,
"Error, data_out = NULL!\n"
);
}
void assertNodesUnChanged(
const typename DataStore<Scalar>::DataStoreVector_t & nodes,
const typename DataStore<Scalar>::DataStoreVector_t & nodes_copy
)
{
typedef Teuchos::ScalarTraits<Scalar> ST;
int N = nodes.size();
int Ncopy = nodes_copy.size();
TEUCHOS_TEST_FOR_EXCEPTION( N != Ncopy, std::logic_error,
"Error! The number of nodes passed in through setNodes has changed!"
);
if (N > 0) {
RCP<Thyra::VectorBase<Scalar> > xdiff = nodes[0].x->clone_v();
RCP<Thyra::VectorBase<Scalar> > xdotdiff = xdiff->clone_v();
V_S(outArg(*xdiff),ST::one());
V_S(outArg(*xdotdiff),ST::one());
for (int i=0 ; i<N ; ++i) {
V_StVpStV(outArg(*xdiff),ST::one(),*nodes[i].x,-ST::one(),*nodes_copy[i].x);
if ((!Teuchos::is_null(nodes[i].xdot)) && (!Teuchos::is_null(nodes_copy[i].xdot))) {
V_StVpStV(outArg(*xdotdiff),ST::one(),*nodes[i].xdot,-ST::one(),*nodes_copy[i].xdot);
} else if (Teuchos::is_null(nodes[i].xdot) && Teuchos::is_null(nodes_copy[i].xdot)) {
V_S(outArg(*xdotdiff),ST::zero());
}
Scalar xdiffnorm = norm_inf(*xdiff);
Scalar xdotdiffnorm = norm_inf(*xdotdiff);
TEUCHOS_TEST_FOR_EXCEPTION(
( ( nodes[i].time != nodes_copy[i].time ) ||
( xdiffnorm != ST::zero() ) ||
( xdotdiffnorm != ST::zero() ) ||
( nodes[i].accuracy != nodes_copy[i].accuracy ) ),
std::logic_error,
"Error! The data in the nodes passed through setNodes has changed!"
);
}
}
}
void HermiteInterpolator<Scalar>::assertInterpolatePreconditions(
const typename DataStore<Scalar>::DataStoreVector_t &data_in
,const Array<Scalar> &t_values
,typename DataStore<Scalar>::DataStoreVector_t *data_out
) const
{
assertBaseInterpolatePreconditions(data_in,t_values,data_out);
for (int i=0; i<Teuchos::as<int>(data_in.size()) ; ++i) {
TEUCHOS_TEST_FOR_EXCEPTION(
is_null(data_in[i].x), std::logic_error,
"Error, data_in[" << i << "].x == Teuchos::null.\n"
);
TEUCHOS_TEST_FOR_EXCEPTION(
is_null(data_in[i].xdot), std::logic_error,
"Error, data_in[" << i << "].xdot == Teuchos::null.\n"
);
}
}
void computeCubicSplineCoeff(
const typename DataStore<Scalar>::DataStoreVector_t & data,
const Ptr<CubicSplineCoeff<Scalar> > & coeffPtr
)
{
using Teuchos::outArg;
typedef Teuchos::ScalarTraits<Scalar> ST;
using Thyra::createMember;
TEUCHOS_TEST_FOR_EXCEPTION(
(data.size() < 2), std::logic_error,
"Error! A minimum of two data points is required for this cubic spline."
);
// time data in the DataStoreVector should be unique and sorted
Array<Scalar> t;
Array<Teuchos::RCP<const Thyra::VectorBase<Scalar> > > x_vec, xdot_vec;
dataStoreVectorToVector<Scalar>( data, &t, &x_vec, &xdot_vec, NULL );
#ifdef HAVE_RYTHMOS_DEBUG
assertTimePointsAreSorted<Scalar>( t );
#endif // HAVE_RYTHMOS_DEBUG
// 11/18/08 tscoffe: Question: Should I erase everything in coeffPtr or
// re-use what I can? For now, I'll erase and create new each time.
CubicSplineCoeff<Scalar>& coeff = *coeffPtr;
// If there are only two points, then we do something special and just create
// a linear polynomial between the points and return.
if (t.size() == 2) {
coeff.t.clear();
coeff.a.clear(); coeff.b.clear(); coeff.c.clear(); coeff.d.clear();
coeff.t.reserve(2);
coeff.a.reserve(1); coeff.b.reserve(1); coeff.c.reserve(1); coeff.d.reserve(1);
coeff.t.push_back(t[0]);
coeff.t.push_back(t[1]);
coeff.a.push_back(x_vec[0]->clone_v());
coeff.b.push_back(createMember(x_vec[0]->space()));
coeff.c.push_back(createMember(x_vec[0]->space()));
coeff.d.push_back(createMember(x_vec[0]->space()));
Scalar h = coeff.t[1] - coeff.t[0];
V_StVpStV(outArg(*coeff.b[0]),ST::one()/h,*x_vec[1],-ST::one()/h,*x_vec[0]);
V_S(outArg(*coeff.c[0]),ST::zero());
V_S(outArg(*coeff.d[0]),ST::zero());
return;
}
// Data objects we'll need:
int n = t.length()-1; // Number of intervals
coeff.t.clear(); coeff.t.reserve(n+1);
coeff.a.clear(); coeff.a.reserve(n+1);
coeff.b.clear(); coeff.b.reserve(n);
coeff.c.clear(); coeff.c.reserve(n+1);
coeff.d.clear(); coeff.d.reserve(n);
Array<Scalar> h(n);
Array<RCP<Thyra::VectorBase<Scalar> > > alpha(n), z(n+1);
Array<Scalar> l(n+1), mu(n);
for (int i=0 ; i<n ; ++i) {
coeff.t.push_back(t[i]);
coeff.a.push_back(x_vec[i]->clone_v());
coeff.b.push_back(Thyra::createMember(x_vec[0]->space()));
coeff.c.push_back(Thyra::createMember(x_vec[0]->space()));
coeff.d.push_back(Thyra::createMember(x_vec[0]->space()));
alpha[i] = Thyra::createMember(x_vec[0]->space());
z[i] = Thyra::createMember(x_vec[0]->space());
}
coeff.a.push_back(x_vec[n]->clone_v());
coeff.t.push_back(t[n]);
coeff.c.push_back(Thyra::createMember(x_vec[0]->space()));
z[n] = Thyra::createMember(x_vec[0]->space());
Scalar zero = ST::zero();
Scalar one = ST::one();
Scalar two = Scalar(2*ST::one());
Scalar three = Scalar(3*ST::one());
// Algorithm starts here:
for (int i=0 ; i<n ; ++i) {
h[i] = coeff.t[i+1]-coeff.t[i];
}
for (int i=1 ; i<n ; ++i) {
V_StVpStV(outArg(*(alpha[i])),three/h[i],*coeff.a[i+1],-3/h[i],*coeff.a[i]);
Vp_StV(outArg(*(alpha[i])),-three/h[i-1],*coeff.a[i]);
Vp_StV(outArg(*(alpha[i])),+three/h[i-1],*coeff.a[i-1]);
}
l[0] = one;
mu[0] = zero;
V_S(outArg(*(z[0])),zero);
for (int i=1 ; i<n ; ++i) {
l[i] = 2*(coeff.t[i+1]-coeff.t[i-1])-h[i-1]*mu[i-1];
mu[i] = h[i]/l[i];
V_StVpStV(outArg(*(z[i])),one/l[i],*alpha[i],-h[i-1]/l[i],*z[i-1]);
}
l[n] = one;
V_S(outArg(*(z[n])),zero);
V_S(outArg(*(coeff.c[n])),zero);
for (int j=n-1 ; j >= 0 ; --j) {
V_StVpStV(outArg(*(coeff.c[j])),one,*z[j],-mu[j],*coeff.c[j+1]);
V_StVpStV(outArg(*(coeff.b[j])),one/h[j],*coeff.a[j+1],-one/h[j],*coeff.a[j]);
Vp_StV(outArg(*(coeff.b[j])),-h[j]/three,*coeff.c[j+1]);
Vp_StV(outArg(*(coeff.b[j])),-h[j]*two/three,*coeff.c[j]);
V_StVpStV(outArg(*(coeff.d[j])),one/(three*h[j]),*coeff.c[j+1],-one/(three*h[j]),*coeff.c[j]);
}
// Pop the last entry off of a and c to make them the right size.
coeff.a.erase(coeff.a.end()-1);
coeff.c.erase(coeff.c.end()-1);
}
