11 template <
typename Scalar>
12 StateVectorTpl<Scalar>::StateVectorTpl(
const std::size_t& nx) : StateAbstractTpl<Scalar>(nx, nx) {}
14 template <
typename Scalar>
15 StateVectorTpl<Scalar>::~StateVectorTpl() {}
17 template <
typename Scalar>
18 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::zero()
const {
19 return MathBase::VectorXs::Zero(nx_);
22 template <
typename Scalar>
23 typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::rand()
const {
24 return MathBase::VectorXs::Random(nx_);
27 template <
typename Scalar>
28 void StateVectorTpl<Scalar>::diff(
const Eigen::Ref<const typename MathBase::VectorXs>& x0,
29 const Eigen::Ref<const typename MathBase::VectorXs>& x1,
30 Eigen::Ref<typename MathBase::VectorXs> dxout)
const {
31 if (static_cast<std::size_t>(x0.size()) != nx_) {
32 throw_pretty(
"Invalid argument: " 33 <<
"x0 has wrong dimension (it should be " + std::to_string(nx_) +
")");
35 if (static_cast<std::size_t>(x1.size()) != nx_) {
36 throw_pretty(
"Invalid argument: " 37 <<
"x1 has wrong dimension (it should be " + std::to_string(nx_) +
")");
39 if (static_cast<std::size_t>(dxout.size()) != ndx_) {
40 throw_pretty(
"Invalid argument: " 41 <<
"dxout has wrong dimension (it should be " + std::to_string(ndx_) +
")");
46 template <
typename Scalar>
47 void StateVectorTpl<Scalar>::integrate(
const Eigen::Ref<const typename MathBase::VectorXs>& x,
48 const Eigen::Ref<const typename MathBase::VectorXs>& dx,
49 Eigen::Ref<typename MathBase::VectorXs> xout)
const {
50 if (static_cast<std::size_t>(x.size()) != nx_) {
51 throw_pretty(
"Invalid argument: " 52 <<
"x has wrong dimension (it should be " + std::to_string(nx_) +
")");
54 if (static_cast<std::size_t>(dx.size()) != ndx_) {
55 throw_pretty(
"Invalid argument: " 56 <<
"dx has wrong dimension (it should be " + std::to_string(ndx_) +
")");
58 if (static_cast<std::size_t>(xout.size()) != nx_) {
59 throw_pretty(
"Invalid argument: " 60 <<
"xout has wrong dimension (it should be " + std::to_string(nx_) +
")");
65 template <
typename Scalar>
66 void StateVectorTpl<Scalar>::Jdiff(
const Eigen::Ref<const typename MathBase::VectorXs>&,
67 const Eigen::Ref<const typename MathBase::VectorXs>&,
68 Eigen::Ref<typename MathBase::MatrixXs> Jfirst,
69 Eigen::Ref<typename MathBase::MatrixXs> Jsecond, Jcomponent firstsecond)
const {
70 assert_pretty(is_a_Jcomponent(firstsecond), (
"firstsecond must be one of the Jcomponent {both, first, second}"));
71 if (firstsecond == first || firstsecond == both) {
72 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
73 throw_pretty(
"Invalid argument: " 74 <<
"Jfirst has wrong dimension (it should be " + std::to_string(ndx_) +
"," + std::to_string(ndx_) +
78 Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, -1.);
80 if (firstsecond == second || firstsecond == both) {
81 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
82 throw_pretty(
"Invalid argument: " 83 <<
"Jsecond has wrong dimension (it should be " + std::to_string(ndx_) +
"," +
84 std::to_string(ndx_) +
")");
87 Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
91 template <
typename Scalar>
92 void StateVectorTpl<Scalar>::Jintegrate(
const Eigen::Ref<const typename MathBase::VectorXs>&,
93 const Eigen::Ref<const typename MathBase::VectorXs>&,
94 Eigen::Ref<typename MathBase::MatrixXs> Jfirst,
95 Eigen::Ref<typename MathBase::MatrixXs> Jsecond,
96 Jcomponent firstsecond)
const {
97 assert_pretty(is_a_Jcomponent(firstsecond), (
"firstsecond must be one of the Jcomponent {both, first, second}"));
98 if (firstsecond == first || firstsecond == both) {
99 if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ || static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
100 throw_pretty(
"Invalid argument: " 101 <<
"Jfirst has wrong dimension (it should be " + std::to_string(ndx_) +
"," + std::to_string(ndx_) +
105 Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
107 if (firstsecond == second || firstsecond == both) {
108 if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ || static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
109 throw_pretty(
"Invalid argument: " 110 <<
"Jsecond has wrong dimension (it should be " + std::to_string(ndx_) +
"," +
111 std::to_string(ndx_) +
")");
114 Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
Definition: action-base.hxx:11