worldmodel.h

agentspark 机器人模拟代码 适用robocup 机器人步态模拟仿真（机器人动作在NAOGETUP.cpp下修改）

/*
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef WORLDMODEL_H
#define WORLDMODEL_H

//#include <oxygen/gamecontrolserver/baseparser.h>
//#include <oxygen/gamecontrolserver/predicate.h>
#include "types.h"
#include <boost/shared_ptr.hpp>
#include "./salt/matrix.h"
//#include "Logger.h"
#include "Parser.h"
#include "./Utilities/StaticModule.h"

#define WM StaticModule<WorldModel>::getInstance()

class Self;
class Ball;
class FieldInfo;

class WorldModel
{
public:
    WorldModel();
    ~WorldModel();

	void Init();
   void Update(const std::string& message);

	void setMySimTime  ( float simTime );
	VisionObject setVisionObjectMap ( std::string objName );
	void setVisionSenseMap (  VisionObject VO,float distance,float theta,float phi );

protected:

   // void OnUnLink();

    void ShiftInfo();

    void ResetInfo();

    void SetupVisionObjectMap();

    void SetupVisionSenseMap();
	
    void CalculateVisionObjectLocalPos();

    salt::Matrix LocalizeWithThreeFlagsTriangle(const VisionObject vo1,
                                                const VisionObject vo2,
                                                const VisionObject vo3);

    salt::Matrix LocalizeWithThreeFlagsLine(const VisionObject vo1,
                                            const VisionObject vo2,
                                            const VisionObject vo3);

    bool Localize();

    void UpdateSelf();

    void UpdateBall();

    void UpdatePlayer();

private:

    //CachedPath<Self>        mSelf;
	Self *mSelf;
	//CachedPath<Ball>        mBall;
	Ball *mBall;
    //CachedPath<FieldInfo>   mFieldInfo;
	FieldInfo *mFieldInfo;

    typedef std::map<VisionObject, VisionSense> TVisionSenseMap;
    TVisionSenseMap mVisionSenseMap;

    typedef std::map<std::string, VisionObject> TVisionObjectMap;
    TVisionObjectMap mVisionObjectMap;

    /** current simulation time */
    float           mSimTime;

    /** simulation time of last cycle */
    float           mLastSimTime;

    salt::Matrix    mVisionPerceptorMatrix;

    int             mLeftFlagsSee;

    int             mRightFlagsSee;
};

#endif