restrictedvisionperceptor.cpp

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

void
RestrictedVisionPerceptor::ApplyNoise(ObjectData& od) const
{
    if (mAddNoise)
    {
        od.mDist  += (*(mDistRng.get()))() * od.mDist / 100.0;
        od.mTheta += (*(mThetaRng.get()))();
        od.mPhi   += (*(mPhiRng.get()))();
    }
}


bool
RestrictedVisionPerceptor::StaticAxisPercept(boost::shared_ptr<PredicateList> predList)
{
    Predicate& predicate = predList->AddPredicate();
    predicate.name       = mPredicateName;
    predicate.parameter.Clear();

    TTeamIndex  ti       = mAgentState->GetTeamIndex();
    salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();

    TObjectList visibleObjects;
    SetupVisibleObjects(visibleObjects);

    for (std::list<ObjectData>::iterator i = visibleObjects.begin();
         i != visibleObjects.end(); ++i)
    {
        ObjectData& od = (*i);

        od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
        if (mAddNoise)
        {
            od.mRelPos += mError;
        }

        if (
            (od.mRelPos.Length() <= 0.1) ||
            (CheckOcclusion(myPos,od))
            )
        {
            // object is occluded or too close
            continue;
        }

        // theta is the angle in the X-Y (horizontal) plane
        assert(gAbs(GetPan()) <= 360);
        od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0])) - GetPan();
        od.mTheta = gNormalizeDeg(od.mTheta);
        // latitude
        assert(gAbs(GetTilt()) <= 360);
        od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist)) - GetTilt();
        od.mPhi = gNormalizeDeg(od.mPhi);

        // make some noise
        ApplyNoise(od);
        // check if the object is in the current field of view
        if (gAbs(od.mTheta) > mHViewCone) continue;
        if (gAbs(od.mPhi) > mVViewCone) continue;

        // generate a sense entry
        AddSense(predicate,od);
    }

    if (mSenseMyPos)
    {
        Vector3f sensedMyPos = SoccerBase::FlipView(myPos, ti);

        ParameterList& element = predicate.parameter.AddList();
        element.AddValue(std::string("mypos"));
        element.AddValue(sensedMyPos[0]);
        element.AddValue(sensedMyPos[1]);
        element.AddValue(sensedMyPos[2]);
    }

    return true;
}

bool
RestrictedVisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
{
    Predicate& predicate = predList->AddPredicate();
    predicate.name       = mPredicateName;
    predicate.parameter.Clear();

    const int hAngle_2 = mHViewCone>>1;
    const int vAngle_2 = mVViewCone>>1;

    TTeamIndex  ti = mAgentState->GetTeamIndex();

    const Vector3f& up = mTransformParent->GetWorldTransform().Up();

    // calc the percptors angle in the horizontal plane
    double fwTheta = gNormalizeRad(Vector2f(up[0],up[1]).GetAngleRad());

    // calc the perceptors angle in the vertical plane
    // for this the vector has to rotated, i.e. you cannot just use x and z component
    //double fwPhi = gNormalizeRad(Vector2f(Vector2f(up[0],up[1]).Length(),up[2]).GetAngleRad());

    // assume that perceptor is always horizontal.
    // FIXME: this is magic
    double fwPhi = 0.0;


    TObjectList visibleObjects;
    SetupVisibleObjects(visibleObjects);

    // log for 7th agent of the first team
    if ((mAgentState->GetTeamIndex() == 1) && (mAgentState->GetUniformNumber() ==7))
        GetLog()->Debug() << "percept: " << visibleObjects.size() << " objects. :::"
                          << up << " theta " << gRadToDeg(fwTheta)
                          << " phi " << gRadToDeg(fwPhi) << "\n";

    for (std::list<ObjectData>::iterator i = visibleObjects.begin();
         i != visibleObjects.end(); ++i)
    {
        ObjectData& od = (*i);


        od.mRelPos = SoccerBase::FlipView(od.mRelPos, ti);
        if ((mAgentState->GetTeamIndex() == 1) && (mAgentState->GetUniformNumber() ==7))
            GetLog()->Debug() << "object " << od.mObj->GetPerceptName()
                              << " at : " << od.mRelPos << "\n";

        if (mAddNoise)
        {
            od.mRelPos += mError;
        }

        if (od.mRelPos.Length() <= 0.1)
        {
            // object is too close
            continue;
        }

        // theta is the angle in horizontal plane, with fwAngle as 0 degree
        od.mTheta = gRadToDeg(gNormalizeRad(
                                  Vector2f(od.mRelPos[0],od.mRelPos[1]).GetAngleRad() -
                                  fwTheta
                                  ));
        // flags are always visible
        if ((gAbs(od.mTheta) > hAngle_2) && (od.mObj->GetPerceptName() != "Flag"))
        {
            // object is out of view range
//                                 GetLog()->Debug() << "(RestrictedVisionPerceptor) Omitting "
//                               << od.mObj->GetPerceptName() << od.mObj->GetID()
//                               << ": h-angle = " << od.mTheta << ".\n" ;
            continue;
        }


        // latitude with fwPhi as 0 degreee
        od.mPhi = gRadToDeg(gNormalizeRad(
                                Vector2f(
                                    Vector2f(od.mRelPos[0],od.mRelPos[1]).Length(),
                                    od.mRelPos[2]).GetAngleRad() - fwPhi
                                ));


        if ((gAbs(od.mPhi) > vAngle_2) && (od.mObj->GetPerceptName() != "Flag"))
            continue;

        // log for 7th agent of the first team
        if ((mAgentState->GetTeamIndex() == 1) && (mAgentState->GetUniformNumber() ==7))
            GetLog()->Debug() << "percept: " << "adding object: "
                              << gAbs(od.mPhi) << ":" << vAngle_2 << "\n";

        // make some noise
        ApplyNoise(od);

        // generate a sense entry
        AddSense(predicate,od);

    }

    if (mSenseMyPos)
    {
        salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();

        Vector3f sensedMyPos = myPos;
        SoccerBase::FlipView(sensedMyPos, ti);

        ParameterList& element = predicate.parameter.AddList();
        element.AddValue(std::string("mypos"));
        element.AddValue(sensedMyPos[0]);
        element.AddValue(sensedMyPos[1]);
        element.AddValue(sensedMyPos[2]);
    }
    return true;
}

bool
RestrictedVisionPerceptor::Percept(boost::shared_ptr<PredicateList> predList)
{
    if (
        (mTransformParent.get() == 0) ||
        (mActiveScene.get() == 0) ||
        (mAgentState.get() == 0)
        )
    {
        return false;
    }
#if 1
    return StaticAxisPercept(predList);
#else
    return mStaticSenseAxis ?
        StaticAxisPercept(predList) :
        DynamicAxisPercept(predList);
#endif
}

bool RestrictedVisionPerceptor::CheckOcclusion(const Vector3f& my_pos, const ObjectData& od) const
{
    // (occlusion test disabled for now, every object is visible)
    return false;

//     if (mRay.get() == 0) return;

//     // sort objects wrt their distance
//     visible_objects.sort();
//     // check visibility
//     std::list<ObjectData>::iterator start = visible_objects.begin();
//     ++start;

//     // this is going to be expensive now: to check occlusion of an object,
//     // we have to check all closer objects. For n objects, we have to
//     // check at most n*(n-1)/2 objects.
//     for (std::list<ObjectData>::iterator i = start;
//          i != visible_objects.end(); ++i)
//     {
//         for (std::list<ObjectData>::iterator j = visible_objects.begin();
//              j != i; ++j)
//         {
//             // cast ray from camera to object (j)
//             mRay->SetParams(j->mRelPos,my_pos,j->mDist);

//             dContactGeom contact;

//             shared_ptr<Collider> collider = shared_static_cast<Collider>
//                 (i->mObj->GetChildSupportingClass("Collider"));

//             if (mRay->Intersects(collider))
//                 {
//                     j->mVisible = false;
//                     j = i;
//                 }
//         }
//     }
}

void
RestrictedVisionPerceptor::SetSenseMyPos(bool sense)
{
    mSenseMyPos = sense;
}