field_model.h

// Model for the soccer field
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#ifndef FIELD_MODEL_H
#define FIELD_MODEL_H

#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <config_server/parameter.h>

namespace field_model
{

class WorldObject
{
public:
    enum Type
    {
        Type_Circle,      
        Type_Goal,        
        Type_GoalPost,    
        Type_XMarker,     
        Type_FieldLine,   
        Type_LineXingT,   
        Type_LineXingX,   
        Type_LineXingL,   
        Type_MagneticHeading, 
        NumTypes
    };

    typedef std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > PointsType;

    WorldObject(Type type, const Eigen::Vector3d& pose);
    WorldObject(Type type, double x, double y, double t = 0.0);
    WorldObject(Type type, const PointsType& points);

    WorldObject mirrorX() const;
    WorldObject mirrorY() const;

    inline Eigen::Vector3d pose() const { return m_pos; }

    inline const PointsType& points() const { return m_points; }

    inline Type type() const { return m_type; }

protected:
    void setPose(const Eigen::Vector3d& pose);

private:
    friend class FieldModel;

    WorldObject() {}

    Type m_type;
    Eigen::Vector3d m_pos;
    PointsType m_points;
};

class FieldModel
{
public:
    static FieldModel* getInstance();

    enum FieldType
    {
        UnknownField = 0,
        TeenSizeField,
        KidSizeField,
        BonnField,
        NumFieldTypes
    };
    static const std::string FieldTypeName[NumFieldTypes];
    static const std::string& fieldTypeName(FieldType type) { if(type >= UnknownField && type < NumFieldTypes) return FieldTypeName[type]; else return FieldTypeName[UnknownField]; }

    inline FieldType type() const
    { return m_type; }

    inline const std::string& typeName() const
    { return fieldTypeName(m_type); }

    inline double width() const
    { return m_width; }

    inline double length() const
    { return m_length; }

    inline double boundary() const
    { return m_boundary; }

    inline double goalWidth() const
    { return m_goalWidth; }

    inline double goalAreaWidth() const
    { return m_goalAreaWidth; }

    inline double goalAreaDepth() const
    { return m_goalAreaDepth; }

    inline double centerCircleDiameter() const
    { return m_centerCircleDiameter; }

    inline double penaltyMarkerDist() const
    { return m_penaltyMarkerDist; }

    inline double ballDiameter() const
    { return m_ballDiameter; }

    inline double borderTop() const
    { return m_borderTop; }

    inline double borderBottom() const
    { return m_borderBottom; }

    inline double borderLeft() const
    { return m_borderLeft; }

    inline double borderRight() const
    { return m_borderRight; }

    inline const std::vector<WorldObject>& objects(WorldObject::Type type) const
    { return m_objects[type]; }

    void setMagneticHeading(double heading);
    double magneticHeading() const;

private:
    FieldModel();

    enum MirrorFlag
    {
        NoMirror = 0,
        MirrorX = (1 << 0),
        MirrorY = (1 << 1),
        MirrorAll = MirrorX | MirrorY
    };
    WorldObject* addObject(WorldObject::Type type, double x, double y, double t, int flags = MirrorX | MirrorY);
    void addLine(const WorldObject::PointsType& points, int flags = MirrorX | MirrorY);

    static FieldModel* m_instance;

    FieldType m_type;
    double m_width;
    double m_length;
    double m_boundary;
    double m_goalWidth;
    double m_goalAreaWidth;
    double m_goalAreaDepth;
    double m_centerCircleDiameter;
    double m_penaltyMarkerDist;
    double m_ballDiameter;
    double m_borderTop;
    double m_borderBottom;
    double m_borderLeft;
    double m_borderRight;

    std::vector<WorldObject> m_objects[WorldObject::NumTypes];

    WorldObject* m_magneticHeading;
    void updateMagneticHeading();
    config_server::Parameter<float> m_attEstMagCalibX;
    config_server::Parameter<float> m_attEstMagCalibY;
    double m_heading;
};

}

#endif