controller_misc.cpp

Go to the documentation of this file.

#include "controller_misc.h"

/// calculates 1/x
double one_over(double x){
  return 1/x;
}

/// creates random number from -1 to 1
double random_minusone_to_one(double){
  return (((double)rand() / RAND_MAX) - 0.5) * 2.0;
}

/* stores at least left top 4x4 submatrix (row-wise) (if exists) and 
   then the rest of the diagonal elements into a list
   @return list of values
*/
list<D> store4x4AndDiagonal(const Matrix& m){
  list<D> l;
  unsigned short smalldimM = min(m.getM(), (unsigned short)4);
  unsigned short smalldimN = min(m.getN(), (unsigned short)4);
  unsigned short smallerdim = min(m.getM(), m.getN());
  // 4x4
  for(unsigned short i=0; i < smalldimM; i++){
    for(unsigned short j=0; j < smalldimN; j++){
      l.push_back(m.val(i,j));
    }
  }
  // diagonal below 4x4
  for(unsigned short i=4; i < smallerdim; i++){
    l.push_back(m.val(i,i));
  }
  return l;  
}

/* stores at least left top 4x4 submatrix (row-wise) (if exists) and 
   then the rest of the diagonal elements
  @param len Length of the provided buffer 
  (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 
  @return number of actually written elements
*/
unsigned int store4x4AndDiagonal(const Matrix& m, D* buffer, unsigned int len){
  unsigned short smalldimM = min(m.getM(), (unsigned short)4);
  unsigned short smalldimN = min(m.getN(), (unsigned short)4);
  unsigned short smallerdim = min(m.getM(), m.getN());
  unsigned int written=0;
  assert(len >= unsigned(smalldimM * smalldimN + max(0, signed(smallerdim) - 4)));
  // 4x4
  for(unsigned short i=0; i < smalldimM; i++){
    for(unsigned short j=0; j < smalldimN; j++){
      buffer[written]=m.val(i,j);
      written++;
    }
  }
  // diagonal below 4x4
  for(unsigned short i=4; i < smallerdim; i++){
    buffer[written]=m.val(i,i);
    written++;
  }
  return written;  
}


/* returns the number of elements stored by store4x4AndDiagonal
  (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 
*/
unsigned int get4x4AndDiagonalSize(const Matrix& m){
  unsigned short smalldimM = min(m.getM(), (unsigned short)4);
  unsigned short smalldimN = min(m.getN(), (unsigned short)4);
  unsigned short smallerdim = min(m.getM(), m.getN());
  unsigned short sm = unsigned(max(0, signed(smallerdim) - 4));
  return smalldimM * smalldimN + sm;
}


/* writes the names of the fields stored by store4x4AndDiagonal into a list
  @param matrixName name of the matrix (prefix for all fields)
  @param keylist list for field names
  @param len Length of the provided buffer 
  (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 
  @return number of actually written elements
*/
list<Inspectable::iparamkey> store4x4AndDiagonalFieldNames(const Matrix& m, const char* matrixName){
  list<Inspectable::iparamkey> l;
  char buffer[32];
  unsigned short smalldimM = min(m.getM(), (unsigned short)4);
  unsigned short smalldimN = min(m.getN(), (unsigned short)4);
  unsigned short smallerdim = min(m.getM(), m.getN());
  assert(matrixName);
  // 4x4
  for(unsigned short i=0; i < smalldimM; i++){
    for(unsigned short j=0; j < smalldimN; j++){
      sprintf(buffer,"%s[%d,%d]",matrixName,i,j); 
      l.push_back(string(buffer));
    }
  }
  // diagonal below 4x4
  for(unsigned short i=4; i < smallerdim; i++){
    sprintf(buffer,"%s[%d,%d]",matrixName,i,i); 
    l.push_back(string(buffer));
  }
  return l;  
}

/* stores the names of the fields stored by store4x4AndDiagonal 
  @param matrixName name of the matrix (prefix for all fields)
  @param keylist list for field names
  @param len Length of the provided buffer 
  (should be min(getN(),4)*min(getM(),4)+ max(0,min(getM()-4,getN()-4))) 
  @return number of actually written elements
*/
unsigned int store4x4AndDiagonalFieldNames(const Matrix& m, const char* matrixName, 
                                           char** keylist, unsigned int len){
  unsigned short smalldimM = min(m.getM(), (unsigned short)4);
  unsigned short smalldimN = min(m.getN(), (unsigned short)4);
  unsigned short smallerdim = min(m.getM(), m.getN());
  unsigned int written=0;
  assert(len >= get4x4AndDiagonalSize(m));
  assert(matrixName);
  unsigned char keyLen = strlen(matrixName)+10;
  // 4x4
  for(unsigned short i=0; i < smalldimM; i++){
    for(unsigned short j=0; j < smalldimN; j++){
      keylist[written] = (char*) malloc(keyLen);
      sprintf(keylist[written],"%s[%d,%d]",matrixName,i,j); 
      written++;
    }
  }
  // diagonal below 4x4
  for(unsigned short i=4; i < smallerdim; i++){
    keylist[written] = (char*) malloc(keyLen);
    sprintf(keylist[written],"%s[%d,%d]",matrixName,i,i); 
    written++;
  }
  return written;  
}

/* stores the names of the all matrix fieldnames produces by convertToBuffer into a list
  @return list of names
*/
list<Inspectable::iparamkey> storeMatrixFieldNames(const Matrix& m, const char* matrixName){
  list<Inspectable::iparamkey> l;
  char buffer[32];
  unsigned int dimM = m.getM();
  unsigned int dimN = m.getN();
  assert(matrixName);
  for(unsigned short i=0; i < dimM; i++){
    for(unsigned short j=0; j < dimN; j++){
      sprintf(buffer,"%s[%d,%d]",matrixName,i,j); 
      l.push_back(string(buffer));
    }
  }
  return l;  
}

/* stores the names of the all vector (mx1 matrix) fieldnames  produces by convertToBuffer into a list
  @return list of names
*/
list<Inspectable::iparamkey> storeVectorFieldNames(const Matrix& m, const char* vectorName){
  list<Inspectable::iparamkey> l;
  char buffer[32];
  unsigned int dimM = m.getM();
  assert(vectorName);
  assert(m.getN()==1);
  for(unsigned short i=0; i < dimM; i++){
      sprintf(buffer,"%s[%d]",vectorName,i); 
      l.push_back(string(buffer));
  }
  return l;  
}

/* stores the names of the all matrix fieldnames produces by convertToBuffer
  @param matrixName name of the matrix (prefix for all fields)
  @param keylist list for field names
  @param len Length of the provided buffer 
  (should be getN()*getM()
  @return number of actually written elements
*/
unsigned int storeMatrixFieldNames(const Matrix& m, const char* matrixName, 
                                   char** keylist, unsigned int len){
  unsigned int dimM = m.getM();
  unsigned int dimN = m.getN();
  unsigned int written=0;
  assert(matrixName);
  assert(len >= dimM*dimN);
  unsigned char keyLen = strlen(matrixName)+10;
  for(unsigned short i=0; i < dimM; i++){
    for(unsigned short j=0; j < dimN; j++){
      keylist[written] = (char*) malloc(keyLen);
      sprintf(keylist[written],"%s[%d,%d]",matrixName,i,j); 
      written++;
    }
  }
  return written;  
}

/* stores the names of the all vector (mx1 matrix) fieldnames produces by convertToBuffer
  @param vectorName name of the vector (prefix for all fields)
  @param keylist list for field names
  @param len Length of the provided buffer (should be getM())
  @return number of actually written elements
*/
unsigned int storeVectorFieldNames(const Matrix& m, const char* vectorName, 
                                   char** keylist, unsigned int len){
  unsigned int dimM = m.getM();
  unsigned int written=0;
  assert(vectorName);
  assert(m.getN()==1);
  assert(len >= dimM);
  unsigned char keyLen = strlen(vectorName)+5;
  for(unsigned short i=0; i < dimM; i++){
      keylist[written] = (char*) malloc(keyLen);
      sprintf(keylist[written],"%s[%d]",vectorName,i); 
      written++;
  }
  return written;  
}


/** stores the Matrix into the given file stream (binary)
 */
bool storeMatrix(const Matrix& m, FILE* f){
  int dim[2] = { m.getM(), m.getN() };
  unsigned int len = dim[0] * dim[1];
  D* buffer = (D*)malloc(len * sizeof(D));
  m.convertToBuffer(buffer, len);
  bool rval=false;
  if(fwrite(dim, sizeof(int), 2, f) == 2) 
    if(fwrite(buffer, sizeof(D), len, f) == len)       
      rval=true;
  free(buffer);
  return rval;
}

/** reads a Matrix from the given file stream (binary)
 */
bool restoreMatrix(Matrix& m, FILE* f){
  int dim[2];
  bool rval = false;
  if(fread(dim, sizeof(int), 2, f) == 2){
    if(dim[0] == m.getM() && dim[1] == m.getN()) {
      unsigned int len = dim[0] * dim[1];
      D* buffer = (D*)malloc(len * sizeof(D));
      if(fread(buffer, sizeof(D), len, f) == len) {  
        m.set(buffer);
        rval = true;
      }else fprintf(stderr, "cannot read matrix"); 
      free(buffer);
    }else{
      fprintf(stderr, "restoreMatrix: dimension is wrong: expected (%i,%i), got (%i,%i)", 
              m.getM(), m.getN(), dim[0], dim[1]);
    }
  }    
  return rval;
}

Matrix noiseMatrix(unsigned int m, unsigned int n, NoiseGenerator& ng, double p1, double p2){
  int len = m*n;
  D* noise = (D*) malloc(len*sizeof(D));
  memset(noise, D_Zero, sizeof(D)*len);
  ng.add(noise, p1, p2);
  Matrix result(m, n, noise);
  free(noise);
  return result;
}

/// parameter adaptation algorithm. 
//   @param p current parameter value
//   @param actual actual value of some size controlled by p
//   @param nominal nominal value of some size controlled by p
//   @param up_rate adaptation rate for increasing p (<< 1)
//   @param down_rate adaptation rate for decreasing p (<< 1)
//   @return new value of p (there is no clipping done)
double adapt(double p, double actual, double nominal, double up_rate, double down_rate){
  double result;
  // use a different rate for decreasing and increasing
  result = p * ((actual - nominal) > 0 ? (1.0 - down_rate) : (1.0 + up_rate));
  return result;
}

/// like adapt but that the adaption is just done if the actual value is outside the given interval (_min, _max)
double adaptMinMax(double p, double actual, double _min, double _max, double up_rate, double down_rate){
  double result=p;
  if(actual < _min){
    //    printf("Under: %g, eps: %g\n", updateSize, eps);
    result = p * (1+up_rate);
  } else if(actual > _max){
    //    printf("Over : %g, eps: %g\n", updateSize, eps);
    result = p * (1-down_rate);
  }  
  return result;
}

---