Introduce new struct for array handling inside qp-problem

- replace raw pointer arrays by the new class
- add construction/destruction
- add public methods: fill,resize, stick_together
- add public members array_, id_, nrows_, ncols_

src/ZMPRefTrajectoryGeneration/qp-problem.hh

@@ -29,6 +29,7 @@
 
 #include <jrl/mal/matrixabstractlayer.hh>
 #include <Mathematics/qld.h>
+#include <privatepgtypes.h>
 
 namespace PatternGeneratorJRL
 {
@@ -118,20 +119,20 @@ namespace PatternGeneratorJRL
     /// \brief Set the number of optimization parameters.
     ///
     /// \param nb_variables
-    inline void setNbVariables( int nb_variables )
-    { m_NbVariables = n = nb_variables;};
+    void setNbVariables( int nb_variables )
+    { m_NbVariables = nb_variables;};
 
     /// \brief Set the number of optimization parameters.
     ///
-    /// \param nb_variables
+    /// \param nb_eq_constraints
     inline void setNbEqConstraints( int nb_eq_constraints )
-    { m_NbEqConstraints = me = nb_eq_constraints;};
+    { m_NbEqConstraints = nb_eq_constraints;};
 
     /// \brief Set the number of optimization parameters.
     ///
-    /// \param nb_variables
+    /// \param nb_constraints
     inline void setNbConstraints( int nb_constraints )
-    { m_NbVariables = n = nb_constraints;};
+    { m_NbConstraints = nb_constraints;};
 
     /// \brief Set the dimensions of the problem.
     /// This method has an internal logic to 
@@ -158,7 +159,7 @@ namespace PatternGeneratorJRL
         initialize(NewArray, new_size, (type)0);
         for(int i = 0; i < old_size; i++)
           {
-          NewArray[i] = array[i];
+            NewArray[i] = array[i];
           }
         if (array!=0)
           delete [] array;
@@ -172,18 +173,18 @@ namespace PatternGeneratorJRL
     /// \brief Add a matrix to the final optimization problem in array form
     ///
     /// \param Mat Added matrix
-    /// \param target Target matrix
+    /// \param type Target matrix type
     /// \param row First row inside the target
     /// \param col First column inside the target
     void addTerm(const MAL_MATRIX (&Mat, double), int type,
-		 unsigned int row, unsigned int col);
+		 int row, int col);
     /// \brief Add a vector to the final optimization problem in array form
     ///
     /// \param Mat Added vector
-    /// \param target Target vector type
+    /// \param ype Target vector type
     /// \param row First row inside the target
     void addTerm(const MAL_VECTOR (&Vec, double), int type,
-		 unsigned int row);
+		 int row);
 
     /// \name Dumping functions
     /// \{
@@ -199,7 +200,7 @@ namespace PatternGeneratorJRL
     /// \param type
     /// \param filename
     void dump( int type, const char *filename);
-    void dump( int type, std::ostream &);
+    void dump( int type, std::ostream & aos);
     /// \}
 
     /// \brief Initialize array
@@ -209,6 +210,9 @@ namespace PatternGeneratorJRL
     template <class type> void initialize(type * array, int size, type value)
     { std::fill_n(array,size,value); }
 
+    /// \brief Initialize arrays
+    void clear( );
+
     /// \brief Initialize whole array
     ///
     /// \param[in] type
@@ -242,9 +246,92 @@ namespace PatternGeneratorJRL
     /// \brief Allocate memory.
     /// Called when setting the dimensions of the problem.
     ///
-    /// \param nb_variables
-    /// \param nb_constraints
-    void resizeAll( int nb_variables, int nb_constraints);
+    /// \param[in] nb_variables
+    /// \param[in] nb_constraints
+    void resizeAll();
+
+    //
+    //Private types
+    //
+  private:
+
+    /// \brief Handle matrices/vectors in array form
+    template<class type>
+    struct array_s
+    {
+      type * array_;
+
+      int id_;
+      int nrows_, ncols_;
+
+      void fill( type value)
+      { std::fill_n(array_, nrows_*ncols_, value); }
+
+      void fill( type * array, int size, type value)
+      { std::fill_n(array, size, value); }
+
+
+      /// \brief Make a contiguous array
+      ///
+      /// \param[in] nrows Size of the new array
+      /// \param[in] ncols Size of the new array
+      /// \param[in] preserve Preserve old values
+      /// \return 0
+      int stick_together(type *& final_array, int nrows, int ncols)
+      {
+        try {
+          type * NewArray = new type[nrows*ncols];
+          fill(NewArray, nrows*ncols, (type)0);
+            for(int i = 0; i < nrows; i++)
+              for(int j = 0; j < ncols; j++)
+                NewArray[i+nrows*j] = array_[i+nrows_*j];
+          if (final_array!=0)
+            delete [] final_array;
+
+          final_array = NewArray;
+          nrows_ = nrows;
+          ncols_ = ncols;
+        }
+        catch (std::bad_alloc& ba)
+        {std::cerr << "bad_alloc caught: " << ba.what() << std::endl; }
+
+        return 0;
+      }
+
+      /// \brief Resize array
+      ///
+      /// \param[in] nrows Size of the new array
+      /// \param[in] ncols Size of the new array
+      /// \param[in] preserve Preserve old values
+      /// \return 0
+      int resize(int nrows, int ncols, bool preserve)
+      {
+        try {
+          type * NewArray = new type[nrows*ncols];
+          fill(NewArray, nrows*ncols, (type)0);
+          if(preserve) {
+            for(int i = 0; i < nrows_; i++)
+              for(int j = 0; j < ncols_; j++)
+                NewArray[i+nrows*j] = array_[i+nrows_*j]; }
+          if (array_!=0)
+            delete [] array_;
+
+          array_ = NewArray;
+          nrows_ = nrows;
+          ncols_ = ncols;
+        }
+        catch (std::bad_alloc& ba)
+        {std::cerr << "bad_alloc caught: " << ba.what() << std::endl; }
+
+        return 0;
+      }
+
+      array_s():
+        array_(0),id_(0),nrows_(0),ncols_(0){
+      };
+      ~array_s()
+      {  if (array_!=0) delete [] array_;};
+    };
 
     //
     //Private members
@@ -254,8 +341,8 @@ namespace PatternGeneratorJRL
     /// \name ql-parameters
     /// \{
     int m, me, mmax, n, nmax, mnn;
-    double *Q, *D, *DU, *DS, *XL, *XU, *X, *U, *war;
-    int *iwar;
+    array_s<double> Q_, Q_dense_, D_, DU_, DU_dense_, DS_, XL_, XU_, X_, U_, war_;
+    array_s<int> iwar_;
     int iout, ifail, iprint, lwar, liwar;
     double Eps;
     /// \}
@@ -275,6 +362,12 @@ namespace PatternGeneratorJRL
 //    /// \brief Primal Least square Distance Problem solver
 //    Optimization::Solver::PLDPSolverHerdt * m_PLDPSolverHerdt;
 
+    int scale_factor_;
+
+    /// \brief Maximal number of variables
+    int max_var_;
+    /// \brief Maximal number of constraints
+    int max_constr_;
   };
   typedef struct QPProblem_s QPProblem;
 }