Tools/PotentialFields/FieldObject.cpp

Go to the documentation of this file.

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

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



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


Object::Object(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;
}


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


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


void Object::operator = (const Object& 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);
  }
}


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


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


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


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


double Object::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 Object::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 Object::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 Object::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 Object::setFunction(PotentialfieldFunction* function)
{
  if(this->function != 0)
  {
    delete (this->function);
    this->function = 0;
  }
  if(function != 0)
  {
    this->function = function->clone();
  }
}


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


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


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


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



/*
* $Log: FieldObject.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:37:25  cvsadm
* created new repository GT2004_WM
*
* Revision 1.1  2004/01/20 15:42:20  tim
* Added potential fields implementation
*
* Revision 1.7  2003/06/13 14:27:58  tim
* added random generator and tangential fields
*
* Revision 1.6  2003/06/09 20:00:04  tim
* Changed potentialfield architecture
*
* Revision 1.5  2003/05/20 12:43:43  tim
* Changed function representation, fixed crash on SuperCore, integrated social functions
*
* Revision 1.4  2003/04/22 14:35:17  tim
* Merged changes from GO
*
* Revision 1.4  2003/04/09 19:03:06  tim
* Last commit before GermanOpen
*
* Revision 1.3  2003/04/04 14:50:53  tim
* Fixed bugs, added minor features
*
* Revision 1.2  2003/03/23 20:32:37  loetzsch
* removed green compiler warning: no newline at end of file
*
* Revision 1.1  2003/03/23 17:51:27  tim
* Added potentialfields
*
*/

---