worldmodelupdate_8c-source.html

来自「仿真人工智能是指用人工的方法和技术」· HTML 代码 · 共 629 行 · 第 1/5 页

00498   <font class="keywordflow">return</font> bReturn;
00499 
00500 }
00501 
00502 <font class="comment">/******************************************************************************/</font>
00503 <font class="comment">/*************** WORLDMODEL: SEE RELATED UPDATES ******************************/</font>
00504 <font class="comment">/******************************************************************************/</font>
00505 
<a name="l00511"></a><a class="code" href="classWorldModel.html#c1">00511</a> <font class="keywordtype">bool</font> <a class="code" href="classWorldModel.html#c1">WorldModel::updateAfterSeeMessage</a>( )
00512 {
00513    <font class="comment">// update the agent (global position and angle using flags and lines)</font>
00514   <font class="keywordflow">if</font>( <a class="code" href="classWorldModel.html#a4">getCurrentTime</a>().<a class="code" href="classTime.html#a3">getTime</a>() != -1 )
00515     <a class="code" href="classWorldModel.html#c2">updateAgentObjectAfterSee</a>( );
00516 
00517   <font class="comment">// walk past all players on the field an when new information was perceived</font>
00518   <font class="comment">// (and put in the relative attributes) update this dynamic object. When it</font>
00519   <font class="comment">// was not seen, convert its global position (this is an estimate from the</font>
00520   <font class="comment">// sense message) to a relative position</font>
00521   <font class="keywordtype">double</font> dConfThr = <a class="code" href="classWorldModel.html#o1">PS</a>-&gt;<a class="code" href="classPlayerSettings.html#a1">getPlayerConfThr</a>();
00522   <font class="keywordtype">int</font>    iIndex;
00523 
00524   <font class="keywordflow">for</font>( <a class="code" href="SoccerTypes_8h.html#a240">ObjectT</a> o = <a class="code" href="classWorldModel.html#a31">iterateObjectStart</a>( iIndex, <a class="code" href="SoccerTypes_8h.html#a241a117">OBJECT_SET_PLAYERS</a>, dConfThr );
00525        o != <a class="code" href="SoccerTypes_8h.html#a240a113">OBJECT_ILLEGAL</a>;
00526        o = <a class="code" href="classWorldModel.html#a32">iterateObjectNext</a> ( iIndex, <a class="code" href="SoccerTypes_8h.html#a241a117">OBJECT_SET_PLAYERS</a>, dConfThr ) )
00527   {
00528     <font class="keywordflow">if</font>( <a class="code" href="classWorldModel.html#a65">getTimeLastSeen</a>( o ) == <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() &amp;&amp;
00529         o != <a class="code" href="classWorldModel.html#a34">getAgentObjectType</a>() )
00530       <a class="code" href="classWorldModel.html#c3">updateDynamicObjectAfterSee</a>   ( o );
00531     <font class="keywordflow">else</font>
00532       <a class="code" href="classWorldModel.html#c10">updateObjectRelativeFromGlobal</a>( o );
00533   }
00534   <a class="code" href="classWorldModel.html#a33">iterateObjectDone</a>( iIndex);
00535 
00536   <font class="comment">// if ball was seen update him, otherwise make estimated global relative</font>
00537   <font class="keywordflow">if</font>( <a class="code" href="classWorldModel.html#a65">getTimeLastSeen</a>( <a class="code" href="SoccerTypes_8h.html#a240a24">OBJECT_BALL</a> ) == <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() )
00538     <a class="code" href="classWorldModel.html#c3">updateDynamicObjectAfterSee</a>   ( <a class="code" href="SoccerTypes_8h.html#a240a24">OBJECT_BALL</a> );
00539   <font class="keywordflow">else</font>
00540     <a class="code" href="classWorldModel.html#c10">updateObjectRelativeFromGlobal</a>( <a class="code" href="SoccerTypes_8h.html#a240a24">OBJECT_BALL</a> );
00541 
00542   <font class="comment">// delete objects from wordmodel that should have been seen, but aren't</font>
00543   <a class="code" href="classWorldModel.html#a101">removeGhosts</a>();
00544 
00545   <font class="keywordflow">return</font> <font class="keyword">true</font>;
00546 }
00547 
<a name="l00552"></a><a class="code" href="classWorldModel.html#c2">00552</a> <font class="keywordtype">bool</font> <a class="code" href="classWorldModel.html#c2">WorldModel::updateAgentObjectAfterSee</a>(  )
00553 {
00554   <a class="code" href="classVecPosition.html">VecPosition</a> posGlobal, velGlobal;
00555   <a class="code" href="Geometry_8h.html#a2">AngDeg</a>      angGlobal;
00556 
00557   <font class="comment">// calculate the state of the agent</font>
00558   <a class="code" href="classWorldModel.html#c11">calculateStateAgent</a>( &amp;posGlobal, &amp;velGlobal, &amp;angGlobal );
00559 
00560   <font class="comment">// and set the needed attributes</font>
00561   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classObject.html#a20">setTimeLastSeen</a>         ( <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00562   <font class="comment">// store difference with predicted global position to compensate for error</font>
00563   <font class="comment">// in global position when global velocity is calculated for other objects.</font>
00564   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classAgentObject.html#a4">setPositionDifference</a>(posGlobal- <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classObject.html#a13">getGlobalPosition</a>());
00565   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classObject.html#a12">setGlobalPosition</a>       ( posGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00566   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classObject.html#a16">setGlobalPositionLastSee</a>( posGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00567   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classAgentObject.html#a13">setGlobalNeckAngle</a>      ( angGlobal );
00568   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classDynamicObject.html#a7">setGlobalVelocity</a>       ( velGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00569 
00570   <font class="keywordflow">return</font> <font class="keyword">true</font>;
00571 }
00572 
00573 
<a name="l00579"></a><a class="code" href="classWorldModel.html#c3">00579</a> <font class="keywordtype">bool</font> <a class="code" href="classWorldModel.html#c3">WorldModel::updateDynamicObjectAfterSee</a>( <a class="code" href="SoccerTypes_8h.html#a240">ObjectT</a> o )
00580 {
00581   <a class="code" href="classVecPosition.html">VecPosition</a> posGlobal, velGlobal;
00582 
00583   <font class="keywordflow">if</font>( o == <a class="code" href="SoccerTypes_8h.html#a240a24">OBJECT_BALL</a> )
00584   {
00585     <a class="code" href="classWorldModel.html#c21">calculateStateBall</a>  ( &amp;posGlobal, &amp;velGlobal );
00586     <a class="code" href="classWorldModel.html#o13">Ball</a>.<a class="code" href="classDynamicObject.html#a7">setGlobalVelocity</a>       ( velGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00587     <a class="code" href="classWorldModel.html#o13">Ball</a>.<a class="code" href="classObject.html#a12">setGlobalPosition</a>       ( posGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00588     <a class="code" href="classWorldModel.html#o13">Ball</a>.<a class="code" href="classObject.html#a16">setGlobalPositionLastSee</a>( posGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00589     <font class="keywordflow">return</font> <font class="keyword">true</font>;
00590   }
00591   <font class="keywordflow">else</font> <font class="keywordflow">if</font>( <a class="code" href="classSoccerTypes.html#d13">SoccerTypes::isKnownPlayer</a>( o ) )
00592   {
00593     <a class="code" href="classWorldModel.html#c28">calculateStatePlayer</a>( o, &amp;posGlobal, &amp;velGlobal );
00594 
00595     <a class="code" href="classPlayerObject.html">PlayerObject</a> *pob = (<a class="code" href="classPlayerObject.html">PlayerObject</a>*) <a class="code" href="classWorldModel.html#c0">getObjectPtrFromType</a>( o );
00596 
00597     pob-&gt;<a class="code" href="classDynamicObject.html#a7">setGlobalVelocity</a>       ( velGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00598     pob-&gt;<a class="code" href="classObject.html#a12">setGlobalPosition</a>       ( posGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00599     pob-&gt;<a class="code" href="classObject.html#a16">setGlobalPositionLastSee</a>( posGlobal, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00600 
00601     <font class="keywordflow">if</font>( pob-&gt;<a class="code" href="classPlayerObject.html#a16">getTimeRelativeAngles</a>() == <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() )
00602     {
00603       <a class="code" href="Geometry_8h.html#a2">AngDeg</a> ang = <a class="code" href="classWorldModel.html#a37">getAgentGlobalNeckAngle</a>()+pob-&gt;<a class="code" href="classPlayerObject.html#a8">getRelativeBodyAngle</a>();
00604       ang = <a class="code" href="classVecPosition.html#d1">VecPosition::normalizeAngle</a>( ang );
00605       pob-&gt;<a class="code" href="classPlayerObject.html#a9">setGlobalBodyAngle</a>( ang, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00606       ang = <a class="code" href="classWorldModel.html#a37">getAgentGlobalNeckAngle</a>() + pob-&gt;<a class="code" href="classPlayerObject.html#a12">getRelativeNeckAngle</a>();
00607       ang = <a class="code" href="classVecPosition.html#d1">VecPosition::normalizeAngle</a>( ang );
00608       pob-&gt;<a class="code" href="classPlayerObject.html#a13">setGlobalNeckAngle</a>( ang, <a class="code" href="classWorldModel.html#a10">getTimeLastSeeMessage</a>() );
00609     }
00610     <font class="keywordflow">return</font> <font class="keyword">true</font>;
00611   }
00612 
00613   <font class="keywordflow">return</font> <font class="keyword">false</font>;
00614 }
00615 
00616 
00617 <font class="comment">/******************************************************************************/</font>
00618 <font class="comment">/******************** SENSE RELATED UPDATES ***********************************/</font>
00619 <font class="comment">/******************************************************************************/</font>
00620 
<a name="l00629"></a><a class="code" href="classWorldModel.html#c4">00629</a> <font class="keywordtype">bool</font> <a class="code" href="classWorldModel.html#c4">WorldModel::updateAfterSenseMessage</a>( )
00630 {
00631   <font class="comment">// update agent information</font>
00632   <a class="code" href="classWorldModel.html#c5">updateAgentAndBallAfterSense</a>( );
00633 
00634   <font class="comment">// update all global information of players that have a good confidence</font>
00635   <font class="keywordtype">double</font> dConfThr = <a class="code" href="classWorldModel.html#o1">PS</a>-&gt;<a class="code" href="classPlayerSettings.html#a1">getPlayerConfThr</a>();
00636   <font class="keywordtype">int</font>    iIndex;
00637 
00638   <font class="keywordflow">for</font>( <a class="code" href="SoccerTypes_8h.html#a240">ObjectT</a> o = <a class="code" href="classWorldModel.html#a31">iterateObjectStart</a>( iIndex, <a class="code" href="SoccerTypes_8h.html#a241a117">OBJECT_SET_PLAYERS</a>, dConfThr );
00639        o != <a class="code" href="SoccerTypes_8h.html#a240a113">OBJECT_ILLEGAL</a>;
00640        o = <a class="code" href="classWorldModel.html#a32">iterateObjectNext</a> ( iIndex, <a class="code" href="SoccerTypes_8h.html#a241a117">OBJECT_SET_PLAYERS</a>, dConfThr ) )
00641   {
00642     <font class="keywordflow">if</font>( o != <a class="code" href="classWorldModel.html#a34">getAgentObjectType</a>() )
00643       <a class="code" href="classWorldModel.html#c7">updateDynamicObjectForNextCycle</a>( o,
00644                                  <a class="code" href="classWorldModel.html#a4">getCurrentTime</a>() - <a class="code" href="classWorldModel.html#a50">getTimeGlobalPosition</a>(o) );
00645   }