Tools/PotentialFields/PfieldDatatypes.h

Go to the documentation of this file.

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

#ifndef PFIELD_DATATYPES_H_
#define PFIELD_DATATYPES_H_


#include "PfieldConfig.h"
#include <cmath>
#include <vector>

/** A minimum value for floating point comparisons*/
const double EPSILON=0.00000001;


/**
* @class PfVec
* A simple vector with two double components
*/
class PfVec
{
public:
  /** The x-component*/
  double x;
  /** The y-component*/
  double y;

  /** Constructor */
  PfVec()
  {
    x = 0.0;
    y = 0.0;
  }

  /** Constructor 
  * @param x The x component
  * @param y The y component
  */
  PfVec(double x, double y)
  {
    this->x = x;
    this->y = y;
  }

  /** Copy-Constructor 
  * @param p Another vector
  */
  PfVec(const PfVec& p)
  {
    (*this) = p;
  }

  /** Distance to another vector 
  * @param v The other vector
  * @return The distance
  */
  double distanceTo(const PfVec& v) const
  {
    double dx = v.x - x;
    double dy = v.y - y;
    return sqrt(dx*dx + dy*dy);
  }

  /** Rotates around an angle
  * @param angle The angle
  */
  void rotate(double angle)
  {
    double ca = cos(angle);
    double sa = sin(angle);
    double newX = x*ca - y*sa;
    y = y*ca + x*sa;
    x = newX;
  }

  /** Normalizes the vector to length==1*/
  void normalize()
  {
    double len = length();
    if(len != 0.0)
    {
      x /= len;
      y /= len;
    }
  }

  /** Returns the length of the vector
  * @return The length
  */
  double length() const
  {
    return sqrt(x*x + y*y);
  }

  /** Returns the squared length of the vector
  * @return The squared length
  */
  double squareLength() const
  {
    return (x*x + y*y);
  }

  /** Assigns another vector
  * @param v The other vector
  */
  void operator =(const PfVec& v)
  {
    x = v.x;
    y = v.y;
  }

  /** Adds another vector
  * @param v The other vector
  * @return The resulting vector
  */
  PfVec operator +(const PfVec& v) const
  {
    PfVec result(x,y);
    result.x += v.x;
    result.y += v.y;
    return result;
  }

  /** Substracts another vector
  * @param v The other vector
  * @return The resulting vector
  */
  PfVec operator -(const PfVec& v) const
  {
    PfVec result(x,y);
    result.x -= v.x;
    result.y -= v.y;
    return result;
  }

  /** Scales the vector
  * @param n The scalar
  */
  void operator *= (const double& n)
  {
    x*=n;
    y*=n;
  }

  /** Returns a scaled vector
  * @param n The scalar
  * @return The resulting vector
  */
  PfVec operator * (const double& n) const
  {
    PfVec newVec(x,y);
    newVec *= n;
    return newVec;
  }

  /** Adds another vector
  * @param p The other vector
  */
  void operator += (const PfVec& p)
  {
    x+=p.x;
    y+=p.y;
  }

  /** Substracts another vector
  * @param p The other vector
  */
  void operator -= (const PfVec& p)
  {
    x-=p.x;
    y-=p.y;
  }

  /** Multiplies with another vector
  * @param p The other vector
  * @return The inner product
  */
  double scalarProduct(const PfVec& p) const
  {
    return (x*p.x + y*p.y);
  }

  /** Computes the angle of the vector
  * @return The angle
  */
  double getAngle() const
  {
    return atan2(y,x);
  }
};


/**
* @class PfPose
* A class to describe the pose of an object in a potential field
*/
class PfPose
{
public:
  /** The position of the pose*/
  PfVec pos;
  /** The rotation of the pose*/
  double rotation;
  /** The current speed*/
  double speed;
  /** The probability of the pose*/
  double probability;
  /** Flag: true, if this pose consists of several poses*/
  bool hasProbabilityDistribution;
  /** A set of poses*/
  std::vector<PfPose> probabilityDistribution;

  /** Constructor */
  PfPose ()
  {
    pos.x=0.0;
    pos.y=0.0;
    rotation=0.0;
    speed=0.0;
    probability=1.0;
    hasProbabilityDistribution=false;
  }

  /** Destructor */
  ~PfPose()
  {
    probabilityDistribution.clear();
  }

  /** Copy-Constructor
  * @param p Another pose
  */
  PfPose (const PfPose& p)
  {
    pos = p.pos;
    rotation = p.rotation;
    speed = p.speed;
    probability = p.probability;
    hasProbabilityDistribution = p.hasProbabilityDistribution;
    if(hasProbabilityDistribution)
    {
      probabilityDistribution = p.probabilityDistribution;
    }
    else
    {
      probabilityDistribution.clear();
    }
  }

  /** Resets all values*/
  void init()
  {
    pos.x=0.0;
    pos.y=0.0;
    rotation=0.0;
    speed=0.0;
    probability=1.0;
    hasProbabilityDistribution=false;
    probabilityDistribution.clear();
  }

  /** Computes a relative vector to a position
  * @param p The position
  * @return The vector
  */
  PfVec getRelativeVectorTo(const PfVec& p) const
  {
    PfVec rel = (p-pos);
    rel.rotate(-rotation);
    return rel;
  }

  /** Computes the relative angle to a position
  * @param p The position
  * @return The angle
  */
  double getAngleTo(const PfVec& p) const
  {
    PfVec relVec(p - pos);
    relVec.rotate(-rotation);
    return relVec.getAngle();
  }

  /** Adds a vector to the pose
  * @param vec The vector
  */
  void addVector(const PfVec& vec)
  {
    PfVec relVec(vec);
    relVec.rotate(rotation);
    pos += relVec;
    if(hasProbabilityDistribution)
    {
      for(unsigned int i=0; i<probabilityDistribution.size(); i++)
        probabilityDistribution[i].addVector(vec);
    }
  }

  /** Adds a vector to the pose not considering the rotation
  * @param vec The vector
  */
  void addAbsVector(const PfVec& vec)
  {
    pos += vec;
    if(hasProbabilityDistribution)
    {
      for(unsigned int i=0; i<probabilityDistribution.size(); i++)
        probabilityDistribution[i].addAbsVector(vec);
    }
  }

  /** Rotates the pose around a position
  * @param position The position to rotate around
  * @param angle The angle to rotate
  */
  void rotateAround(const PfVec& position, double angle)
  {
    pos -= position;
    pos.rotate(angle);
    pos += position;
    rotation += angle;
    normRotation();
    if(hasProbabilityDistribution)
    {
      for(unsigned int i=0; i<probabilityDistribution.size(); i++)
      {
        probabilityDistribution[i].rotateAround(position, angle);
      }
    }
  }

  /** Copies data from another pose
  * @param p The pose
  */
  void operator = (const PfPose& p)
  {
    pos = p.pos;
    rotation = p.rotation;
    speed = p.speed;
    probability = p.probability;
    hasProbabilityDistribution = p.hasProbabilityDistribution;
    if(hasProbabilityDistribution)
    {
      probabilityDistribution = p.probabilityDistribution;
    }
    else
    {
      probabilityDistribution.clear();
    }
  }

  /**
  * Norms the rotation to the interval [-pi,..,pi]
  */
  void normRotation()
  {
    while(rotation>pi)
    {
      rotation -= (pi2);
    }
    while(rotation<-pi)
    {
      rotation += (pi2);
    }
  }

  /** Sets the pose from the sample with the highest probability
  */
  void setPoseFromSamples()
  {
    double bestProbability(0.0);
    for(unsigned int i=0; i<probabilityDistribution.size(); i++)
    {
      if(probabilityDistribution[i].probability > bestProbability)
      {
        pos = probabilityDistribution[i].pos;
        rotation = probabilityDistribution[i].rotation;
        bestProbability = probabilityDistribution[i].probability;
      }
    }
  }
};


#endif //PFIELD_DATATYPES_H_

---