worldmodel.cpp

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.
*/
#include "worldmodel.h"

#include "self.h"
#include "fieldinfo.h"
#include "ball.h"
#include "./salt/geometry.h"

using namespace std;
using namespace boost;
using namespace salt;
//using namespace oxygen;

WorldModel::WorldModel() :
    mSimTime(0.0f),
    mLastSimTime(0.0f)
{
}

WorldModel::~WorldModel()
{
}

void WorldModel::Init()
{
    	SetupVisionObjectMap();
    	SetupVisionSenseMap();
}

/*void WorldModel::OnUnLink()
{
    mParser.reset();
}*/

void WorldModel::SetupVisionObjectMap()
{
    mVisionObjectMap.clear();
    mVisionObjectMap["F1L"]         = FLAG_1_L;
    mVisionObjectMap["F1R"]         = FLAG_1_R;
    mVisionObjectMap["F2L"]         = FLAG_2_L;
    mVisionObjectMap["F2R"]         = FLAG_2_R;
    mVisionObjectMap["G1L"]         = GOAL_1_L;
    mVisionObjectMap["G1R"]         = GOAL_1_R;
    mVisionObjectMap["G2L"]         = GOAL_2_L;
    mVisionObjectMap["G2R"]         = GOAL_2_R;
    mVisionObjectMap["B"]           = BALL;

    mVisionObjectMap["TEAMMATE1"]   = TEAMMATE_1;
    mVisionObjectMap["TEAMMATE2"]   = TEAMMATE_2;
    mVisionObjectMap["TEAMMATE3"]   = TEAMMATE_3;
    mVisionObjectMap["TEAMMATE4"]   = TEAMMATE_4;
    mVisionObjectMap["TEAMMATE5"]   = TEAMMATE_5;
    mVisionObjectMap["TEAMMATE6"]   = TEAMMATE_6;
    mVisionObjectMap["TEAMMATE7"]   = TEAMMATE_7;
    mVisionObjectMap["TEAMMATE8"]   = TEAMMATE_8;
    mVisionObjectMap["TEAMMATE9"]   = TEAMMATE_9;
    mVisionObjectMap["TEAMMATE10"]  = TEAMMATE_10;
    mVisionObjectMap["TEAMMATE11"]  = TEAMMATE_11;

    mVisionObjectMap["OPPONENT1"]   = OPPONENT_1;
    mVisionObjectMap["OPPONENT2"]   = OPPONENT_2;
    mVisionObjectMap["OPPONENT3"]   = OPPONENT_3;
    mVisionObjectMap["OPPONENT4"]   = OPPONENT_4;
    mVisionObjectMap["OPPONENT5"]   = OPPONENT_5;
    mVisionObjectMap["OPPONENT6"]   = OPPONENT_6;
    mVisionObjectMap["OPPONENT7"]   = OPPONENT_7;
    mVisionObjectMap["OPPONENT8"]   = OPPONENT_8;
    mVisionObjectMap["OPPONENT9"]   = OPPONENT_9;
    mVisionObjectMap["OPPONENT10"]  = OPPONENT_10;
    mVisionObjectMap["OPPONENT11"]  = OPPONENT_11;
}

void WorldModel::SetupVisionSenseMap()
{
    mVisionSenseMap.clear();

    const float flagHeight = 0.0f; // 0.375f;
    const float goalFlagX = FIELDINFO.fieldlength/2.0; // + mFieldInfo->goaldepth/2.0; NOTE
    const float goalFlagHeight = FIELDINFO.goalheight/2.0;

    mVisionSenseMap[FLAG_1_L].realPos  = Vector3f(-FIELDINFO.fieldlength/2.0, 
                                                  +FIELDINFO.fieldwidth/2.0, 
                                                  flagHeight);
    mVisionSenseMap[FLAG_1_R].realPos  = Vector3f(+FIELDINFO.fieldlength/2.0, 
                                                  +FIELDINFO.fieldwidth/2.0, 
                                                  flagHeight);
    mVisionSenseMap[FLAG_2_L].realPos  = Vector3f(-FIELDINFO.fieldlength/2.0, 
                                                  -FIELDINFO.fieldwidth/2.0, 
                                                  flagHeight);
    mVisionSenseMap[FLAG_2_R].realPos  = Vector3f(+FIELDINFO.fieldlength/2.0, 
                                                  -FIELDINFO.fieldwidth/2.0, 
                                                  flagHeight);
    mVisionSenseMap[GOAL_1_L].realPos  = Vector3f(-goalFlagX, 
                                                  +FIELDINFO.goalwidth/2.0,
                                                  goalFlagHeight);
    mVisionSenseMap[GOAL_1_R].realPos  = Vector3f(+goalFlagX, 
                                                  +FIELDINFO.goalwidth/2.0,
                                                  goalFlagHeight);
    mVisionSenseMap[GOAL_2_L].realPos  = Vector3f(-goalFlagX, 
                                                  -FIELDINFO.goalwidth/2.0,
                                                  goalFlagHeight);
    mVisionSenseMap[GOAL_2_R].realPos  = Vector3f(+goalFlagX, 
                                                  -FIELDINFO.goalwidth/2.0,
                                                  goalFlagHeight);
}

void WorldModel::setMySimTime ( float simTime)
{	mSimTime = simTime;
}

VisionObject WorldModel::setVisionObjectMap ( string objName )
{	VisionObject vo;
        TVisionObjectMap::iterator vo_iter = mVisionObjectMap.find(objName);
        if (vo_iter == mVisionObjectMap.end())
        {
            	cout << "[WorldModel] unknown object name: " << objName << endl;
        }
        vo = mVisionObjectMap[objName];
	return vo;
}

void WorldModel::setVisionSenseMap ( VisionObject VO,float distance,float theta,float phi)
{	mVisionSenseMap[VO].distance  = distance;
        mVisionSenseMap[VO].theta     = theta;
        mVisionSenseMap[VO].phi       = phi;
}

void WorldModel::ShiftInfo()
{
    mLastSimTime = mSimTime;
}

void WorldModel::ResetInfo()
{
    mLeftFlagsSee   = 0;
    mRightFlagsSee  = 0;
    SELF.mLFForce.Zero();  SELF.mRFForce.Zero();
    SELF.mLFCenter.Zero(); SELF.mRFCenter.Zero();
    SELF.mGyroAngularVel.Zero();
    mVisionPerceptorMatrix.Identity();

    for (int i = (int)(FLAG_1_L); i <= (int)(OPPONENT_11); ++i)
    {
        VisionSense &vs = mVisionSenseMap[static_cast<VisionObject>(i)];
        vs.distance = -1000;
    }
}

void WorldModel::Update(const string& message)
{
    //cout << "(currentcycle: " << SELF.CurrentCycle
                     //  << ") message: " << message << endl;

    ShiftInfo();
    ResetInfo();
    PARSER.parseAll(message);
    CalculateVisionObjectLocalPos();
    UpdateSelf();
    UpdateBall();
    UpdatePlayer();

    if (mSimTime > 1.0f && gAbs(mSimTime - mLastSimTime - 0.02) > 1e-3)
         cerr << "(Time Jump) "
                           << mLastSimTime << " " << mSimTime << endl;
}

void WorldModel::UpdateSelf()
{	
    Localize();
    SELF.UpdateRobotMatrix(mVisionPerceptorMatrix);
}

void WorldModel::UpdateBall()
{	
    BALLINFO.pos = mVisionPerceptorMatrix * (mVisionSenseMap[BALL].localPos);
}

void WorldModel::UpdatePlayer()
{
}

void WorldModel::CalculateVisionObjectLocalPos()
{
    for (int i = (int)(FLAG_1_L); i < (int)(OPPONENT_11); ++i)
    {
        VisionSense& vs = mVisionSenseMap[static_cast<VisionObject>(i)];
        if (vs.distance < 0) continue;

        float distance = vs.distance;
        float theta = gDegToRad(gNormalizeDeg(vs.theta + 90.0f));
        float phi   = gDegToRad(gNormalizeDeg(vs.phi));

        vs.localPos[0] = distance * gCos(phi) * gCos(theta);
        vs.localPos[1] = distance * gCos(phi) * gSin(theta);
        vs.localPos[2] = distance * gSin(phi);

        if (i < static_cast<int>(BALL))
            i % 2 == 0 ? ++mLeftFlagsSee : ++mRightFlagsSee;
    }
}

Matrix WorldModel::LocalizeWithThreeFlagsTriangle(const VisionObject vo1,
                                                  const VisionObject vo2,
                                                  const VisionObject vo3)
{
    Vector3f pos;
    Matrix m;
    m.Identity();

    VisionSense vs1 = mVisionSenseMap[vo1];
    VisionSense vs2 = mVisionSenseMap[vo2];
    VisionSense vs3 = mVisionSenseMap[vo3];

    float a1 = vs1.distance * vs1.distance - vs2.distance * vs2.distance;
    float b1 = 2.0f * (-vs1.realPos.y()) - 2.0f * (-vs2.realPos.y());
    float a2 = vs1.distance * vs1.distance - vs3.distance * vs3.distance;
    float b2 = 2.0f * (-vs1.realPos.x()) - 2.0f * (-vs3.realPos.x() );

    if (gAbs(b1) < 1e-3 ||
        gAbs(b2) < 1e-3)
    {
        return m;
    }

    pos[0] = a2 / b2;
    pos[1] = a1 / b1;
    pos[2] = vs1.realPos.z();

    float under;
    under = b2 * -0.5;
    m(0, 0) = (vs1.localPos.x() - vs3.localPos.x()) / under;
    m(0, 1) = (vs1.localPos.y() - vs3.localPos.y()) / under;
    m(0, 2) = (vs1.localPos.z() - vs3.localPos.z()) / under;

    under = b1 * -0.5;
    m(1, 0) = (vs1.localPos.x() - vs2.localPos.x()) / under;
    m(1, 1) = (vs1.localPos.y() - vs2.localPos.y()) / under;
    m(1, 2) = (vs1.localPos.z() - vs2.localPos.z()) / under;

    Vector3f vx(m(0, 0), m(0, 1), m(0, 2));
    Vector3f vy(m(1, 0), m(1, 1), m(1, 2));
    Vector3f vz = vx.Cross(vy);
    m(2, 0) = vz.x(); m(2, 1) = vz.y(); m(2, 2) = vz.z();

    pos[2] = vs1.realPos.z()
             - m(2, 0) * vs1.localPos.x()
             - m(2, 1) * vs1.localPos.y()
             - m(2, 2) * vs1.localPos.z();

    m.Pos() = pos;
    return m;
}

/** if using this function to localize, the accuracy of z pos is low */
Matrix WorldModel::LocalizeWithThreeFlagsLine(const VisionObject vo1,
                                              const VisionObject vo2,
                                              const VisionObject vo3)
{
    Vector3f pos(0.0f, 0.0f, 0.0f);
    Matrix mat;
    mat.Identity(); 

    VisionSense vs1 = mVisionSenseMap[vo1];
    VisionSense vs2 = mVisionSenseMap[vo2];
    VisionSense vs3 = mVisionSenseMap[vo3];

    float a = vs1.distance * vs1.distance - vs2.distance * vs2.distance;
    float b = 2.0f * (-vs1.realPos.y()) - 2.0f * (-vs2.realPos.y());
    assert(gAbs(b) > 1e-5);
    pos[1] = a / b;

    a = vs2.distance * vs2.distance - vs3.distance * vs3.distance -
         (pos[1]-vs2.realPos.y())*(pos[1]-vs2.realPos.y()) +
         (pos[1]-vs3.realPos.y())*(pos[1]-vs3.realPos.y()) -
          vs2.realPos.z()*vs2.realPos.z() + vs3.realPos.z()*vs3.realPos.z();
    b = 2.0f * (-vs2.realPos.z()) - 2.0f * (-vs3.realPos.z());
    assert(gAbs(b) > 1e-5);
    pos[2] = a / b;

    float c =  vs1.distance * vs1.distance -
               (pos[1]-vs1.realPos.y())*(pos[1]-vs1.realPos.y()) -
               (pos[2]-vs1.realPos.z())*(pos[2]-vs1.realPos.z());
    if (c < 0) return mat;

    float x1 =  gSqrt(c) + vs1.realPos.x();
    float x2 = -gSqrt(c) + vs1.realPos.x();
    pos[0] = (gAbs(x1) < mFieldInfo->fieldlength/2.0) ? x1 : x2;

    Vector3f l1 = vs1.localPos;
    Vector3f r1 = vs1.realPos - pos;
    Vector3f l2 = vs2.localPos;
    Vector3f r2 = vs2.realPos - pos;
    Vector3f l3 = vs3.localPos;
    Vector3f r3 = vs3.realPos - pos;

    Matrix m;
    m.Identity();
    m(0, 0) = l1.x(); m(0, 1) = l1.y(); m(0, 2) = l1.z();
    m(1, 0) = l2.x(); m(1, 1) = l2.y(); m(1, 2) = l2.z();
    m(2, 0) = l3.x(); m(2, 1) = l3.y(); m(2, 2) = l3.z();
    if (! InverseMatrix(m))
    {
        cerr
             << "(WorldModel) localize matrix inverse error" << endl;
    }
    // TODO: comments here 
    Vector3f c1(r1.x(), r2.x(), r3.x());
    Vector3f c2(r1.y(), r2.y(), r3.y());
    Vector3f c3(r1.z(), r2.z(), r3.z());

    Vector3f s1, s2, s3;
    s1 = m.Rotate(c1);
    s2 = m.Rotate(c2);
    s3 = m.Rotate(c3);

    mat(0, 0) = s1.x(); mat(0, 1) = s1.y(); mat(0, 2) = s1.z();
    mat(1, 0) = s2.x(); mat(1, 1) = s2.y(); mat(1, 2) = s2.z();
    mat(2, 0) = s3.x(); mat(2, 1) = s3.y(); mat(2, 2) = s3.z();

    mat.Pos() = pos;
    return mat;
}

bool WorldModel::Localize()
{	
    Vector3f pos(0.0f, 0.0f, 0.0f);
    Matrix mat;

    mat = LocalizeWithThreeFlagsTriangle(FLAG_1_L, FLAG_2_L, FLAG_1_R);
    pos += mat.Pos();
    mat = LocalizeWithThreeFlagsTriangle(FLAG_2_L, FLAG_1_L, FLAG_2_R);
    pos += mat.Pos();
    mat = LocalizeWithThreeFlagsTriangle(FLAG_1_R, FLAG_2_R, FLAG_1_L);
    pos += mat.Pos();
    mat = LocalizeWithThreeFlagsTriangle(FLAG_2_R, FLAG_1_R, FLAG_2_L);
    pos += mat.Pos();
    mat = LocalizeWithThreeFlagsTriangle(GOAL_1_L, GOAL_2_L, GOAL_1_R);
    pos += mat.Pos();
    mat = LocalizeWithThreeFlagsTriangle(GOAL_2_L, GOAL_1_L, GOAL_2_R);
    pos += mat.Pos();
    mat = LocalizeWithThreeFlagsTriangle(GOAL_1_R, GOAL_2_R, GOAL_1_L);
    pos += mat.Pos();
    mat = LocalizeWithThreeFlagsTriangle(GOAL_2_R, GOAL_1_R, GOAL_2_L);
    pos += mat.Pos();

    mVisionPerceptorMatrix = mat;
    pos.z() += 4.0f * FIELDINFO.goalheight / 2.0f;
    mVisionPerceptorMatrix.Pos() = pos / 8.0f;

    return true;
}