Tools/PotentialFields/PotentialFunctions.cpp

Go to the documentation of this file.

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

#include "PotentialFunctions.h"
#include "PfieldDatatypes.h"
#include "PfieldGeometry.h"



double PotentialfieldFunction::computeValue(double x, bool smooth)
{
  if(x >= range)
  {
    return 0.0;
  }
  else if(smooth && (x > smoothingAtBorderPosition))
  {
    return computeSmoothedValue(smoothingAtBorderPosition, f(smoothingAtBorderPosition),
                                d(smoothingAtBorderPosition), range, 0.0, gradientAtBorder, x);
  }
  else if(smooth && (x < smoothingAtObjectPosition))
  {
    return computeSmoothedValue(0.0, atZero, gradientAtObject,
                                smoothingAtObjectPosition, f(smoothingAtObjectPosition),
                                d(smoothingAtObjectPosition), x);
  }
  else
  {
    return f(x);
  }
}
 

double PotentialfieldFunction::computeDerivativeValue(double x, bool smooth)
{
  if(x >= range)
  {
    return 0.0;
  }
  else if(smooth && (x > smoothingAtBorderPosition))
  {
    return computeSmoothedValue(smoothingAtBorderPosition, d(smoothingAtBorderPosition),
                                dd(smoothingAtBorderPosition), range, 0.0, 0.0, x);
  }
  else if(smooth && (x < smoothingAtObjectPosition))
  {
    return computeSmoothedValue(0.0, atZero, 0.0,
                                smoothingAtObjectPosition, d(smoothingAtObjectPosition),
                                dd(smoothingAtObjectPosition), x);
  }
  else
  {
    return d(x);
  }
}


double PotentialfieldFunction::computeSmoothedValue
                              (double x1, double fx1, double dx1,
                               double x2, double fx2, double dx2, double x)
{
  double denominator(x2*x2*x2 + 3*x1*x2*(x1-x2) - x1*x1*x1);
  double a(dx2*(x2-x1) + dx1*(x2-x1) - 2*fx2 + 2*fx1);
  a /= denominator;
  double b((dx2 - dx1 - 3*a*(x2*x2-x1*x1))/(2*(x2-x1)));
  double c(dx1 - 3*a*x1*x1 - 2*b*x1);
  double d(fx1 - a*x1*x1*x1 - b*x1*x1 - c*x1);
  return (a*x*x*x + b*x*x + c*x +d);
}


double computeChargeForPointfield(const PfPose& objectPose, const PfPose& testedPose,
                                  PotentialfieldFunction* function)
{
  double x = objectPose.pos.distanceTo(testedPose.pos);
  return function->computeValue(x);
}


double computeChargeForShapefield(const PfPose& objectPose, const PfPose& testedPose,
                                  PotentialfieldFunction* function, 
                                  PfieldGeometricObject* geometry)
{
  PfVec dummy;
  double x = geometry->distanceTo(objectPose, testedPose.pos, dummy);
  if(x == 0.0)
  {
    return function->computeValue(x, false);
  }
  else
  {
    return function->computeValue(x);
  }
}


double computeChargeForSectorfield(const PfPose& objectPose, const PfPose& testedPose,
                                   PotentialfieldFunction* function, 
                                   const Sector& sector)
{
  double dist = objectPose.pos.distanceTo(testedPose.pos);
  if((dist > function->getRange()) ||
     !sector.pointInside(objectPose, testedPose.pos))
  {
    return 0.0;
  }
  double distValue = function->computeValue(dist);
  double x = fabs(objectPose.getAngleTo(testedPose.pos));
  sector.crossFunction->setParameters(distValue, sector.openingAngle/2.0);
  return sector.crossFunction->computeValue(x);
}


PfVec computeGradientForPointfield(const PfPose& objectPose, const PfPose& testedPose,
                                   PotentialfieldFunction* function)
{
  PfVec vecToObject = (objectPose.pos - testedPose.pos);
  double x = vecToObject.length();
  vecToObject.normalize();
  PfVec result(0.0,0.0);
  double gradient = function->computeDerivativeValue(x);
  result = (vecToObject * gradient);
  return result;
}


PfVec computeGradientForShapefield(const PfPose& objectPose, const PfPose& testedPose,
                                   PotentialfieldFunction* function, 
                                   PfieldGeometricObject* geometry,
                                   ObjectType objectType)
{
  PfVec nextObjectPosition;
  double x = geometry->distanceTo(objectPose, testedPose.pos, nextObjectPosition);
  PfVec result(0.0,0.0); 
  PfVec vecToObject;
  double gradient;
  if(x == 0.0)
  {
    if(objectType == ATTRACTIVE)
    {
      return result;
    }
    else
    {
      vecToObject = (testedPose.pos - nextObjectPosition);   
      gradient = function->computeDerivativeValue(x, false);
    }
  }
  else
  {
    vecToObject = (nextObjectPosition - testedPose.pos); 
    gradient = function->computeDerivativeValue(x);
  }
  vecToObject.normalize(); 
  result = (vecToObject * gradient);
  return result;
}


PfVec computeGradientForSectorfield(const PfPose& objectPose, const PfPose& testedPose,
                                    PotentialfieldFunction* function, 
                                    const Sector& sector)
{
  PfVec result(0.0,0.0);
  double dist = objectPose.pos.distanceTo(testedPose.pos);
  if((dist > function->getRange()) ||
     !sector.pointInside(objectPose, testedPose.pos))
  {
    return result;
  }
  PfVec vecToObject = (objectPose.pos - testedPose.pos);
  vecToObject.normalize();
  double gradient = function->computeDerivativeValue(dist);
  PfVec gradientToOrigin(vecToObject * gradient);

  PfVec centerOfSector(1.0,0.0);
  centerOfSector.rotate(objectPose.rotation);
  PfVec vecToCenter = (objectPose.pos - testedPose.pos + (centerOfSector*dist));
  if(vecToCenter.length() == 0.0)
  {
    return gradientToOrigin;
  }
  vecToCenter.normalize();
  double x = fabs(objectPose.getAngleTo(testedPose.pos))*dist;
  double distValue = function->computeValue(dist);
  double range = (sector.openingAngle/2.0)*dist;
  sector.crossFunction->setParameters(distValue, range);
  double cgradient(sector.crossFunction->computeDerivativeValue(x));
  result = (gradientToOrigin + (vecToCenter*cgradient));
  return result;
}

---