diff --git a/src/ZMPRefTrajectoryGeneration/qp-problem.hh b/src/ZMPRefTrajectoryGeneration/qp-problem.hh
index 53bad913039413092547de957e5009ea664555e3..d028a65151d332e5382d892abcb7a35bb0c9ec08 100644
--- a/src/ZMPRefTrajectoryGeneration/qp-problem.hh
+++ b/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;
}