worldmodelupdate_8c-source.html

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

00646   <a class="code" href="classWorldModel.html#a33">iterateObjectDone</a>( iIndex);
00647 
00648   <font class="comment">// update ball for collision with players</font>
00649   <a class="code" href="classWorldModel.html#c8">updateBallForCollision</a>();
00650 
00651   <font class="keywordflow">return</font> <font class="keyword">true</font>;
00652 }
00653 
<a name="l00659"></a><a class="code" href="classWorldModel.html#c5">00659</a> <font class="keywordtype">bool</font> <a class="code" href="classWorldModel.html#c5">WorldModel::updateAgentAndBallAfterSense</a>( )
00660 {
00661   <font class="comment">// get info from commands from previous cycle (if timestopped current cycle)</font>
00662   <font class="keywordtype">bool</font>        bProcessedHole = <font class="keyword">false</font>;
00663   <font class="keywordtype">bool</font>        bBallUpdated   = <font class="keyword">false</font>;
00664   <a class="code" href="classVecPosition.html">VecPosition</a> pos            = <a class="code" href="classWorldModel.html#a43">getAgentGlobalPosition</a>();
00665   <a class="code" href="Geometry_8h.html#a2">AngDeg</a>      angGlobalNeck  = <a class="code" href="classWorldModel.html#a37">getAgentGlobalNeckAngle</a>();
00666   <a class="code" href="Geometry_8h.html#a2">AngDeg</a>      angGlobalBody  = <a class="code" href="classWorldModel.html#a38">getAgentGlobalBodyAngle</a>();
00667   <a class="code" href="classStamina.html">Stamina</a>     sta            = <a class="code" href="classWorldModel.html#a39">getAgentStamina</a>();
00668   <a class="code" href="classVecPosition.html">VecPosition</a> vel            = <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classAgentObject.html#a11">getSpeedRelToNeck</a>(); <font class="comment">// !!</font>
00669   <a class="code" href="classTime.html">Time</a>        time           = <a class="code" href="classWorldModel.html#a4">getCurrentTime</a>() - 1 ;
00670 
00671   <font class="comment">// calculate velocity at end of previous cycle using velocity from current</font>
00672   <font class="comment">// cycle. Although we do not know direction yet (this is relative to neck</font>
00673   <font class="comment">// which is not yet known), we can use the magnitude to determine travelled</font>
00674   <font class="comment">// distance (speed) of the agent</font>
00675   <font class="comment">// After neck angle is estimated, we can rotate velocity vector to get</font>
00676   <font class="comment">// actual velocity.</font>
00677   vel.<a class="code" href="classVecPosition.html#a31">setMagnitude</a>( vel.<a class="code" href="classVecPosition.html#a32">getMagnitude</a>()/<a class="code" href="classWorldModel.html#o0">SS</a>-&gt;<a class="code" href="classServerSettings.html#a8">getPlayerDecay</a>() );
00678 
00679   <font class="comment">// turn_neck is done before other commands, so should be estimated first</font>
00680   <font class="keywordtype">int</font> i = (int)<a class="code" href="SoccerTypes_8h.html#a247a183">CMD_TURNNECK</a>;
00681   <font class="keywordflow">if</font>( <a class="code" href="classWorldModel.html#o25">performedCommands</a>[i] == <font class="keyword">true</font> &amp;&amp;           <font class="comment">// turn_neck was performed</font>
00682       ( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a3">getTime</a>() == time.<a class="code" href="classTime.html#a3">getTime</a>() ||      <font class="comment">// no hole</font>
00683         <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a3">getTime</a>() == time.<a class="code" href="classTime.html#a3">getTime</a>() - 1 ) ) <font class="comment">// hole</font>
00684   {
00685 
00686     <font class="keywordflow">if</font>( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a3">getTime</a>() == time.<a class="code" href="classTime.html#a3">getTime</a>() - 1  &amp;&amp;
00687         time.<a class="code" href="classTime.html#a3">getTime</a>() &gt; 0)
00688     {
00689       bProcessedHole = <font class="keyword">true</font>;
00690       <a class="code" href="classWorldModel.html#m0">iNrHoles</a>++;
00691     }
00692 
00693     <a class="code" href="classWorldModel.html#a102">predictStateAfterCommand</a>( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i], &amp;pos, &amp;vel, &amp;angGlobalBody,
00694                               &amp;angGlobalNeck, &amp;sta ) ;
00695     <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a1">updateTime</a>( -1 );
00696   }
00697 
00698 
00699   <font class="comment">// now all the other commands</font>
00700   <font class="keywordflow">for</font>( i = 0; i &lt; <a class="code" href="SoccerTypes_8h.html#a5">MAX_COMMANDS</a>; i ++  )
00701   {
00702     <font class="keywordflow">if</font>( <a class="code" href="classWorldModel.html#o25">performedCommands</a>[i] == <font class="keyword">true</font> &amp;&amp; <font class="comment">// sense msg indicates we executed this</font>
00703         ( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a3">getTime</a>() == time.<a class="code" href="classTime.html#a3">getTime</a>() ||      <font class="comment">// no hole</font>
00704           <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a3">getTime</a>() == time.<a class="code" href="classTime.html#a3">getTime</a>() - 1 ) )<font class="comment">// hole</font>
00705     {
00706       <font class="keywordflow">if</font>( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a3">getTime</a>() == time.<a class="code" href="classTime.html#a3">getTime</a>() - 1 &amp;&amp;
00707           bProcessedHole == <font class="keyword">false</font> &amp;&amp; time.<a class="code" href="classTime.html#a3">getTime</a>() &gt; 0 )
00708       {
00709         bProcessedHole = <font class="keyword">true</font>;                                 <font class="comment">// process hole</font>
00710         <a class="code" href="classWorldModel.html#m0">iNrHoles</a>++;
00711       }
00712 
00713       <font class="keywordflow">switch</font>( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m1">commandType</a> )
00714       {
00715         <font class="keywordflow">case</font> <a class="code" href="SoccerTypes_8h.html#a247a186">CMD_KICK</a>:
00716           <a class="code" href="classWorldModel.html#c6">updateBallAfterKick</a>( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].dPower,
00717                                <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].dAngle);
00718           bBallUpdated  = <font class="keyword">true</font>;             <font class="comment">// should not be updated later on.        </font>
00719           <font class="keywordflow">break</font>;
00720         <font class="keywordflow">case</font> <a class="code" href="SoccerTypes_8h.html#a247a182">CMD_TURN</a>:
00721           <a class="code" href="classWorldModel.html#a102">predictStateAfterCommand</a>( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i], &amp;pos, &amp;vel,
00722                                     &amp;angGlobalBody, &amp;angGlobalNeck, &amp;sta );                                  
00723           <font class="keywordflow">break</font>;
00724         <font class="keywordflow">case</font> <a class="code" href="SoccerTypes_8h.html#a247a187">CMD_MOVE</a>:
00725           pos.<a class="code" href="classVecPosition.html#a29">setVecPosition</a>( <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].dX, <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].dY );
00726           <a class="code" href="classWorldModel.html#c13">initParticlesAgent</a>( pos );
00727           <font class="keywordflow">break</font>;
00728         <font class="keywordflow">case</font> <a class="code" href="SoccerTypes_8h.html#a247a181">CMD_DASH</a>:       <font class="comment">// not necessary to do anything, since resulting</font>
00729                              <font class="comment">// velocity is already available from sense, this</font>
00730                              <font class="comment">// velocity can be used to update global position</font>
00731           <font class="keywordflow">break</font>;
00732         <font class="keywordflow">case</font> <a class="code" href="SoccerTypes_8h.html#a247a183">CMD_TURNNECK</a>:   <font class="comment">// already handled before this loop</font>
00733           <font class="keywordflow">default</font>:
00734             <font class="keywordflow">break</font>;
00735       }
00736       <a class="code" href="classWorldModel.html#o24">queuedCommands</a>[i].<a class="code" href="classSoccerCommand.html#m0">time</a>.<a class="code" href="classTime.html#a1">updateTime</a>( -1 );                  <font class="comment">// processed</font>
00737     }
00738   }
00739 
00740 
00741   <font class="comment">// reset pos and vel information to previous cycle (since can be changed in</font>
00742   <font class="comment">// predictStateAfterCommand)</font>
00743   vel = <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classAgentObject.html#a11">getSpeedRelToNeck</a>();
00744   pos = <a class="code" href="classWorldModel.html#a43">getAgentGlobalPosition</a>();
00745   vel.<a class="code" href="classVecPosition.html#a31">setMagnitude</a>( vel.<a class="code" href="classVecPosition.html#a32">getMagnitude</a>()/<a class="code" href="classWorldModel.html#o0">SS</a>-&gt;<a class="code" href="classServerSettings.html#a8">getPlayerDecay</a>() );
00746   vel.<a class="code" href="classVecPosition.html#a47">rotate</a>( angGlobalNeck ); <font class="comment">// rotate velocity using information about neck</font>
00747 
00748   <font class="comment">// update particles that keep track of position of agent using this vel</font>
00749   <a class="code" href="classWorldModel.html#c16">updateParticlesAgent</a>( vel, <font class="keyword">true</font> );
00750 
00751   <font class="comment">// predict the global position using the calculated velocity at the end of</font>
00752   <font class="comment">// the previous cycle (power and direction can thus both be set to zero).</font>
00753   <font class="comment">// There is just little noise in this perception, so gives a good estimate</font>
00754   <a class="code" href="classWorldModel.html#a105">predictStateAfterDash</a>( 0.0, &amp;pos, &amp;vel, &amp;sta, 0 );
00755 
00756   <font class="comment">// update ball if it hasn't been done yet</font>
00757   <font class="keywordflow">if</font>( !bBallUpdated )
00758   {
00759     <a class="code" href="classWorldModel.html#c7">updateDynamicObjectForNextCycle</a>( <a class="code" href="SoccerTypes_8h.html#a240a24">OBJECT_BALL</a>, 1 );
00760     <a class="code" href="classWorldModel.html#c25">updateParticlesBall</a>( <a class="code" href="classWorldModel.html#o22">particlesPosBall</a>, <a class="code" href="classWorldModel.html#o23">particlesVelBall</a>, <a class="code" href="classWorldModel.html#r1">iNrParticlesBall</a>, 0, 0 );    
00761   }
00762 
00763   <font class="comment">// body angle is not set since relative angle to neck is already contained</font>
00764   <font class="comment">// in sense_body message, same holds for stamina</font>
00765   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classObject.html#a12">setGlobalPosition</a> ( pos,        <a class="code" href="classWorldModel.html#a4">getCurrentTime</a>() );
00766   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classDynamicObject.html#a7">setGlobalVelocity</a> ( vel,        <a class="code" href="classWorldModel.html#a4">getCurrentTime</a>() );
00767   <a class="code" href="classWorldModel.html#o14">agentObject</a>.<a class="code" href="classAgentObject.html#a13">setGlobalNeckAngle</a>( angGlobalNeck );
00768 
00769 
00770   <font class="keywordflow">return</font> <font class="keyword">true</font>;
00771 }
00772 
<a name="l00781"></a><a class="code" href="classWorldModel.html#c6">00781</a> <font class="keywordtype">bool</font> <a class="code" href="classWorldModel.html#c6">WorldModel::updateBallAfterKick</a>( <font class="keywordtype">double</font> dPower, <a class="code" href="Geometry_8h.html#a2">AngDeg</a> ang )
00782 {
00783   <font class="keywor