8#include <boost/core/span.hpp>
11template <
typename Derived>
13 boost::span<const Eigen::Index> rowBlockSizes,
15 Eigen::Index startIdx = 0;
16 for (std::size_t kk = 0; kk < rowIdx; kk++) {
17 startIdx += rowBlockSizes[kk];
19 return matrix.const_cast_derived().middleRows(startIdx,
20 rowBlockSizes[rowIdx]);
23template <
typename Derived>
25 boost::span<const Eigen::Index> blockSizes,
26 std::size_t blockIdx) {
27 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
28 Eigen::Index startIdx = 0;
29 for (std::size_t kk = 0; kk < blockIdx; kk++) {
30 startIdx += blockSizes[kk];
32 return matrix.const_cast_derived().segment(startIdx, blockSizes[blockIdx]);
40template <
typename Scalar>
51 ALIGATOR_RUNTIME_ERROR(
"Number of components and corresponding "
52 "block sizes should be the same.");
63 for (std::size_t i = 0; i < m_components.size(); i++) {
65 res += m_components[i]->evaluate(zblock);
70 void projection(
const ConstVectorRef &z, VectorRef zout)
const override {
71 for (std::size_t i = 0; i < m_components.size(); i++) {
74 m_components[i]->projection(inblock, outblock);
79 VectorRef zout)
const override {
80 for (std::size_t i = 0; i < m_components.size(); i++) {
83 m_components[i]->normalConeProjection(inblock, outblock);
88 MatrixRef Jout)
const override {
89 for (std::size_t i = 0; i < m_components.size(); i++) {
92 m_components[i]->applyProjectionJacobian(inblock, outblock);
97 MatrixRef Jout)
const override {
98 for (std::size_t i = 0; i < m_components.size(); i++) {
101 m_components[i]->applyNormalConeProjectionJacobian(inblock, outblock);
106 Eigen::Ref<ActiveType> out)
const override {
107 for (std::size_t i = 0; i < m_components.size(); i++) {
110 m_components[i]->computeActiveSet(inblock, outblock);
114 const std::vector<xyz::polymorphic<Base>> &
components()
const {
117 const std::vector<Eigen::Index> &
blockSizes()
const {
return m_blockSizes; }
120 std::vector<xyz::polymorphic<Base>> m_components;
121 std::vector<Eigen::Index> m_blockSizes;
auto blockMatrixGetRow(const Eigen::MatrixBase< Derived > &matrix, boost::span< const Eigen::Index > rowBlockSizes, std::size_t rowIdx)
auto blockVectorGetRow(const Eigen::MatrixBase< Derived > &matrix, boost::span< const Eigen::Index > blockSizes, std::size_t blockIdx)
ConstraintSetProductTpl(const ConstraintSetProductTpl &)=default
Scalar evaluate(const ConstVectorRef &zproj) const override
typename Base::ActiveType ActiveType
void normalConeProjection(const ConstVectorRef &z, VectorRef zout) const override
Compute projection of z onto the normal cone to the set. The default implementation is just .
ConstraintSetProductTpl & operator=(const ConstraintSetProductTpl &)=default
ConstraintSetProductTpl & operator=(ConstraintSetProductTpl &&)=default
ConstraintSetTpl< Scalar > Base
void projection(const ConstVectorRef &z, VectorRef zout) const override
Compute projection of variable z onto the constraint set.
const std::vector< xyz::polymorphic< Base > > & components() const
ConstraintSetProductTpl(ConstraintSetProductTpl &&)=default
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
void applyNormalConeProjectionJacobian(const ConstVectorRef &z, MatrixRef Jout) const override
Apply the jacobian of the projection on the normal cone.
const std::vector< Eigen::Index > & blockSizes() const
ConstraintSetProductTpl(const std::vector< xyz::polymorphic< Base > > components, const std::vector< Eigen::Index > &blockSizes)
void computeActiveSet(const ConstVectorRef &z, Eigen::Ref< ActiveType > out) const override
void applyProjectionJacobian(const ConstVectorRef &z, MatrixRef Jout) const override
Apply a jacobian of the projection/proximal operator to a matrix.
ConstraintSetTpl()=default
Eigen::Matrix< bool, Eigen::Dynamic, 1 > ActiveType