Fix index handling from unsigned int to proper template derivation.

src/ActiveSet.hpp

@@ -21,74 +21,258 @@ namespace soth
    * is stored in WMLY will need to inverse the W.indirectRow map, which is not
    * done in the class.
    */
+  template <typename Indirect>
   class SOTH_EXPORT ActiveSet
   {
+    typedef typename Indirect::Index Index;
   public: /* --- Construction --- */
-    ActiveSet( unsigned int nr );
-    void                  reset( void );
+    ActiveSet( Index nr )
+      :cstMap(nr),cstMapInv(nr),freerow(nr),freezed(nr),activated(nr)
+      ,dampingInf(nr),dampingSup(nr),nba(0)
+      {
+	reset();
+      }
+
+    void                  reset( void )
+    {
+      std::fill( cstMap.begin(),cstMap.end(),CONSTRAINT_VOID );
+      std::fill( cstMapInv.begin(),cstMapInv.end(),size() );
+      std::fill( freerow.begin(),freerow.end(),true );
+      std::fill( freezed.begin(),freezed.end(),false );
+      activated.fill(Bound::BOUND_NONE);
+      dampingInf.setZero();    dampingSup.setZero();
+      
+      nba=0;
+    }
+
 
   public: /* --- Active set management --- */
     /* Active the constraint <ref> (ie in J(ref,:)) at line <row> (ie in ML(row,:))
      * with bound type. */
-    void                  active( unsigned int ref, Bound::bound_t type, unsigned int row );
+    void active( Index ref, Bound::bound_t type, const Index row )
+    {
+      assert( ref<size() );
+      assert( (cstMap[ref].type == Bound::BOUND_NONE)
+	      &&"Constraint has not been properly unactivated." );
+      assert( row<size() );
+      assert( freerow[row] );
+      assert( cstMapInv[row]==size() );
+      assert( type!=Bound::BOUND_NONE );
+      
+      cstMap[ref]=ConstraintRef(row,type);
+      cstMapInv[row]=ref;
+      freerow[row]=false; nba++;
+      
+      if( activated[ref]==Bound::BOUND_NONE )
+	activated[ref]=type;
+      else
+	{
+	  //assert( activated[ref]!=type );
+	  assert( type==Bound::BOUND_INF || type==Bound::BOUND_SUP );
+	  activated[ref]=Bound::BOUND_DOUBLE;
+	}
+    }
+
     /* Active the given constraint at any free row of ML, and return the
      * position of the selected free row. */
-    unsigned int          activeRow( unsigned int ref, Bound::bound_t type );
-    inline unsigned int   activeRow( const ConstraintRef& cst ) { return activeRow( cst.row,cst.type ); }
+    Index          activeRow( Index ref, Bound::bound_t type )
+    {
+      const Index row = getAFreeRow();
+      assert( row<size() );
+      active( ref,type,row );
+      return row;
+    }
+
+    inline Index   activeRow( const ConstraintRef& cst ) { return activeRow( cst.row,cst.type ); }
     /* Unactive the constraint at line <row> of ML and frees the corresponding line. */
-    void                  unactiveRow( unsigned int row );
-    /* Pass the constraint to a twin mode. */
-    void                  freeze( unsigned int ref );
+    void                  unactiveRow( Index row )
+    {
+      assert( row<size() );
+      
+      const Index & cst = cstMapInv[row];
+      assert( cst<size() );
+      assert( cstMap[cst].type != Bound::BOUND_NONE );
+      
+      cstMap[cst] = CONSTRAINT_VOID;
+      freeARow(row);
+      cstMapInv[row] = size();
+      nba--;
+    }
 
+    /* Pass the constraint to a twin mode. */
+    void                  freeze( Index ref )
+    {
+      assert( ref<size() );
+      assert( cstMap[ref].type != Bound::BOUND_NONE );
+      assert( cstMap[ref].type != Bound::BOUND_TWIN );
+      assert(! freezed[ref] );
+      
+      freezed[ref]=true;
+    }
 
   public: /* --- Accessors --- */
     /* Return the number of active constraint. */
-    inline unsigned int   nbActive( void ) const { return nba; }
-    inline unsigned int   size( void ) const { return (unsigned int)cstMap.size(); }
-    bool                  isFreezed( unsigned int ref ) const;
-    bool                  isActive( unsigned int ref ) const;
-    bool                  wasActive( unsigned int ref,const Bound::bound_t type ) const;
-    Bound::bound_t        whichBound( unsigned int ref,bool checkActive=false ) const;
-    double                sign( unsigned int ref ) const;
+    inline Index   nbActive( void ) const { return nba; }
+    inline Index   size( void ) const { return (Index)cstMap.size(); }
+    bool                  isFreezed( Index ref ) const
+    {
+      assert( isActive(ref) );
+      return ( cstMap[ref].type == Bound::BOUND_TWIN )||(freezed[ref]);
+    }
+    bool                  isActive( Index ref ) const
+    {
+      assert( ref<size() );
+      return( cstMap[ref].type != Bound::BOUND_NONE );
+    }
+
+    bool                  wasActive( Index ref,const Bound::bound_t type ) const
+    {
+      assert( ref<size() ); assert( activated.size() == (int)size() );
+      assert( type==Bound::BOUND_INF || type==Bound::BOUND_SUP );
+      assert( activated[ref]==Bound::BOUND_INF || activated[ref]==Bound::BOUND_SUP
+	      || activated[ref]==Bound::BOUND_NONE || activated[ref]==Bound::BOUND_DOUBLE );
+      if( type==Bound::BOUND_INF )
+	return activated[ref]==Bound::BOUND_INF || activated[ref]==Bound::BOUND_DOUBLE;
+      else if( type==Bound::BOUND_SUP )
+	return activated[ref]==Bound::BOUND_SUP || activated[ref]==Bound::BOUND_DOUBLE;
+      else return( false );
+    }
+
+    Bound::bound_t        whichBound( Index ref,bool checkActive=false ) const
+    {
+      assert( isActive(ref) );
+      const Bound::bound_t & res = cstMap[ref].type;
+      if( checkActive ) { assert( (res!=Bound::BOUND_NONE)&&(res!=Bound::BOUND_DOUBLE) ); }
+      return res;
+    }
+
+    double                sign( Index ref ) const
+    {
+      assert( isActive(ref) );
+      assert( cstMap[ref].type != Bound::BOUND_DOUBLE );
+      return (cstMap[ref].type==Bound::BOUND_INF)?-1:+1;
+    }
+
     /* Map: from the cst ref, give the row of J.
      * Map inversion: give the reference of the constraint (ie line in J) located at row <row> of
      * the working space ML. */
-    unsigned int          map( unsigned int ref ) const;
-    unsigned int          mapInv( unsigned int row ) const;
-    VectorXi              getIndirection( void ) const;
+    Index          map( Index ref ) const
+    {
+      assert( isActive(ref) );
+      return cstMap[ref].row;
+    }
+
+    Index          mapInv(Index row ) const
+    {
+      assert( row<size() );
+      assert( (cstMapInv[row]<size())&&"The requested row is not active" );
+      return cstMapInv[row];
+    }
+
+    Matrix<Index,Dynamic,1>       getIndirection( void ) const
+    {
+      if (nba==0)
+	return Matrix<Index,Dynamic,1>();
+      
+      Matrix<Index,Dynamic,1> res(nba);
+      int row = 0;
+      for( Index i=0;i<size();++i )
+	if(! freerow[i] ) res(row++) = whichConstraint(i);
+      return res;
+    }
+
 
     /* For compatibility */
-    inline unsigned int   where( unsigned int ref ) const { return map(ref); }
-    inline unsigned int   whichConstraint( unsigned int row ) const { return mapInv(row); }
+    inline Index   where( Index ref ) const { return map(ref); }
+    inline Index   whichConstraint( Index row ) const { return mapInv(row); }
 
     /* Damping */
-    void dampBoundValue( const ConstraintRef & cst,const double & value );
-    std::pair<double,double> getBoundDamping( const unsigned int & cst );
+    void dampBoundValue( const ConstraintRef & cst,const double & value )
+    {
+      assert( cst.type==Bound::BOUND_INF || cst.type==Bound::BOUND_SUP );
+      const Index & i = cst.row;
+      if( cst.type==Bound::BOUND_INF && +value>dampingInf[i] ) dampingInf[i] = +value;
+      if( cst.type==Bound::BOUND_SUP && -value>dampingSup[i] ) dampingSup[i] = -value;
+    }
+
+    std::pair<double,double> getBoundDamping( const Index & cst )
+    {
+      return std::make_pair( dampingInf[cst],dampingSup[cst] );
+    }
+
+
 
   public: /* --- Display --- */
     /* Return a compact of the active line, ordered by row values. */
-    void                  disp( std::ostream& os,bool classic=true ) const;
+    void                  disp( std::ostream& os,bool classic=true ) const
+    {
+      if( classic )
+	{
+	  os << " [ ";
+	  for( unsigned int r=0;r<size();++r )
+	    {
+	      if( freerow[r] ) continue;
+	      const Index cst = mapInv(r);
+	      if( whichBound(cst) == Bound::BOUND_INF ) os << "-";
+	      else if( whichBound(cst) == Bound::BOUND_SUP ) os << "+";
+	      os <<r << " ";
+	    }
+	  os << " ]";
+	}
+      else
+	{
+	  for( unsigned int i=0;i<size();++ i )
+	    {
+	      os << i << ": ";
+	      if(isActive(i)) os<< cstMap[i].type; else os <<"Unactive";
+	      os << std::endl;
+	    }
+	  for( unsigned int i=0;i<freerow.size();++ i )
+	    { os << (freerow[i]?"0":"1"); }
+	  os<<std::endl;
+	}
+    }
 
   public: /* --- Deprecated --- */
     /* DEPRECATED*/void   permuteRows( const VectorXi & P );
 
   protected: /* TODO: change that for using the ConstraintRef def in Bound.hpp. */
-    typedef std::vector<unsigned int> mapinv_vector_t;
+    typedef std::vector<Index> mapinv_vector_t;
   protected:
     cstref_vector_t cstMap;
     mapinv_vector_t cstMapInv;
     std::vector<bool> freerow,freezed;
     Matrix<Bound::bound_t,Dynamic,1> activated;
     VectorXd dampingInf, dampingSup;
-    unsigned int nba;
+    Index nba;
 
   protected: /* Internal management */
-    bool isRowFree( unsigned int row );
-    unsigned int getAFreeRow( void );
-    void freeARow( unsigned int row );
+    bool isRowFree( Index row )
+    {
+      return freerow[row];
+    }
+
+    Index getAFreeRow( void )
+    {
+      /* TODO: it is possible to store the first freeline. */
+      assert( nba < size() );
+      for( Index row=0;row<size();++row )
+	{
+	  if( freerow[row] ) return row;
+	}
+      assert( false&&"Could never happen." );
+      return -1;
+    }
+
+    void freeARow( Index row )
+    {
+      assert( ! freerow[row] );
+      freerow[row]=true;
+    }
+
 
   public:
-    inline operator VectorXi (void) const {  return getIndirection(); }
+    inline operator Matrix<Index,Dynamic,1> (void) const {  return getIndirection(); }
     SOTH_EXPORT friend std::ostream& operator<< ( std::ostream & os,const ActiveSet& as );
   };
 
@@ -100,27 +284,29 @@ namespace soth
   class SubActiveSet
     : protected AS
   {
+    typedef typename Indirect::Index Index;
   public:
-    SubActiveSet( unsigned int nr );
-    SubActiveSet( unsigned int nr, Indirect& idx );
+    SubActiveSet( Index nr );
+    SubActiveSet( Index nr, Indirect& idx );
     SubActiveSet( const SubActiveSet& clone );
 
     inline bool           ownIndirection(void) const { return &self_indirect == &indirect; }
 
   public:
     void                  reset( void );
-    unsigned int          activeRow( unsigned int ref, Bound::bound_t type );
-    inline unsigned int   activeRow( const ConstraintRef& cst ) { return activeRow( cst.row,cst.type ); }
-    void                  unactiveRow( unsigned int row );
-    unsigned int          mapInv( unsigned int row ) const;
-    unsigned int          map( unsigned int ref ) const;
-    Bound::bound_t        whichBoundInv( unsigned int row ) const;
+    Index                 activeRow( Index ref, Bound::bound_t type );
+    inline Index   activeRow( const ConstraintRef& cst ) { return activeRow( cst.row,cst.type ); }
+    void                  unactiveRow( Index row );
+    Index          mapInv( Index row ) const;
+
+    Index          map( Index ref ) const;
+    Bound::bound_t        whichBoundInv( Index row ) const;
     /* For compatibility */
-    inline unsigned int   whichConstraint( unsigned int row ) const { return mapInv(row); }
-    inline unsigned int   where( unsigned int cst ) const { return map(cst); }
-    VectorXi              getIndirection( void ) const;
+    inline Index   whichConstraint( Index row ) const { return mapInv(row); }
+    inline Index   where( Index cst ) const { return map(cst); }
+    Matrix<Index,Dynamic,1>              getIndirection( void ) const;
     void                  disp( std::ostream& os, bool classic=true ) const;
-    inline                operator VectorXi (void) const {  return getIndirection(); }
+    inline                operator Matrix<Index,Dynamic,1> (void) const {  return getIndirection(); }
 
     void                  defrag( void );
     void                  setInitialActivation( const AS& as0 );
@@ -138,8 +324,8 @@ namespace soth
     using AS::            getBoundDamping;
 
   protected: /* Forbidden to avoid ambiguities on which 'row' the arg refers to. */
-    void                  active( unsigned int ref, Bound::bound_t type, unsigned int row );
-    unsigned int          pushIndirectBack( unsigned int rowup );
+    void                  active( Index ref, Bound::bound_t type, Index row );
+    Index          pushIndirectBack( Index rowup );
 
   protected:
     Indirect self_indirect;
@@ -147,7 +333,6 @@ namespace soth
     bool isEmpty;
     using AS::nba;
 
-    typedef typename Indirect::Index Index;
 
   };
 
@@ -160,12 +345,12 @@ namespace soth
   /* --- HEAVY CODE --------------------------------------------------------- */
   template< typename AS,typename Indirect >
   SubActiveSet<AS,Indirect>::
-  SubActiveSet( unsigned int nr )
+  SubActiveSet( Index nr )
     : AS(nr), self_indirect(1),indirect(self_indirect),isEmpty(true) {}
 
   template< typename AS,typename Indirect >
   SubActiveSet<AS,Indirect>::
-  SubActiveSet( unsigned int nr, Indirect& idx )
+  SubActiveSet( Index nr, Indirect& idx )
     : AS(nr), self_indirect(1),indirect(idx),isEmpty(true) {}
 
   template< typename AS,typename Indirect >
@@ -184,16 +369,16 @@ namespace soth
   }
 
   template< typename AS,typename Indirect >
-  unsigned int SubActiveSet<AS,Indirect>::
-  activeRow( unsigned int ref, Bound::bound_t type )
+  typename Indirect::Index SubActiveSet<AS,Indirect>::
+  activeRow( Index ref, Bound::bound_t type )
   {
     assert( (isEmpty&&(nba==0)) || (int(nba) == indirect.size()) );
-    unsigned int rowup = AS::activeRow(ref,type);
+    Index rowup = AS::activeRow(ref,type);
     return pushIndirectBack(rowup);
   }
   template< typename AS,typename Indirect >
-  unsigned int SubActiveSet<AS,Indirect>::
-  pushIndirectBack( unsigned int rowup )
+  typename Indirect::Index SubActiveSet<AS,Indirect>::
+  pushIndirectBack(  typename Indirect::Index rowup )
   {
     assert( rowup<size() );
     if( isEmpty )
@@ -211,43 +396,43 @@ namespace soth
   }
   template< typename AS,typename Indirect >
   void SubActiveSet<AS,Indirect>::
-  unactiveRow( unsigned int rowrm )
+  unactiveRow(  typename Indirect::Index rowrm )
   {
-    assert( int(nba) == indirect.size() );
+    assert( Index(nba) == indirect.size() );
     assert( rowrm<nba ); // nba>0
 
-    const int internalRowrm = indirect(rowrm);
+    const  typename Indirect::Index internalRowrm = indirect(rowrm);
     if( nba==1 )
       { isEmpty = true ; indirect.resize(0); }
     else
       {
-	const Index s = nba-rowrm-1;
+	const  typename Indirect::Index s = nba-rowrm-1;
 	indirect.segment( rowrm,s ) = indirect.tail( s );
 	indirect.conservativeResize( nba-1 );
       }
     AS::unactiveRow(internalRowrm);
   }
   template< typename AS,typename Indirect >
-  unsigned int SubActiveSet<AS,Indirect>::
-  mapInv( unsigned int row ) const
+  typename Indirect::Index SubActiveSet<AS,Indirect>::
+  mapInv(  typename Indirect::Index row ) const
   {
     assert( row<nba );
     return AS::mapInv( indirect(row) );
   }
 
   template< typename AS,typename Indirect >
-  unsigned int SubActiveSet<AS,Indirect>::
-  map( unsigned int cst ) const
+  typename Indirect::Index SubActiveSet<AS,Indirect>::
+  map(  typename Indirect::Index cst ) const
   {
-    const int row_ = AS::map(cst);
-    for( unsigned int row=0;row<nba;++row )
+    const  typename Indirect::Index row_ = AS::map(cst);
+    for(  typename Indirect::Index row=0;row<nba;++row )
       { if( indirect(row)==row_ )  return row; }
     assert( false && "This could not happen." );
     return -1;
   }
   template< typename AS,typename Indirect >
   Bound::bound_t SubActiveSet<AS,Indirect>::
-  whichBoundInv( unsigned int row ) const
+  whichBoundInv(  typename Indirect::Index row ) const
   {
     return whichBound(mapInv(row));
   }
@@ -257,8 +442,8 @@ namespace soth
   setInitialActivation( const AS& as0 )
   {
     assert( &as0!=this );
-    reset(); unsigned int row=0;
-    for( unsigned int i=0;i<as0.size();++i )
+    reset();  typename Indirect::Index row=0;
+    for(  typename Indirect::Index i=0;i<as0.size();++i )
       {
 	if( as0.isActive(i) )
 	  {
@@ -282,9 +467,9 @@ namespace soth
     if( classic )
       {
 	os << " [ ";
-	for( unsigned int row=0;row<nba;++row )
+	for(  typename Indirect::Index row=0;row<nba;++row )
 	  {
-	    const unsigned int cst = mapInv(row);
+	    const  typename Indirect::Index cst = mapInv(row);
 	    if( whichBound(cst) == Bound::BOUND_INF ) os << "-";
 	    else if( whichBound(cst) == Bound::BOUND_SUP ) os << "+";
 	    os <<cst << " ";
@@ -303,13 +488,13 @@ namespace soth
 
   /*: Return a compact of the active line, ordered by row values. */
   template< typename AS,typename Indirect >
-  VectorXi SubActiveSet<AS,Indirect>::
+  Matrix< typename Indirect::Index, Dynamic,1> SubActiveSet<AS,Indirect>::
   getIndirection(void) const
   {
     if (nba==0)
-      return VectorXi();
+      return Matrix<Indirect::Index,Dynamic,1>();
 
-    VectorXi res(nba);
+    Matrix< typename Indirect::Index, Dynamic,1> res(nba);
     for( unsigned int r=0;r<nba;++r )
       {
 	res[r] = mapInv(r);
@@ -321,7 +506,7 @@ namespace soth
 
   template< typename AS,typename Indirect >
   void SubActiveSet<AS,Indirect>::
-  active( unsigned int ref, Bound::bound_t type, unsigned int row )
+  active(  typename Indirect::Index ref, Bound::bound_t type,  typename Indirect::Index row )
   {
     AS::active(ref,type,row);
     pushIndirectBack(row);
@@ -331,7 +516,7 @@ namespace soth
   void SubActiveSet<AS,Indirect>::
   defrag( void )
   {
-    VectorXi csts = *this;
+    Matrix<typename Indirect::Index,Dynamic,1> csts = *this;
     cstref_vector_t cstrefs( csts.size() );
     for( int i=0;i<csts.size();++i )
       { cstrefs[i] = ConstraintRef( csts[i],whichBound(csts[i]) ); }

src/Allocator.cpp

@@ -13,27 +13,27 @@ namespace soth
 
   /* Replace the tokens [min,max[ in the ressource. */
   void AllocatorML::
-  resetTop( unsigned int min )
+  resetTop( Index min )
   {
     assert(min<=max);
     resource.resize( max-min );
-    unsigned int inc = min;
+    Index inc = min;
     for( resource_iter_t iter=resource.begin();iter!=resource.end();++iter )
       { (*iter) = inc++; }
     assert( resource.size() == 0 || resource.back() == max-1 );
   }
 
-  unsigned int AllocatorML::
+  typename AllocatorML::Index AllocatorML::
   get()
   {
     assert( resource.size()>0 );
-    const unsigned int token = resource.front();
+    const Index token = resource.front();
     resource.pop_front();
     return token;
   }
 
   void AllocatorML::
-  put( const unsigned int & token )
+  put( const Index & token )
   {
     resource.push_front(token);
     assert( resource.size()<=max );

src/Allocator.hpp

@@ -6,23 +6,26 @@
 #include <iostream>
 #include <soth/api.hpp>
 
+#include <Eigen/Core>
+
 namespace soth
 {
   class SOTH_EXPORT AllocatorML
   {
-    typedef std::list<unsigned int> resource_t;
+    typedef Eigen::MatrixXd::Index Index;
+    typedef std::list<Index> resource_t;
     typedef resource_t::iterator resource_iter_t;
 
     resource_t resource;
-    unsigned int max;
+    Index max;
 
   public:
-    AllocatorML( unsigned int max ) : resource(),max(max) {}
+    AllocatorML( Index max ) : resource(),max(max) {}
 
     void reset();
-    void resetTop( unsigned int min );
-    unsigned int get();
-    void put( const unsigned int & token );
+    void resetTop( Index min );
+    Index get();
+    void put( const Index & token );
     void disp( std::ostream & os ) const;
 
     SOTH_EXPORT friend std::ostream& operator<<( std::ostream & os, const AllocatorML & aml );

src/BaseY.hpp

@@ -31,7 +31,13 @@ namespace soth
     BaseY( const unsigned int & size );
 
     void computeExplicitly();
-    void reset() { rank=0; isExplicit=false; }
+    void reset() { 
+      rank=0; 
+#ifndef NDEBUG
+      matrixExplicit.setZero();
+#endif
+      isExplicit=false; 
+    }
   public:
     /* --- Accessor --- */
     MatrixXd& getHouseholderEssential() {return householderEssential;}

src/BasicStage.cpp

@@ -22,7 +22,7 @@ namespace soth
   /* --- STAGE -------------------------------------------------------------- */
 
   BasicStage::
-  BasicStage( const unsigned int innr, const unsigned int innc,
+  BasicStage( const Index innr, const Index innc,
 	      const double * Jdata, const Bound * bdata, const BaseY& Y )
     :boundsInternal()
     ,Jmap( Jdata,innr,innc )
@@ -35,7 +35,7 @@ namespace soth
   {}
 
   BasicStage::
-  BasicStage( const unsigned int innr, const unsigned int innc,
+  BasicStage( const Index innr, const Index innc,
 	      const double * Jdata, const BaseY& Y )
     :boundsInternal(innr)
     ,Jmap( Jdata,innr,innc )
@@ -54,7 +54,7 @@ namespace soth