FSQPConstraintManager.h

Go to the documentation of this file.

#ifndef _OCRABASE_FSQP_CONSTRAINT_MANAGER_H_
#define _OCRABASE_FSQP_CONSTRAINT_MANAGER_H_

// includes
#include "Constraint.h"
#include <vector>

namespace ocra
{
  class FSQPConstraintManager
  {
    // ------------------------ structures --------------------------------------
  public:
    typedef CwiseUnaryOp<internal::scalar_multiple_op<double>, MatrixXdRow> ScalarMultMatrixXdRow;

    struct Mapping
    {
      int length;
      int index;

      Mapping() : length(0), index(0) {}
      Mapping(int i) : length(0), index(i) {}
      Mapping(int l, int i) : length(l), index(i) {}
    };

    // ------------------------ constructors ------------------------------------
  private:
    FSQPConstraintManager(const FSQPConstraintManager&);
    FSQPConstraintManager& operator=(const FSQPConstraintManager&);
  protected:
  public:
    FSQPConstraintManager();

    // ------------------------ public interface --------------------------------
  public:
    void addConstraint(LinearConstraint& constraint);
    void addConstraint(GenericConstraint& constraint);
    void removeConstraint(LinearConstraint& constraint);
    void removeConstraint(GenericConstraint& constraint);

    double                      getValue(int i) const;
    const ScalarMultMatrixXdRow getGradient(int i) const;
    const ScalarMultMatrixXdRow getGradient(const std::pair<GenericConstraint*, int>& p) const;

    int  nineqn() const {updateMapping(); return _nineqn;}
    int  nineq()  const {updateMapping(); return _nineq;}
    int  neqn()   const {updateMapping(); return _neqn;}
    int  neq()    const {updateMapping(); return _neq;}

    void updateMapping() const; //force the update of the mapping which is otherwise done lazily when calling operator[]
    void invalidateMapping();   

    std::pair<GenericConstraint*, int> operator[] (int i) const;


    // ------------------------ private members ---------------------------------
  private:
    mutable int _nineqn;
    mutable int _nineq;
    mutable int _neqn;
    mutable int _neq;
    std::vector<GenericConstraint*> _nonLinearIneq;
    std::vector<LinearConstraint*>  _linearIneq;
    std::vector<GenericConstraint*> _nonLinearEq;
    std::vector<LinearConstraint*>  _linearEq;

    mutable std::vector<Mapping>  _mappings;
    mutable int                   _invalidatedIndex;                  
  };

  void testFsqpMapping();
}

#endif  //_OCRABASE_FSQP_CONSTRAINT_MANAGER_H_