controller_misc.h

Go to the documentation of this file.

/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 *                                                                         *
 *   $Log: controller_misc.h,v $
 *   Revision 1.9.6.3  2006/01/17 16:58:39  martius
 *   loading and storing
 *
 *   Revision 1.9.6.2  2005/12/15 17:04:39  martius
 *   min, max and so on are template functions now
 *
 *   Revision 1.9.6.1  2005/11/15 12:30:27  martius
 *   new selforg structure and OdeAgent, OdeRobot ...
 *
 *   Revision 1.9  2005/11/10 09:13:33  martius
 *   readded defines like min, max sqr,...
 *
 *   Revision 1.8  2005/10/27 14:17:17  martius
 *   some functions are now found in mathutils.h (dir: ../ode_robots/utils)
 *
 *   Revision 1.7  2005/10/21 11:50:33  martius
 *   adapt functions
 *   random number function for usage with Matrix::map
 *
 *   Revision 1.6  2005/10/06 17:06:57  martius
 *   switched to stl lists
 *
 *   Revision 1.5  2005/09/22 10:33:07  martius
 *   max and min more save
 *
 *   Revision 1.4  2005/08/29 09:05:31  fhesse
 *   removed weird bug in get4x4AndDiagonalSize, probably caused by some compiler optimization
 *
 *   Revision 1.3  2005/08/06 20:47:54  martius
 *   Commented
 *
 *   Revision 1.2  2005/07/29 14:26:56  martius
 *   fixed bug in length calculation of 4x4 diagonal routines
 *
 *   Revision 1.1  2005/07/28 11:14:52  martius
 *   helper functions for controller
 *    support for internal parameters more easy
 *
 *
 ***************************************************************************/
#ifndef __CONTOLLER_MISC_H
#define __CONTOLLER_MISC_H

#include <matrix.h>
using namespace matrix;
#include <assert.h>
#include <stdlib.h>
#include <math.h>

#include "stl_adds.h"
#include "noisegenerator.h"
#include "inspectable.h"

template<typename T>
inline T sign(T v) { return v<(T)0?(T)-1:(T)1; }

template<typename T>
inline T sqr(T v) { return v*v; }

template<typename T>
inline T clip(T x, T lobound, T highbound) { 
  return (x)<(lobound) ? (lobound) : ( (x) > (highbound) ? (highbound) : (x) ); 
}


/// calculates 1/x
double one_over(double x);

/// creates random number from -1 to 1
double random_minusone_to_one(double);

/* 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);

/* 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);

/* 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);

/* 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);

/* 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);

/* 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);

/* 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);

/* 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);

/* 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);

/** stores the Matrix into the given file stream (binary)
 */
bool storeMatrix(const Matrix& m, FILE* f);

/** reads a Matrix from the given file stream (binary)
 */
bool restoreMatrix(Matrix& m, FILE* f);

Matrix noiseMatrix(unsigned int m, unsigned int n, NoiseGenerator& ng, double p1, double p2);

/// 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);

/// 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);

#endif

---