monitor.cpp

robocup rcssserver 运行防真机器人足球比赛所用的服务器端

    mRightCamPositions.push_back(rp);

    // third position for watching the penalty area
    lp[0] = -hfl+5;          rp[0] = -lp[0];
    lp[1] = -hfw+4;          rp[1] = lp[1];
    lp[2] = 5.5;             rp[2] = lp[2];
    mLeftCamPositions.push_back(lp);
    mRightCamPositions.push_back(rp);



}

void
Monitor::DrawScene(int pass)
{
    Vector3f pos;
    for (int i = static_cast<int>(GameState::eFLAG_1_L);
         i != static_cast<int>(GameState::eILLEGAL);
         ++i)
    {
        if (mGameState.GetFlag(static_cast<GameState::EFlagType>(i), pos))
        {
            DrawFlag(static_cast<GameState::EFlagType>(i), pos, pass);
        }
    }

    int i = 0;
    int unum;
    TTeamIndex side;
    float size;
    while (mGameState.GetPlayer(i, pos, side, unum, size))
    {
        DrawPlayer(side, unum, pos, size, pass);
        ++i;
    }

    if (mGameState.GetBall(pos, size))
    {
        DrawBall(pos, size, pass);

        if (mCameraMode == eFollowBall && pass == 0)
        {
            Vector3f& ballGroundPos = pos;
            ballGroundPos[2] = 0.0f;
            mGLServer.SetLookAtPos(ballGroundPos);
        }

        if (mCameraMode == eCenterBall && pass == 0)
        {
            Vector3f ballGroundPos = pos;
            ballGroundPos[2] = 0.0f;
            mGLServer.SetLookAtPos(ballGroundPos);
            ballGroundPos[1] = mCenterBallCameraY;
            ballGroundPos[2] = mCenterBallCameraZ;
            mGLServer.SetCameraPos(ballGroundPos);
        }
    }

}

void
Monitor::DrawStatusText()
{
    std::stringstream sl, sc, sr;
    static std::stringstream debug;

    sl << mGameState.GetTeamnameLeft() << " " << mGameState.GetScoreLeft();
    sr << mGameState.GetScoreRight() << " " << mGameState.GetTeamnameRight();

    if (mShowCamIconTime>=0)
    {
        mShowCamIconTime--;
        sc << mCamModeMap[mCameraMode] << "   ";
    }

    sc << "(" << ((mGameState.GetHalf() == GH_FIRST) ?
                  "first" : "second") << " half) ";

    int play_mode = mGameState.GetPlayMode();

    switch (play_mode)
    {
    case PM_PlayOn:
        if (mSecondHalfKickOff==CommServerBase::eRandom)
        {
            mSecondHalfKickOff=mKickOff;    //store who will kickoff at 2nd half
        }


        break;
    case PM_BeforeKickOff:
        if (mGameState.GetHalf() == GH_SECOND)
        {
            mKickOff=mSecondHalfKickOff;
        }
        break;
    case PM_KickOff_Left:
        mKickOff = CommServerBase::eRight;
        break;
    case PM_KickOff_Right:
        mKickOff = CommServerBase::eLeft;
        break;
//     case PM_Goal_Left:
//         cout << "GOOOAAAAAAAAAAAAAAAAAAAAAAAAL! (left) \n";
//         break;
//     case PM_Goal_Right:
//         cout << "GOOOAAAAAAAAAAAAAAAAAAAAAAAAL! (right) \n";
//         break;
    };

    sc << mGameState.PlayMode2Str(play_mode) << " ";
    sc << "t=" << setiosflags(ios::fixed) << setprecision(2) << mGameState.GetTime();

    glColor3f(1.0, 1.0, 1.0);
    mGLServer.DrawTextPix(sl.str().c_str(),
                          Vector2f( 35, mGLServer.GetTextHeight()));
    mGLServer.DrawTextPix(sc.str().c_str(),
                          Vector2f( 0, mGLServer.GetTextHeight()),
                          GLServer::eCENTER);
    mGLServer.DrawTextPix(sr.str().c_str(),
                          Vector2f( -35, mGLServer.GetTextHeight()),
                          GLServer::eRIGHT);

    if (mDrawDebug)
    {
#if 0
        ++frame;
        time=glutGet(GLUT_ELAPSED_TIME);
        if (time - timebase > 1000)
        {
            debug.str("");
            debug << setiosflags(ios::fixed) << setprecision(2)
                  << "FPS: " << frame*1000.0/(time-timebase);
        }
        timebase = time;
        frame = 0;
#elif 1
        debug.str("");
        Vector3f pos;
        mGameState.GetBall(pos);
        debug << setiosflags(ios::fixed) << setprecision(2)
              << mGameState.GetTime() << "   Ball position: " << pos;
        pos[2] = 0.0;
        debug << "   Ground distance from origin: " << pos.Length();
#endif

        mGLServer.DrawTextPix(debug.str().c_str(),
                              Vector2f( 0,  glutGet(GLUT_WINDOW_HEIGHT) - mGLServer.GetTextHeight()),
                              GLServer::eCENTER);
    }
}

void
Monitor::DrawStatusLine()
{
    float aspect = float(mGLServer.GetWidth())/mGLServer.GetHeight();

    glColor4fv(sTeamColorLeft);
    mGLServer.DrawSphere(Vector3f(-0.97*aspect,0.93,0.0), 0.025, 25);
    glColor4fv(sTeamColorRight);
    mGLServer.DrawSphere(Vector3f( 0.97*aspect,0.93,0.0), 0.025, 20);


    // Test: draw a big blue ball
    //mGLServer.DrawSphere(Vector3f( 0.5,.9,0.0), 0.1, 20);

    int play_mode = mGameState.GetPlayMode();

    if (play_mode == PM_BeforeKickOff)
    {
        glColor4fv(sSphereDefaultColor);
        switch (mKickOff)
        {
        case CommServerBase::eRandom:
            mGLServer.DrawSphere(Vector3f( 0.0,0.93,0.0), 0.025, 25);
            break;
        case CommServerBase::eLeft:
            mGLServer.DrawSphere(Vector3f( -0.92*aspect,0.93,0.0), 0.025, 25);
            break;
        case CommServerBase::eRight:
            mGLServer.DrawSphere(Vector3f( 0.92*aspect,0.93,0.0), 0.025, 25);
            break;
        }
    }
}

Vector3f Monitor::Get2DPos(const Vector3f& pos)
{
    float aspect = float(mGLServer.GetWidth())/mGLServer.GetHeight();
    //cout << "(get2dpos) aspect = " << aspect << endl;

    return Vector3f
        (
            aspect*(-1.0) + s2DLowX + s2DScale * (pos[0] + mGameState.GetFieldLength()/2),

            1.0 - s2DLowY - (s2DScale * mGameState.GetFieldWidth()) +
            s2DScale * (pos[1] + mGameState.GetFieldWidth()/2),

            0.0
         );
}

void
Monitor::DrawOverview()
{
    glDisable(GL_LIGHTING);
    glDisable(GL_POINT_SMOOTH);
    glDisable(GL_LINE_SMOOTH);
    glLineWidth(1);

    // radius of player

    float pl_radius = 0.01;
//         = sSzY / mGameState.GetFieldWidth() *
//         mGameState.GetAgentRadius();

    // radius of ball
    float b_radius = 0.005;

//         sSzY / mGameState.GetFieldWidth() *
//         mGameState.GetBallRadius();

    // resolution of a sphere
    int res  = 8;

    const float& fl = mGameState.GetFieldLength();
    const float& fw = mGameState.GetFieldWidth();
//     const float& fh = mGameState.GetFieldHeight();
//     const float& lw = mGameState.GetLineWidth();
//     const float& bs = mGameState.GetBorderSize();
    const float& gw = mGameState.GetGoalWidth();

    // field rect

    glEnable(GL_BLEND);                // Turn Blending On
    glColor4fv(sGroundColor2D);
    glBegin(GL_TRIANGLE_STRIP);
    glVertex3fv(Get2DPos(Vector3f(-fl/2,-fw/2,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(fl/2,-fw/2,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2,fw/2,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(fl/2,fw/2,0)).GetData());
    glEnd();
    glDisable(GL_BLEND);                // Turn Blending Off

    glColor4fv(sLineColor);
    glBegin(GL_LINE_LOOP);
    glVertex3fv(Get2DPos(Vector3f(-fl/2,-fw/2,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(fl/2,-fw/2,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(fl/2,fw/2,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2,fw/2,0)).GetData());
    glEnd();

    // middle line
    glBegin(GL_LINES);
    glVertex3fv(Get2DPos(Vector3f(0,-fw/2,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(0,fw/2,0)).GetData());
    glEnd();

    // middle circle
    mGLServer.DrawCircle(Get2DPos(Vector3f(0,0,0)),sCenterRadius*s2DScale);

    // left goal box
    glBegin(GL_LINE_STRIP);
    glVertex3fv(Get2DPos(Vector3f(-fl/2,gw/2+sGoalBoxLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2+sGoalBoxLength,gw/2+sGoalBoxLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2+sGoalBoxLength,-gw/2-sGoalBoxLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2,-gw/2-sGoalBoxLength,0)).GetData());
    glEnd();

    // right goal box
    glBegin(GL_LINE_STRIP);
    glVertex3fv(Get2DPos(Vector3f(+fl/2,gw/2+sGoalBoxLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(+fl/2-sGoalBoxLength,gw/2+sGoalBoxLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(+fl/2-sGoalBoxLength,-gw/2-sGoalBoxLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(+fl/2,-gw/2-sGoalBoxLength,0)).GetData());
    glEnd();

    // penalty area left
    glBegin(GL_LINE_STRIP);
    glVertex3fv(Get2DPos(Vector3f(-fl/2,gw/2+sPenaltyLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2+sPenaltyLength,gw/2+sPenaltyLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2+sPenaltyLength,-gw/2-sPenaltyLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(-fl/2,-gw/2-sPenaltyLength,0)).GetData());
    glEnd();

    // penalty area right
    glBegin(GL_LINE_STRIP);
    glVertex3fv(Get2DPos(Vector3f(+fl/2,gw/2+sPenaltyLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(+fl/2-sPenaltyLength,gw/2+sPenaltyLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(+fl/2-sPenaltyLength,-gw/2-sPenaltyLength,0)).GetData());
    glVertex3fv(Get2DPos(Vector3f(+fl/2,-gw/2-sPenaltyLength,0)).GetData());
    glEnd();

    Vector3f pos,oldpos;
    float size;

    int i = 0;
    int unum;
    TTeamIndex side;

    //JAN
    double timeDiff = mGameState.GetTime() - mOldGameState.GetTime();

    while (mGameState.GetPlayer(i, pos, side, unum, size))
    {
        switch (side)
            {
            case TI_LEFT:
                glColor4fv(sTeamColorLeft);
                break;
            case TI_RIGHT:
                glColor4fv(sTeamColorRight);
                break;
            default:
                glColor4fv(sSphereDefaultColor);
            }

        mGLServer.DrawSphere(Get2DPos(pos), pl_radius, res);

        if (mDrawVelocity && timeDiff != 0.0)
        {

            glColor3f(1.0f,1.0f,1.0f);
            mOldGameState.GetPlayer(i, oldpos, side, unum, size);
            mGLServer.DrawArbitraryLine(Get2DPos(pos), Get2DPos(pos + (pos-oldpos)/timeDiff));
        }

        if (mDrawUnums)
            {
                glColor4fv(sUnumColor);
                stringstream ss;
                ss << unum;
                mGLServer.DrawText3D(ss.str(), Get2DPos(pos));
            }

        ++i;
    }

    static double pass = mGameState.GetTime();
    if (pass == mGameState.GetTime()) return;
    pass = mGameState.GetTime();
#if 0
    static double vel0 = 0.0;
    static double vel1 = 0.0;
    static std::queue<Vector3f> prev;
    while (prev.size() < 10) prev.push(Vector3f(0,0,0));

    mOldGameState.GetPlayer(0, oldpos, side, unum, size);
    mGameState.GetPlayer(0,pos,side,unum,size);
    vel1 = ((pos-oldpos)/timeDiff).x();


    std::cerr << mGameState.GetTime() << " " << pos << "  "
              << vel0 << " " << vel1 << " "
              << pos.x() << " " << (pos-prev.front()).x() << "\n";

    prev.push(pos);
    prev.pop();

    vel0 = vel1;
#elif 0
    mGameState.GetPlayer(0,pos,side,unum,size);
    std::cerr << mGameState.GetTime() << " "
              << (pos - Vector3f(-1,-1,pos[2])).Length() << "\n";
#elif 0
    static double vel0 = 0.0;
    static double vel1 = 0.0;
    static bool once = true;

    if (once)
    {
        once = false;
        std::cerr << "## Time | pos.z | oldvel.z | vel.z | velZDiff | TimeDiff | vel.z*TimeDiff \n\n";
    }

    mOldGameState.GetBall(oldpos, size);
    mGameState.GetBall(pos, size);

    vel1 = (pos-oldpos).z()/timeDiff;
    if ((vel0 < 0) && !(vel1 <= 0))
    {
        std::cerr << mGameState.GetTime() << " " << pos.z() << " " << vel0 << " " << vel1
                  << " " << vel1 - vel0  << " " << timeDiff << " " << (pos-oldpos).z() << "\n";
    }
    vel0 = vel1;
#elif 0
    static Vector3f vel0;
    static Vector3f vel1;
    static bool once = true;

    if (once)
    {
        once = false;
        std::cerr << "## Time | pos | oldvel | vel | velDiff \n\n";
    }

    mOldGameState.GetBall(oldpos, size);
    mGameState.GetBall(pos, size);


    vel1 = (pos-oldpos)/timeDiff;
    std::cerr << mGameState.GetTime() << " " << pos << " "
              << vel0 << " " << vel1 << " " << vel1 - vel0 << "\n";
    vel0 = vel1;

#endif
    if (mGameState.GetBall(pos, size))
    {
        glDisable(GL_DEPTH_TEST);
        glColor4fv(sBallColor);
        mGLServer.DrawSphere(Get2DPos(pos), b_radius, res);
        if (mDrawVelocity && timeDiff != 0.0)
        {
            glColor3f(1.0f,1.0f,1.0f);
            mOldGameState.GetBall(oldpos);
            mGLServer.DrawArbitraryLine(Get2DPos(pos), Get2DPos(pos + (pos-oldpos)/timeDiff));
        }
        glEnable(GL_DEPTH_TEST);
    }

    glEnable(GL_POINT_SMOOTH);
    glEnable(GL_LINE_SMOOTH);

    glEnable(GL_LIGHTING);

}

void
Monitor::DrawFlag(GameState::EFlagType i, salt::Vector3f pos, int pass)
{
    TFlagInfoMap::const_iterator j = mFlagInfo.find(i);
    FlagInfo fi;
    if (j == mFlagInfo.end())
    {
        memcpy(&fi, &mDefaultFlagInfo, sizeof(FlagInfo));
    } else {
        memcpy(&fi, &(j->second), sizeof(FlagInfo));
    }

    // pass=0, first pass with depth test disabled
    if (pass == 0)
    {
        mGLServer.DrawShadowOfSphere(pos + fi.mOffset, fi.mRadius);
    }
    else
    // pass=1, second pass with depth test
    {
        glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, fi.mColor);
        //glColor4fv(fi.mColor);
        mGLServer.DrawSphere(pos + fi.mOffset, fi.mRadius);
    }
}

void
Monitor::DrawPlayer(TTeamIndex side,
                    int unum, const salt::Vector3f& pos, float size, int pass)
{
    if (pass == 0)
    {
        const GLfloat outerlineColor[] = {0.2f,0.2f,0.2f,1.0f};
        glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, outerlineColor);
        mGLServer.DrawShadowOfSphere(pos, size);
    }
    else
    // pass=1, second pass with depth test
    {
        switch (side)
        {
        case TI_LEFT:
            glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, sTeamColorLeft);
            //glColor4fv(sTeamColorLeft);
            break;
        case TI_RIGHT:
            glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, sTeamColorRight);
            //glColor4fv(sTeamColorRight);
            break;