Tools/PotentialFields/Pfield.cpp

Go to the documentation of this file.

/**
* @file Pfield.cpp
* 
* Implementation of class Potentialfield
* This file was originally named Potentialfield.cpp but had to be renamed
* because of another Potentialfield.cpp in this project.
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/

#include "Pfield.h"
#include "Motionfield.h"
#include "FieldObject.h"
#include "RandomMotionGenerator.h"
#include "PotentialfieldComposition.h"



Potentialfield::Potentialfield()
{
  isActive = true;
  currentlySelected = false;
  block = 0;
}


Potentialfield::~Potentialfield()
{
  combinedFields.clear();
  objects.clear();
}


void Potentialfield::getValueArray(double x1, double y1, double x2, double y2,
                                   int xSteps, int ySteps, double value[], double& max)
{
  double stepWidthX = (x2-x1)/(double)xSteps;
  double stepWidthY = (y2-y1)/(double)ySteps;
  PfPose currentPose;
  currentPose.pos.x = x1;
  currentPose.pos.y = y1;
  double min = getFieldValueAt(currentPose);
  double mx = min;
  for(int y=0; y<ySteps; y++)
  {
    for(int x=0; x<xSteps; x++)
    {
      int pos = y*ySteps + x;
      currentPose.pos.x = x1+x*stepWidthX;
      currentPose.pos.y = y1+y*stepWidthY;
      value[pos] = getFieldValueAt(currentPose);
      if(value[pos]<min)
      {
        min = value[pos];
      }
      else if(value[pos]>mx)
      {
        mx = value[pos];
      }
    }
  }
  min = -1 * min;
  max = mx + min;
  for(int i = 0; i<xSteps*ySteps; i++)
  {
    value[i] += min;
  }
}


void Potentialfield::getDirectionArray(double x1, double y1, double x2, double y2,
                                       int xSteps, int ySteps, PfVec directions[])
{
  double stepWidthX = (x2-x1)/(double)xSteps;
  double stepWidthY = (y2-y1)/(double)ySteps;
  PfPose currentPose;
  for(int y=0; y<ySteps; y++)
  {
    for(int x=0; x<xSteps; x++)
    {
      int pos = y*ySteps + x;
      currentPose.pos.x = x1+x*stepWidthX;
      currentPose.pos.y = y1+y*stepWidthY;
      PfVec objVec(getFieldVecAt(currentPose));
      if(objVec.length() >= 1.0)
      {
        objVec.normalize();
      }
      directions[pos] = objVec;
    }
  }
}


PfVec Potentialfield::getFieldVecAt(const PfPose& pose)
{
  PfVec absVec(0.0,0.0);
  std::vector < PotentialFieldsObject* >::iterator object;
  for (object = objects.begin (); object != objects.end (); ++object)
  {
    absVec += (*object)->computeAbsFieldVecAt(pose);
  }
  if(this->getBehaviorFieldType() == MOTION_FIELD)
  {
    absVec += ((Motionfield*)(this))->getRandomVector();
  }
  return absVec;
}


double Potentialfield::getFieldValueAt(const PfPose& pose)
{
  double result(0.0);
  std::vector < PotentialFieldsObject* >::iterator object;
  for (object = objects.begin (); object != objects.end (); ++object)
  {
    result += (*object)->computeChargeAt(pose);
  }
  return result;
}


void Potentialfield::setSelectionFeedback(bool selected)
{
  if(selected)
  {
    if(currentlySelected)
    {
      selectedCalls++;
    }
    else
    {
      currentlySelected = true;
      selectedSince = getSystemTime();
      selectedCalls = 1;
    }
    checkForBlockAppliance();
  }
  else
  {
    if(currentlySelected)
    {
      currentlySelected = false;
      checkForBlockAppliance();
    }
  }
}


bool Potentialfield::isBlocked()
{
  if(blockForM == CALLS)
  {
    if(block>0)
    {
      --block;
    }
    return (block > 0);
  }
  else //blockForM == MILLISECONDS
  {
    return (getSystemTime() < block);
  }
}


void Potentialfield::checkForBlockAppliance()
{
  if(currentlySelected)
  {
    if(keepMaxForO == CALLS)
    {
      long sel2(static_cast< long >(selectedCalls));
      if(sel2 >= o)
      {
        currentlySelected = false;
      }
    }
    else if(o != -1)
    {
      unsigned int o2(static_cast< unsigned int>(o));
      if((getSystemTime() - selectedSince) > o2)
      {
        currentlySelected = false;
      }
    }
  }
  if(!currentlySelected)   //No ELSE here, value of currentlySelected may change!
  {
    if(blockForM == CALLS)
    {
      block = m;
    }
    else
    {
      block = getSystemTime()+m;
    }
  }
}


bool Potentialfield::hasToRemainActive()
{
  if(!currentlySelected)
  {
    return false;
  }
  if(keepForN == CALLS)
  {
    return (selectedCalls < n);
  }
  else
  {
    return (getSystemTime() < (selectedSince+n));
  }
}


void Potentialfield::getResult(const PfPose& pose, PotentialfieldResult& result)
{
  result.action = name;
  if(isBlocked() || (!isActive))
  {
    result.actionPossible = false;
  }
  else if(currentlySelected && (whatToKeep == KEEP_RESULT) &&
          ( ((keepForN==CALLS)&&(selectedCalls<n)) || 
            ((keepForN==MILLISECONDS) && (getSystemTime() < (selectedSince+n))) ) )
  {
    result = lastResult;
  }
  else
  {
    execute(pose,result);
    result.timeStamp = getSystemTime();
    lastResult = result;
  }
}

---