HRICS_BodiesDistance.h

#ifndef HRICS_CF_BODIESDISTANCE_H
#define HRICS_CF_BODIESDISTANCE_H

#include "HRICS_CF_VirtualCostFunction.h"
#include "API/planningAPI.hpp"

namespace HRICS {
namespace CF {

class BodiesDistance :public VirtualCostFunction
{
public:
    using VirtualCostFunction::getCost;

    BodiesDistance(CostFunctionCommon *commonCF, double min,double max, double low,double up,double maxToMeanRatio,FunctionShape shape,bool useBoundingBoxes=false);
    virtual ~BodiesDistance();

    static std::string getName(){return "BodiesDistance";}

    double getCost();
    double getCost(Robot *r, Robot *human);
    std::pair<double,double> getCost(Robot *r1, unsigned int o1, Robot *r2, unsigned int o2);

    bool useBoundingBoxes() const;
    void setUseBoundingBoxes(bool useBoundingBoxes);

private:
    bool _useBoundingBoxes; 

};

} // namespace CF
} // namespace HRICS

#endif // HRICS_CF_BODIESDISTANCE_H