Tools/PotentialFields/PotentialFunctions.h

Go to the documentation of this file.

/**
* @file PotentialFunctions.h
* 
* Definition of several functions used by potential fields
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/

#ifndef POTENTIAL_FUNCTIONS_H_
#define POTENTIAL_FUNCTIONS_H_


#include <cmath>
#include <cassert>

class PfPose;
class PfVec;
class Sector;
class Polygon;
class PfieldGeometricObject;


/** The object types */
enum ObjectType {ATTRACTIVE, REPULSIVE};

/** The function types */
enum FunctionType {NO_FUNCTION, LINEAR_FUNCTION, PARABOLIC_FUNCTION, ASYMPTOTIC_FUNCTION};

/** The field types */
enum FieldType {POINT_FIELD, SHAPE_FIELD, SECTOR_FIELD};


/**
* @class PotentialfieldFunction
*
* An abstract class representing a function in a potential field
*/
class PotentialfieldFunction
{
public:
  /** Computes the value of the function for a given x 
  * including range checking and smoothing
  * @param x The distance for which to compute the value of the function
  * @param smooth Flag: Use smoothing, if true
  * @return The value of the function
  */
  virtual double computeValue(double x, bool smooth=true);

  /** Computes the value of the derivative of the function for a given x
  * including range checking and smoothing
  * @param x The distance for which to compute the value of the function
  * @param smooth Flag: Use smoothing, if true
  * @return The value of the function
  */
  virtual double computeDerivativeValue(double x, bool smooth=false);

  /** Clones a function object
  * @return A pointer to a copy of the object
  */
  virtual PotentialfieldFunction* clone() = 0;

  /** Returns the range of the function
  * @return The range
  */
  double getRange()
  { return range;}

  /** Sets the two main parameters, recomputes smoothing parameters
  * @param atZero The value of f(0)
  * @param range At range, f(range) is 0
  */
  void setParameters(double atZero, double range)
  { 
    this->atZero = atZero; 
    this->range = range;
    smoothingAtObjectPosition = smoothingAtObjectPercentage*range; 
    smoothingAtBorderPosition = range - smoothingAtBorderPercentage*range;
    init();
  }
  
  /** Sets parameters for smoothing at the end of the function's interval
  *   Both values have a range from 0.0 to 1.0
  * @param smoothingAtObject Smoothing near the object
  * @param smoothingAtBorder Smoothing near the end of the range
  * @param gradientAtObject The gradient at the object
  * @param gradientAtBorder The gradient at (range,0)
  */
  void setSmoothingParameters(double smoothingAtObject, 
                              double smoothingAtBorder,
                              double gradientAtObject,
                              double gradientAtBorder)
  { 
    this->gradientAtObject = gradientAtObject;
    this->gradientAtBorder = gradientAtBorder;
    smoothingAtObjectPercentage = smoothingAtObject;
    smoothingAtBorderPercentage = smoothingAtBorder;
    smoothingAtObjectPosition = smoothingAtObject*range; 
    smoothingAtBorderPosition = range - smoothingAtBorder*range;
  }

protected:
  /** Last position of smoothing near object*/
  double smoothingAtObjectPosition;
  /** First position of smoothing near the end of the range*/
  double smoothingAtBorderPosition;
  /** The gradient at the object (used together with smoothing)*/
  double gradientAtObject;
  /** The gradient at (range,0) (used together with smoothing)*/
  double gradientAtBorder;
  /** Smoothing near object [0.0, .., 1.0] */
  double smoothingAtObjectPercentage;
  /** Smoothing near the end of the range [0.0, .., 1.0] */
  double smoothingAtBorderPercentage;
  /** The value of f(0) */
  double atZero;
  /** f(range) = 0 */
  double range;
  /** Internal variable for faster computation set by init()*/
  double a;
  /** Internal variable for faster computation set by init()*/
  double b;

  /** Generates a smooth curve between two points and computes function value
  *   of a given x.
  * @param x1 x component of the left end of the curve
  * @param fx1 y component of the left end of the curve
  * @param dx1 Gradient at (x1,fx1)
  * @param x2 x component of the right end of the curve
  * @param fx2 y component of the right end of the curve
  * @param dx2 Gradient at (x2,fx22)
  * @param x The position for which f(x) is computed
  * @return A smoothed f(x)
  */
  double computeSmoothedValue(double x1, double fx1, double dx1,
                              double x2, double fx2, double dx2,
                              double x);

  /** Computes the value of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  virtual double f(double x) const 
  { return 0.0;}
  
  /** Computes the value of the derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  virtual double d(double x) const 
  { return 0.0;}

  /** Computes the value of the second derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  virtual double dd(double x) const 
  { return 0.0;}

  /** Computes the values of a and b*/
  virtual void init() {}

  /** Sets standard parameters from other function
  * @param other The other function
  */
  void getStandardParameters(PotentialfieldFunction* other)
  { this->smoothingAtObjectPosition = other->smoothingAtObjectPosition;
    this->smoothingAtBorderPosition = other->smoothingAtBorderPosition;
    this->gradientAtObject = other->gradientAtObject;
    this->gradientAtBorder = other->gradientAtBorder;
    this->smoothingAtObjectPercentage = other->smoothingAtObjectPercentage;
    this->smoothingAtBorderPercentage = other->smoothingAtBorderPercentage;}
};


/**
* @class LinearFunction
*
* A class representing a linear function in a potential field
* f(x) = a*x +b
*/
class LinearFunction: public PotentialfieldFunction
{
public:
  /**
  * Constructor
  * @param range The range of the function
  * @param atZero The value of the function for x having the value 0
  */
  LinearFunction(double range, double atZero)
  {
    this->atZero = atZero;
    this->range = range;
    init();
  }

  /** Clones a function object
  * @return A pointer to a copy of the object
  */
  virtual PotentialfieldFunction* clone()
  {
    LinearFunction* newFunction = new LinearFunction(range,atZero);
    newFunction->getStandardParameters(this);
    return newFunction;
  }

protected:
  /** Computes the value of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  double f(double x) const
  { 
    return (a*x + b);
  }

  /** Computes the value of the derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  double d(double x) const
  {
    return a;
  }

  /** Computes the value of the second derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  virtual double dd(double x) const
  {
    return 0.0;
  }

  /** Computes the values of a and b*/
  virtual void init() 
  {
    assert(range != 0.0);
    a = -atZero/range;
    b = atZero;
  }
};


/**
* @class ParabolicFunction
*
* A class representing a parabolic function in a potential field
* f(x) = a*x*x +b
*/
class ParabolicFunction: public PotentialfieldFunction
{
public:
  /**
  * Constructor
  * @param range The range of the function
  * @param atZero The value of the function for x having the value 0
  */
  ParabolicFunction(double range, double atZero)
  {
    this->atZero = atZero;
    this->range = range;
    init();
  }

  /** Clones a function object
  * @return A pointer to a copy of the object
  */
  virtual PotentialfieldFunction* clone()
  {
    ParabolicFunction* newFunction = new ParabolicFunction(range,atZero);
    newFunction->getStandardParameters(this);
    return newFunction;
  }

protected:
  /** Computes the value of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  double f(double x) const 
  { 
    return (a*x*x + b);
  }

  /** Computes the value of the derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  double d(double x) const
  {
    return (2.0*a*x);
  }

  /** Computes the value of the second derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  virtual double dd(double x) const
  {
    return (2.0*a);
  }

  /** Computes the values of a and b*/
  virtual void init() 
  {
    assert(range != 0.0);
    a = -atZero/(range*range);
    b = atZero;
  }
};


/**
* @class AsymptoticFunction
*
* A class representing an asymptotic function in a potential field
* f(x) = a/x +b
*/
class AsymptoticFunction: public PotentialfieldFunction
{
public:
  /**
  * Constructor
  * @param range The range of the function
  * @param atZero The value of the function for x having the value 0
  * @param solidCenter f(x) = atZero for x < solidCenter 
  */
  AsymptoticFunction(double range, double atZero, double solidCenter)
  {
    this->atZero = atZero;
    this->range = range;
    this->solidCenter = solidCenter;
    init();
   }

  /** Clones a function object
  * @return A pointer to a copy of the object
  */
  virtual PotentialfieldFunction* clone()
  {
    AsymptoticFunction* newFunction = new AsymptoticFunction(range,atZero,solidCenter);
    newFunction->getStandardParameters(this);
    return newFunction;
  }

protected:
  /** f(x) = atZero for x <= solidCenter */
  double solidCenter;

  /** Computes the value of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  double f(double x) const
  { 
    if(x <= solidCenter)
    {
      return atZero;
    }
    else
    {
      return (a/x + b);
    }
  }

  /** Computes the value of the derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  double d(double x) const
  {
    if(x <= solidCenter)
    {
      return (-a/solidCenter*solidCenter);
    }
    else
    {
      return (-a/(x*x));
    }
  }

  /** Computes the value of the second derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @return The value of the function
  */
  virtual double dd(double x) const
  {
    return (2.0*a/(x*x*x));
  }

  /** Computes the values of a and b*/
  virtual void init() 
  {
    assert((range != 0.0) && (solidCenter != 0.0) && (range != solidCenter));
    a = atZero/(1.0/solidCenter - 1.0/range);
    b = -a/range;
  }
};


/**
* @class SocialFunction
*
* A class representing a social function in a potential field
*/
class SocialFunction: public PotentialfieldFunction
{
public:
  /**
  * Constructor
  * @param repC A repulsive constant
  * @param repOm A repulsive quotient
  * @param attC An attractive constant
  * @param attOm An attractive quotient
  * @param epsilon The minimum possible x
  * @param k A constant value for value computation
  */
  SocialFunction(double repC, double repOm, double attC, double attOm,
                 double epsilon, double k)
  {
    this->repC = repC;
    this->repOm = repOm;
    this->attC = attC;
    this->attOm = attOm;
    this->epsilon = epsilon;
    this->k = k;
  }

  /** Computes the value of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @param smooth flag, not used here
  * @return The value of the function
  */
  virtual double computeValue(double x, bool smooth=true)
  { 
    if(x < epsilon)
    {
      x = epsilon;
    }
    double repValue;
    double attValue;
    if(repOm == 1.0)
    {
      repValue = repC * log(x);
    }
    else
    {
      repValue = (repC / (-repOm + 1.0)) * pow(x, -repOm+1.0);
    }
    if(attOm == 1.0)
    {
      attValue = attC * log(x);
    }
    else
    {
      attValue = (attC / (-attOm + 1.0)) * pow(x, -attOm+1.0);
    }
    return (-repValue + attValue + k);
  }

  /** Computes the value of the derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @param smooth flag, not used here
  * @return The value of the function
  */
  double computeDerivativeValue(double x, bool smooth=false)
  {
    if(x < epsilon)
    {
      x = epsilon;
    }
    return ((-repC/pow(x,repOm))+(attC/pow(x,attOm)));
  }

  /** Clones a function object
  * @return A pointer to a copy of the object
  */
  virtual PotentialfieldFunction* clone()
  {
    SocialFunction* newFunction = new SocialFunction(repC,repOm,attC,attOm, epsilon, k);
    newFunction->getStandardParameters(this);
    return newFunction;
  }

private:
  /** A repulsive constant */
  double repC;
  /** A repulsive quotient */
  double repOm;
  /** An attractive constant */
  double attC;
  /** An attractive quotient */
  double attOm;
  /** The minimum possible x*/
  double epsilon;
  /** A constant value for value computation*/
  double k;
};


/**
* @class NoFunction
*
* A class representing an empty function
*/
class NoFunction: public PotentialfieldFunction
{
public:
  /** Computes the value of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @param smooth flag, not used here
  * @return The value of the function
  */
  double computeValue(double x, bool smooth=true)
  { 
    return 0.0;
  }

  /** Computes the value of the derivative of the function for a given x
  * @param x The distance for which to compute the value of the function
  * @param smooth flag, not used here
  * @return The value of the function
  */
  double computeDerivativeValue(double x, bool smooth=false)
  {
    return 0.0;
  }

  /** Clones a function object
  * @return A pointer to a copy of the object
  */
  virtual PotentialfieldFunction* clone()
  {
    NoFunction* newFunction = new NoFunction();
    return newFunction;
  }
};


/** Computes the charge at a position in a Pointfield
* @param objectPose The pose of the object to which the field belongs
* @param testedPose The pose for which the charge is computed
* @param function The function
* @return The charge
*/
double computeChargeForPointfield(const PfPose& objectPose, const PfPose& testedPose,
                                  PotentialfieldFunction* function);


/** Computes the charge at a position in a Shapefield
* @param objectPose The pose of the object to which the field belongs
* @param testedPose The pose for which the charge is computed
* @param function The function
* @param geometry The geometric shape of the object
* @return The charge
*/
double computeChargeForShapefield(const PfPose& objectPose, const PfPose& testedPose,
                                  PotentialfieldFunction* function, 
                                  PfieldGeometricObject* geometry);

/** Computes the charge at a position in a Sectorfield
* @param objectPose The pose of the object to which the field belongs
* @param testedPose The pose for which the charge is computed
* @param function The function
* @param sector The sector of the field
* @return The charge
*/
double computeChargeForSectorfield(const PfPose& objectPose, const PfPose& testedPose,
                                   PotentialfieldFunction* function, 
                                   const Sector& sector);

/** Computes the gradient at a position in a Pointfield
* @param objectPose The pose of the object to which the field belongs
* @param testedPose The pose for which the charge is computed
* @param function The function
* @return The gradient
*/
PfVec computeGradientForPointfield(const PfPose& objectPose, const PfPose& testedPose,
                                   PotentialfieldFunction* function);

/** Computes the gradient at a position in a Shapefield
* @param objectPose The pose of the object to which the field belongs
* @param testedPose The pose for which the charge is computed
* @param function The function
* @param geometry The geometric shape of the object
* @param objectType The type of the object
* @return The gradient
*/
PfVec computeGradientForShapefield(const PfPose& objectPose, const PfPose& testedPose,
                                   PotentialfieldFunction* function, 
                                   PfieldGeometricObject* geometry,
                                   ObjectType objectType);

/** Computes the gradient at a position in a Sectorfield
* @param objectPose The pose of the object to which the field belongs
* @param testedPose The pose for which the charge is computed
* @param function The function
* @param sector The sector of the field
* @return The gradient
*/
PfVec computeGradientForSectorfield(const PfPose& objectPose, const PfPose& testedPose,
                                    PotentialfieldFunction* function, 
                                    const Sector& sector);

                                    
#endif //POTENTIAL_FUNCTIONS_H_

---