soccer_math.h

説明を見る。
00001 // -*-c++-*-
00002 
00008 /*
00009  *Copyright:
00010 
00011  Copyright (C) Hidehisa AKIYAMA
00012 
00013  This code is free software; you can redistribute it and/or
00014  modify it under the terms of the GNU Lesser General Public
00015  License as published by the Free Software Foundation; either
00016  version 2.1 of the License, or (at your option) any later version.
00017 
00018  This library is distributed in the hope that it will be useful,
00019  but WITHOUT ANY WARRANTY; without even the implied warranty of
00020  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00021  Lesser General Public License for more details.
00022 
00023  You should have received a copy of the GNU Lesser General Public
00024  License along with this library; if not, write to the Free Software
00025  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00026 
00027  *EndCopyright:
00028  */
00029 
00031 
00032 #ifndef RCSC_SOCCERMATH_H
00033 #define RCSC_SOCCERMATH_H
00034 
00035 #include <rcsc/geom/vector_2d.h>
00036 
00037 namespace rcsc {
00038 
00040 const double SERVER_EPS = 1.0e-10;
00041 
00043 // kick command related
00044 
00045 /*-------------------------------------------------------------------*/
00058 inline
00059 double
00060 kick_rate( const double & dist,
00061            const double & dir_diff,
00062            const double & kprate,
00063            const double & bsize,
00064            const double & psize,
00065            const double & kmargin )
00066 {
00067     return kprate * ( 1.0
00068                       - 0.25 * std::fabs( dir_diff ) / 180.0
00069                       - 0.25 * ( dist - bsize - psize ) / kmargin );
00070 }
00071 
00073 // turn command related
00074 
00075 /*-------------------------------------------------------------------*/
00084 inline
00085 double
00086 effective_turn( const double & turn_moment,
00087                 const double & speed,
00088                 const double & inertiamoment )
00089 {
00090     return turn_moment / ( 1.0 + inertiamoment * speed );
00091 }
00092 
00094 // dash command related
00095 
00096 /*-------------------------------------------------------------------*/
00107 inline
00108 double
00109 final_speed( const double & dash_power,
00110              const double & dprate,
00111              const double & effort,
00112              const double & decay )
00113 {
00114     // if player continue to run using the same dash power
00115     // archieved speed at n step later is sum of infinite geometric series
00116 
00117     // !! NOTE !!
00118     // You must compare this value to the PlayerType::playerSpeedMax()
00119 
00120     //return ( (dash_power * dprate * effort) // == accel
00121     //         * (1.0 / (1.0 - decay)) ); // == sum inf geom series
00122     return ( ( std::fabs( dash_power ) * dprate * effort ) // == accel
00123              / ( 1.0 - decay ) ); // == sum inf geom series
00124 }
00125 
00126 /*-------------------------------------------------------------------*/
00137 inline
00138 bool
00139 can_over_speed_max( const double & dash_power,
00140                     const double & dprate,
00141                     const double & effort,
00142                     const double & decay,
00143                     const double & speed_max )
00144 
00145 {
00146     return ( std::fabs( dash_power ) * dprate * effort // max accel
00147              > speed_max * ( 1.0 - decay ) ); // is over speed decay
00148 }
00149 
00151 // predictor method for inertia movement
00152 
00153 /*-------------------------------------------------------------------*/
00162 inline
00163 Vector2D
00164 inertia_n_step_travel( const Vector2D & initial_vel,
00165                        const int n_step,
00166                        const double & decay )
00167 {
00168     return Vector2D( initial_vel )
00169         *= ( ( 1.0 - std::pow( decay, n_step ) ) / ( 1.0 - decay ) );
00170 }
00171 
00172 /*-------------------------------------------------------------------*/
00182 inline
00183 Vector2D
00184 inertia_n_step_point( const Vector2D & initial_pos,
00185                       const Vector2D & initial_vel,
00186                       const int n_step,
00187                       const double & decay )
00188 {
00189     /*
00190       return
00191       initial_pos
00192       + initial_vel * ( (1.0 - std::pow(decay, static_cast<double>(n_step)))
00193       / (1.0 - decay) );
00194     */
00195     /*
00196       return Vector2D(initial_vel
00197       * (1.0 - std::pow(decay, n_step) / (1.0 - decay)))
00198       += initial_pos;
00199     */
00200     /*
00201       double rate = ( (1.0 - std::pow(decay, static_cast<double>(n_step)))
00202       / (1.0 - decay) );
00203       return initial_pos + ( initial_vel * rate );
00204     */
00205     return Vector2D( initial_pos )
00206         += inertia_n_step_travel( initial_vel, n_step, decay );
00207 }
00208 
00209 /*-------------------------------------------------------------------*/
00218 inline
00219 double
00220 inertia_n_step_distance( const double & initial_speed,
00221                          const int n_step,
00222                          const double & decay )
00223 {
00224     return initial_speed
00225         * ( 1.0 - std::pow( decay, n_step ) )
00226         / ( 1.0 - decay );
00227 }
00228 
00229 /*-------------------------------------------------------------------*/
00238 inline
00239 double
00240 inertia_n_step_distance( const double & initial_speed,
00241                          const double & n_step_real,
00242                          const double & decay )
00243 {
00244     return initial_speed
00245         * ( 1.0 - std::pow( decay, n_step_real ) )
00246         / ( 1.0 - decay );
00247 }
00248 
00249 
00250 /*-------------------------------------------------------------------*/
00257 inline
00258 Vector2D
00259 inertia_final_travel( const Vector2D & initial_vel,
00260                       const double & decay )
00261 {
00262     return Vector2D( initial_vel ) /= ( 1.0 - decay );
00263 }
00264 
00265 /*-------------------------------------------------------------------*/
00273 inline
00274 Vector2D
00275 inertia_final_point( const Vector2D & initial_pos,
00276                      const Vector2D & initial_vel,
00277                      const double & decay )
00278 {
00279     /*
00280       return Vector2D(initial_vel / (1.0 - decay)) += initial_pos;
00281     */
00282     /*
00283       Vector2D end_pos(initial_vel);
00284       end_pos /= (1.0 - decay);
00285       return end_pos += initial_pos;
00286     */
00287     /*
00288       return initial_pos + ( initial_vel / (1.0 - decay) );
00289     */
00290     return Vector2D( initial_pos )
00291         += inertia_final_travel( initial_vel, decay );
00292 }
00293 
00294 /*-------------------------------------------------------------------*/
00301 inline
00302 double
00303 inertia_final_distance( const double & initial_speed,
00304                         const double & decay )
00305 {
00306     return initial_speed / ( 1.0 - decay );
00307 }
00308 
00310 // localization
00311 
00312 /*-------------------------------------------------------------------*/
00321 inline
00322 double
00323 quantize( const double & value,
00324           const double & qstep )
00325 {
00326     return rint( value / qstep ) * qstep;
00327 }
00328 
00329 /*-------------------------------------------------------------------*/
00348 inline
00349 double
00350 quantize_dist( const double & unq_dist,
00351                const double & qstep )
00352 {
00353     return quantize( std::exp
00354                      ( quantize( std::log
00355                                  ( unq_dist + SERVER_EPS ), qstep ) ), 0.1 );
00356 }
00357 
00358 /*-------------------------------------------------------------------*/
00365 inline
00366 double
00367 unquantize_min( const double & dist,
00368                 const double & qstep )
00369 {
00370     return ( rint( dist / qstep ) - 0.5 ) * qstep;
00371 }
00372 
00373 /*-------------------------------------------------------------------*/
00380 inline
00381 double
00382 unquantize_max( const double & dist,
00383                 const double & qstep )
00384 {
00385     return ( rint( dist / qstep ) + 0.5 ) * qstep;
00386 }
00387 
00388 /*-------------------------------------------------------------------*/
00400 Vector2D
00401 wind_effect( const double & speed,
00402              const double & weight,
00403              const double & wind_force,
00404              const double & wind_dir,
00405              const double & wind_weight,
00406              const double & wind_rand,
00407              Vector2D * wind_error );
00408 
00409 /*-------------------------------------------------------------------*/
00416 double
00417 unquantize_error( const double & see_dist,
00418                   const double & qstep );
00419 
00420 } // end namespace
00421 
00422 #endif

librcscに対してThu May 1 15:41:22 2008に生成されました。  doxygen 1.5.0