Tools/PotentialFields/FieldObject.cpp

Go to the documentation of this file.

/**
* @file FieldObject.cpp
* 
* Implementation of class PotentialFieldsObject
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/

#include "FieldObject.h"
#include "PotentialfieldComposition.h"

PotentialFieldsObject::PotentialFieldsObject()
{
  refToPfieldComposition = 0; 
  geometry = 0; 
  absGeometry = 0;
  function = 0;
  tangentialField = NO_TANGENTIALFIELD;
}


PotentialFieldsObject::PotentialFieldsObject(const std::string& name, ObjectType objectType)
{
  this->geometry = 0;
  this->absGeometry = 0;
  this->name = name;
  this->objectType = objectType;
  refToPfieldComposition = 0;
  active = true;
  function = 0;
  tangentialField = NO_TANGENTIALFIELD;
}


PotentialFieldsObject::~PotentialFieldsObject()
{
  if(geometry != 0)
  {
    delete geometry;
  }
  if(absGeometry != 0)
  {
    delete absGeometry;
  }
  if(function != 0)
  {
    delete function;
  }
}


PotentialFieldsObject* PotentialFieldsObject::createInstance(const std::string& instanceName)
{
  PotentialFieldsObject* instance = new PotentialFieldsObject();
  (*instance) = (*this);
  instance->name = instanceName;
  return instance;
}


void PotentialFieldsObject::operator = (const PotentialFieldsObject& other)
{
  name = other.name;
  objectType = other.objectType;
  active = other.active;
  setPose(other.pose);
  setActivation(other.active);
  if(function != 0)
  {
    delete function;
    function = 0;
  }
  setFunction(other.function);
  if(geometry != 0)
  {
    delete geometry;
    geometry = 0;
  }
  if(absGeometry != 0)
  {
    delete absGeometry;
    absGeometry = 0;
  }
  setGeometry(other.geometry);
  setTangentialField(other.tangentialField);
  setStatic(other.isStaticVar, other.dynamicPoseId);
  setPfieldCompositionRef(other.refToPfieldComposition);
  switch(other.fieldType)
  {
    case SECTOR_FIELD: setField(other.fieldType, other.sector);
    default: setField(other.fieldType);
  }
}


PotentialFieldsObject* PotentialFieldsObject::getCopy()
{
  PotentialFieldsObject* newObject = new PotentialFieldsObject();
  (*newObject) = (*this);
  return newObject;
}


void PotentialFieldsObject::getMinimalCopyFrom(PotentialFieldsObject* other)
{
  if(other->isActive())
  {
    setPose(other->getPose());
    setActivation(true);
  }
  else
  {
    setActivation(false);
  }
}


void PotentialFieldsObject::setGeometry(PfieldGeometricObject* geometry)
{
  if(geometry == 0)
  {
    this->geometry = 0;
    absGeometry = 0;
  }
  else
  {
    this->geometry = geometry->clone(); 
    absGeometry = geometry->getAbs(pose);
  }
}


void PotentialFieldsObject::setPose(const PfPose& pose)
{ 
  this->pose = pose; 
  if(absGeometry != 0)
  {
    absGeometry->setAbsoluteFromOther(pose, geometry);
  }
}


double PotentialFieldsObject::computeChargeAt(const PfPose& otherPose)
{
  double charge = 0.0;
  if(active)
  {
    if(pose.hasProbabilityDistribution)
    {
      for(unsigned int j=0; j<pose.probabilityDistribution.size(); j++)
      {
        PfPose ownProbPose(pose.probabilityDistribution[j]);
        if(otherPose.hasProbabilityDistribution)
        {
          for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
          {
            PfPose probPose(otherPose.probabilityDistribution[i]);
            double probCharge(computeChargeForSinglePose(ownProbPose, probPose));
            probCharge *= (probPose.probability * ownProbPose.probability);
            charge += probCharge;
          }
        }
        else
        {
          charge += ownProbPose.probability * 
                    computeChargeForSinglePose(ownProbPose, otherPose);
        }
      }
    }
    else
    {
      if(otherPose.hasProbabilityDistribution)
      {
        for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
        {
          PfPose probPose = otherPose.probabilityDistribution[i];
          double probCharge(computeChargeForSinglePose(pose, probPose));
          probCharge *= probPose.probability;
          charge += probCharge;
        }
      }
      else
      {
        charge = computeChargeForSinglePose(pose, otherPose);
      }
    }
  }
  return charge;
}


PfVec PotentialFieldsObject::computeAbsFieldVecAt(const PfPose& otherPose)
{
  PfVec result(0.0,0.0);
  if(active)
  {
    if(pose.hasProbabilityDistribution)
    {
      for(unsigned int j=0; j<pose.probabilityDistribution.size(); j++)
      {
        PfPose ownProbPose(pose.probabilityDistribution[j]);
        if(otherPose.hasProbabilityDistribution)
        {
          for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
          {
            PfPose probPose(otherPose.probabilityDistribution[i]);
            PfVec probVec(computeGradientVecForSinglePose(ownProbPose, probPose));
            probVec *= (probPose.probability * ownProbPose.probability);
            result += probVec;
          }
        }
        else
        {
          result += (computeGradientVecForSinglePose(ownProbPose, otherPose) * 
                     ownProbPose.probability);
        }
      }
    }
    else
    {
      if(otherPose.hasProbabilityDistribution)
      {
        for(unsigned int i=0; i<otherPose.probabilityDistribution.size(); i++)
        {
          PfPose probPose = otherPose.probabilityDistribution[i];
          PfVec probVec(computeGradientVecForSinglePose(pose, probPose));
          probVec *= otherPose.probabilityDistribution[i].probability;
          result += probVec;
        }
      }
      else
      {
        result = computeGradientVecForSinglePose(pose, otherPose);
      }
    }
    if(tangentialField == CLOCKWISE)
    {
      if(objectType == REPULSIVE)
      {
        result.rotate(pi_2);
      }
      else
      {
        result.rotate(-pi_2);
      }
    }
    else if(tangentialField == COUNTER_CLOCKWISE)
    {
      if(objectType == REPULSIVE)
      {
        result.rotate(-pi_2);
      }
      else
      {
        result.rotate(pi_2);
      }
    }
  }
  return result;
}


inline double PotentialFieldsObject::computeChargeForSinglePose(const PfPose& objectPose, 
                                                 const PfPose& position) const
{
  double charge(0.0);
  switch(fieldType)
  {
  case POINT_FIELD:  charge = computeChargeForPointfield(objectPose, position, function);
                     break;
  case SHAPE_FIELD:  charge = computeChargeForShapefield(objectPose, position, function, geometry);
                     break;
  case SECTOR_FIELD: charge = computeChargeForSectorfield(objectPose, position, function, sector);
                     break;
  }
  return charge;
}

 
inline PfVec PotentialFieldsObject::computeGradientVecForSinglePose(const PfPose& objectPose, 
                                                     const PfPose& position) const
{
  PfVec gradient;
  switch(fieldType)
  {
  case POINT_FIELD:  gradient = computeGradientForPointfield(objectPose, position, function);
    break;
  case SHAPE_FIELD:  gradient = computeGradientForShapefield(objectPose, position, function,
                       geometry, objectType);
    break;
  case SECTOR_FIELD: gradient = computeGradientForSectorfield(objectPose, position, function,
                       sector);
    break;
  }
  return gradient;
}


void PotentialFieldsObject::setFunction(PotentialfieldFunction* function)
{
  if(this->function != 0)
  {
    delete (this->function);
    this->function = 0;
  }
  if(function != 0)
  {
    this->function = function->clone();
  }
}


void PotentialFieldsObject::setField(FieldType type)
{
  fieldType = type;
}


void PotentialFieldsObject::setField(FieldType type, const Sector& sector)
{
  fieldType = type;
  this->sector = sector;
  this->sector.crossFunction = sector.crossFunction->clone();
}


void PotentialFieldsObject::updateData()
{
  ObjectStateDescription desc;
  desc = refToPfieldComposition->getDescriptionFromId(dynamicPoseId);
  setPose(desc.pose);
  active = desc.isActive;
}


void PotentialFieldsObject::computeAbsGeometry()
{
  absGeometry->setAbsoluteFromOther(pose, geometry);
}

---