restrictedvisionperceptor.cpp

rcssserver3d Robocup 3D比赛官方指定平台

        ParameterList& element = predicate.parameter.AddList();
        element.AddValue(std::string("P"));

        ParameterList player;
        player.AddValue(std::string("team"));
        player.AddValue
            (std::string
                (agent_state->GetPerceptName(ObjectState::PT_Player)
                )
            );
        element.AddValue(player);

        if (! agent_state->GetID().empty())
        {
            ParameterList id;
            id.AddValue(std::string("id"));
            id.AddValue(agent_state->GetID());
            element.AddValue(id);
        }

        for (TObjectList::iterator j = objectList.begin();
            j != objectList.end(); ++j)
        {
            ObjectData& od = (*j);

            if (!od.mObj->GetID().empty())
            {
                ParameterList id;
                id.AddValue(od.mObj->GetID());

                ParameterList position;
                position.AddValue(std::string("pol"));
                position.AddValue(od.mDist);
                position.AddValue(od.mTheta);
                position.AddValue(od.mPhi);
                id.AddValue(position);

                element.AddValue(id);
            }
        }
    }
    else
    {
        for (TObjectList::iterator j = objectList.begin();
            j != objectList.end(); ++j)
        {
            ObjectData& od = (*j);
            ParameterList& element = predicate.parameter.AddList();
            element.AddValue(od.mObj->GetPerceptName());

            ParameterList& position = element.AddList();
            position.AddValue(std::string("pol"));
            position.AddValue(od.mDist);
            position.AddValue(od.mTheta);
            position.AddValue(od.mPhi);
        }
    }
}

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();

    TNodeObjectsMap visibleNodes;
    SetupVisibleNodes(visibleNodes);

    for (TNodeObjectsMap::iterator i = visibleNodes.begin();
        i != visibleNodes.end(); ++i)
    {
        shared_ptr<BaseNode> node   = (*i).first;
        TObjectList& visibleObjects = (*i).second;

        for (TObjectList::iterator j = visibleObjects.begin();
            j != visibleObjects.end();)
        {
            ObjectData& od = (*j);

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

            if (od.mRelPos.Length() <= 0.1 ||
                (CheckOcclusion(myPos,od))
               )
            {
                // object is too close
                j = visibleObjects.erase(j);
                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);

            if (gAbs(od.mTheta) > mHViewCone ||
                gAbs(od.mPhi) > mVViewCone
               )
            {
                j = visibleObjects.erase(j);
                continue;
            }

            // make some noise
            ApplyNoise(od);

            ++j;
        }

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

    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;

    // get the transformation matrix describing the current orientation
    const Matrix& mat = mTransformParent->GetWorldTransform();

    TNodeObjectsMap visibleNodes;
    SetupVisibleNodes(visibleNodes);

    for (TNodeObjectsMap::iterator i = visibleNodes.begin();
        i != visibleNodes.end(); ++i)
    {
        shared_ptr<BaseNode> node   = (*i).first;
        TObjectList& visibleObjects = (*i).second;

        for (TObjectList::iterator j = visibleObjects.begin();
            j != visibleObjects.end();)
        {
            ObjectData& od = (*j);

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

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

            // determine position relative to the local reference frame
            Vector3f localRelPos = mat.InverseRotate(od.mRelPos);

            // theta is the angle in horizontal plane, with fwAngle as 0 degree
            od.mTheta = gNormalizeDeg (gRadToDeg(gNormalizeRad(
                gArcTan2(localRelPos[1],localRelPos[0])
                )) -90 );

            if (gAbs(od.mTheta) > hAngle_2)
            {
                // object is out of view range
                // GetLog()->Debug() << "(RestrictedVisionPerceptor) Omitting "
                //     << od.mObj->GetPerceptName() << od.mObj->GetID()
                //     << ": h-angle = " << od.mTheta << ".\n" ;
                j = visibleObjects.erase(j);
                continue;
            }

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

            if (gAbs(od.mPhi) > vAngle_2)
            {
                j = visibleObjects.erase(j);
                continue;
            }

            // make some noise
            ApplyNoise(od);

            ++j;
        }

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

    if (mSenseMyPos)
    {
        TTeamIndex  ti       = mAgentState->GetTeamIndex();
        salt::Vector3f myPos = mTransformParent->GetWorldTransform().Pos();
        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::Percept(boost::shared_ptr<PredicateList> predList)
{
    if (
        (mTransformParent.get() == 0) ||
        (mActiveScene.get() == 0) ||
        (mAgentAspect.get() == 0) ||
        (mAgentState.get() == 0)
        )
    {
        return false;
    }

    return mStaticSenseAxis ?
        StaticAxisPercept(predList) :
        DynamicAxisPercept(predList);
}

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;
}