bool
VisionPerceptor::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
od.mTheta = salt::gRadToDeg(salt::gArcTan2(od.mRelPos[1], od.mRelPos[0]));
// latitude
od.mPhi = 90.0 - salt::gRadToDeg(salt::gArcCos(od.mRelPos[2]/od.mDist));
// make some noise
ApplyNoise(od);
// 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 CLiteObjectMgr::RemoveObjectFromList(TObjectList &aList, GameBaseLite *pObject)
{
TObjectList::iterator iObj = std::find(aList.begin(), aList.end(), pObject);
if (iObj == aList.end())
return false;
*iObj = 0;
return true;
}
void CLiteObjectMgr::RemoveObjectList(TObjectList &aList)
{
// Remove the objects in the engine
TObjectList::iterator iCurObj = aList.begin();
for (; iCurObj != aList.end(); ++iCurObj)
{
if (*iCurObj)
{
GameBaseLite *pObj = *iCurObj;
// Clear out the entry in the list so we don't think we have it any more
*iCurObj = 0;
g_pLTServer->RemoveObject(pObj->GetClass(), pObj);
}
}
// Forget...
aList.swap(TObjectList());
}
void CLiteObjectMgr::CleanList(TObjectList &aList)
{
// Clear nulls from the end of the list
while (!aList.empty() && (aList.back() == 0))
aList.pop_back();
// Swap empties to the end of the list and get rid of them
TObjectList::iterator iCurObj = aList.begin();
while (iCurObj != aList.end())
{
if (!*iCurObj)
{
*iCurObj = aList.back();
aList.pop_back();
}
else
++iCurObj;
}
}
bool
VisionPerceptor::DynamicAxisPercept(boost::shared_ptr<PredicateList> predList)
{
Predicate& predicate = predList->AddPredicate();
predicate.name = mPredicateName;
predicate.parameter.Clear();
// get the transformation matrix describing the current orientation
const Matrix& mat = mTransformParent->GetWorldTransform();
TObjectList visibleObjects;
SetupVisibleObjects(visibleObjects);
for (std::list<ObjectData>::iterator i = visibleObjects.begin();
i != visibleObjects.end(); ++i)
{
ObjectData& od = (*i);
if (mAddNoise)
{
od.mRelPos += mError;
}
if (od.mRelPos.Length() <= 0.1)
{
// object is too close
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 );
// latitude with fwPhi as 0 degreee
od.mPhi = gRadToDeg(gNormalizeRad(
gArcTan2( localRelPos[2], Vector2f(localRelPos[0], localRelPos[1]).Length())
));
// make some noise
ApplyNoise(od);
// generate a sense entry
AddSense(predicate,od);
}
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 CLiteObjectMgr::IsObjectInList(TObjectList &aList, GameBaseLite *pObject)
{
return std::find(aList.begin(), aList.end(), pObject) != aList.end();
}
void
RestrictedVisionPerceptor::AddSense(Predicate& predicate,
boost::shared_ptr<BaseNode> node,
TObjectList& objectList) const
{
if (objectList.empty())
{
return;
}
boost::shared_ptr<AgentAspect> agent_aspect =
dynamic_pointer_cast<AgentAspect>(node);
if (agent_aspect != 0)
{
boost::shared_ptr<AgentAspect> aspect =
agent_aspect->FindParentSupportingClass<AgentAspect>().lock();
if (aspect != 0)
{
agent_aspect = aspect;
}
boost::shared_ptr<AgentState> agent_state = static_pointer_cast<AgentState>
(agent_aspect->GetChildOfClass("AgentState",true));
if (agent_state.get() == 0 ||
(agent_state->GetPerceptName(ObjectState::PT_Player).empty())
)
{
return;
}
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);
}
}
}