ConstraintTools.cc 6.49 KB
Newer Older
1 2 3 4 5 6 7 8 9
//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_EIGEN3_AVAILABLE

//== INCLUDES =================================================================
#include "ConstraintTools.hh"

#include <CoMISo/Utils/MutablePriorityQueueT.hh>

10 11
#include <Base/Debug/DebOut.hh>

12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48


namespace COMISO {

//-----------------------------------------------------------------------------

void
ConstraintTools::
remove_dependent_linear_constraints( std::vector<NConstraintInterface*>& _constraints, const double _eps )
{
  // split into linear and nonlinear
  std::vector<NConstraintInterface*> lin_const, nonlin_const;

  for(unsigned int i=0; i<_constraints.size(); ++i)
  {
    if(_constraints[i]->is_linear() && _constraints[i]->constraint_type() == NConstraintInterface::NC_EQUAL)
      lin_const.push_back(_constraints[i]);
    else
      nonlin_const.push_back(_constraints[i]);
  }

  remove_dependent_linear_constraints_only_linear_equality( lin_const);

  for(unsigned int i=0; i<lin_const.size(); ++i)
    nonlin_const.push_back(lin_const[i]);

  // return filtered constraints
  _constraints.swap(nonlin_const);
}


//-----------------------------------------------------------------------------

void
ConstraintTools::
remove_dependent_linear_constraints_only_linear_equality( std::vector<NConstraintInterface*>& _constraints, const double _eps)
{
49
  DEB_enter_func;
50 51 52 53
  // make sure that constraints are available
  if(_constraints.empty()) return;

  // 1. copy (normalized) data into gmm dynamic sparse matrix
Max Lyon's avatar
Max Lyon committed
54 55
  size_t n(_constraints[0]->n_unknowns());
  size_t m(_constraints.size());
56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83
  std::vector<double> x(n, 0.0);
  NConstraintInterface::SVectorNC g;
  RMatrixGMM A;
  gmm::resize(A,m, n+1);
  for(unsigned int i=0; i<_constraints.size(); ++i)
  {
    // store rhs in last column
    A(i,n) = _constraints[i]->eval_constraint(x.data());
    // get and store coefficients
    _constraints[i]->eval_gradient(x.data(), g);
    double v_max(0.0);
    for (NConstraintInterface::SVectorNC::InnerIterator it(g); it; ++it)
    {
      A(i,it.index()) = it.value();
      v_max = std::max(v_max, std::abs(it.value()));
    }
    // normalize row
    if(v_max != 0.0)
      gmm::scale(A.row(i), 1.0/v_max);
  }

  // 2. get additionally column matrix to exploit column iterators
  CMatrixGMM Ac;
  gmm::resize(Ac, gmm::mat_nrows(A), gmm::mat_ncols(A));
  gmm::copy(A, Ac);

  // 3. initialize priorityqueue for sorting
  // init priority queue
Max Lyon's avatar
Max Lyon committed
84
  MutablePriorityQueueT<gmm::size_type, gmm::size_type> queue;
85
  queue.clear(m);
Max Lyon's avatar
Max Lyon committed
86
  for (gmm::size_type i = 0; i<m; ++i)
87
  {
Max Lyon's avatar
Max Lyon committed
88 89 90
    gmm::size_type cur_nnz = gmm::nnz( gmm::mat_row(A,i));
    if (A(i,n) != 0.0)
      --cur_nnz;
91 92 93 94 95 96

    queue.update(i, cur_nnz);
  }

  // track row status -1=undecided, 0=remove, 1=keep
  std::vector<int> row_status(m, -1);
Max Lyon's avatar
Max Lyon committed
97
  std::vector<gmm::size_type> keep;
98 99 100 101 102 103
//  std::vector<int> remove;

  // for all conditions
  while(!queue.empty())
  {
    // get next row
Max Lyon's avatar
Max Lyon committed
104 105
    gmm::size_type i = queue.get_next();
    gmm::size_type j = find_max_abs_coeff(A.row(i));
106 107 108
    double aij = A(i,j);
    if(std::abs(aij) <= _eps)
    {
109 110
//      std::cerr << "drop " << aij << "in row " << i << "and column " << j << std::endl;
      // constraint is linearly dependent
111
      row_status[i] = 0;
112 113
      if(std::abs(A(i,n)) > _eps)
        std::cerr << "Warning: found dependent constraint with nonzero rhs " << A(i,n) << std::endl;
114 115 116
    }
    else
    {
117 118
//      std::cerr << "keep " << aij << "in row " << i << "and column " << j << std::endl;

119 120 121 122 123 124 125 126
      // constraint is linearly independent
      row_status[i] = 1;
      keep.push_back(i);

      // update undecided constraints
      // copy col
      SVectorGMM col = Ac.col(j);

127
      // copy row
Max Lyon's avatar
Max Lyon committed
128
      SVectorGMM row = A.row(i);
129

130
      // iterate over column
Jan Möbius's avatar
Jan Möbius committed
131 132
      gmm::linalg_traits<SVectorGMM>::const_iterator c_it   = gmm::vect_const_begin(col);
      gmm::linalg_traits<SVectorGMM>::const_iterator c_end  = gmm::vect_const_end(col);
133 134 135 136 137

      for(; c_it != c_end; ++c_it)
        if( row_status[c_it.index()] == -1) // only process unvisited rows
        {
          // row idx
Max Lyon's avatar
Max Lyon committed
138
          gmm::size_type k = c_it.index();
139 140 141 142 143 144 145

          double s = -(*c_it)/aij;
          add_row_simultaneously( k, s, row, A, Ac, _eps);
          // make sure the eliminated entry is 0 on all other rows
          A( k, j) = 0;
          Ac(k, j) = 0;

Max Lyon's avatar
Max Lyon committed
146 147 148
          gmm::size_type cur_nnz = gmm::nnz( gmm::mat_row(A,k));
          if( A(k,n) != 0.0)
            --cur_nnz;
149 150 151 152 153 154

          queue.update(k, cur_nnz);
        }
    }
  }

155 156
  DEB_line(2, "removed " << _constraints.size()-keep.size() << 
    " dependent linear constraints out of " << _constraints.size());
157 158 159 160 161 162 163 164 165 166 167 168 169 170

  // 4. store result
  std::vector<NConstraintInterface*> new_constraints;
  for(unsigned int i=0; i<keep.size(); ++i)
    new_constraints.push_back(_constraints[keep[i]]);

  // return linearly independent ones
  _constraints.swap(new_constraints);
}


//-----------------------------------------------------------------------------


Max Lyon's avatar
Max Lyon committed
171
gmm::size_type
172 173 174
ConstraintTools::
find_max_abs_coeff(SVectorGMM& _v)
{
Max Lyon's avatar
Max Lyon committed
175 176
  size_t n = _v.size();
  gmm::size_type imax(0);
177 178
  double       vmax(0.0);

Jan Möbius's avatar
Jan Möbius committed
179 180
  gmm::linalg_traits<SVectorGMM>::const_iterator c_it   = gmm::vect_const_begin(_v);
  gmm::linalg_traits<SVectorGMM>::const_iterator c_end  = gmm::vect_const_end(_v);
181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198

  for(; c_it != c_end; ++c_it)
    if(c_it.index() != n-1)
      if(std::abs(*c_it) > vmax)
      {
        imax = c_it.index();
        vmax = *c_it;
      }

  return imax;
}


//-----------------------------------------------------------------------------


void
ConstraintTools::
Max Lyon's avatar
Max Lyon committed
199
add_row_simultaneously( gmm::size_type _row_i,
200 201 202 203 204 205
                        double      _coeff,
                        SVectorGMM& _row,
                        RMatrixGMM& _rmat,
                        CMatrixGMM& _cmat,
                        const double _eps )
{
Jan Möbius's avatar
Jan Möbius committed
206
  typedef gmm::linalg_traits<SVectorGMM>::const_iterator RIter;
207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227
  RIter r_it  = gmm::vect_const_begin(_row);
  RIter r_end = gmm::vect_const_end(_row);

  for(; r_it!=r_end; ++r_it)
  {
    _rmat(_row_i, r_it.index()) += _coeff*(*r_it);
    _cmat(_row_i, r_it.index()) += _coeff*(*r_it);
    if( std::abs(_rmat(_row_i, r_it.index())) < _eps )
    {
      _rmat(_row_i, r_it.index()) = 0.0;
      _cmat(_row_i, r_it.index()) = 0.0;
    }
  }
}

//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================