soccer_math.h

Go to the documentation of this file.

// -*-c++-*-
 
/*
 *Copyright:
 
 Copyright (C) Hidehisa AKIYAMA
 
 This code is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 3 of the License, or (at your option) any later version.
 
 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
 *EndCopyright:
 */
 
 
#ifndef RCSC_SOCCERMATH_H
#define RCSC_SOCCERMATH_H
 
#include <rcsc/geom/vector_2d.h>
 
namespace rcsc {
 
constexpr double SERVER_EPS = 1.0e-10;
 
// kick command related
 
/*-------------------------------------------------------------------*/
inline
double
kick_rate( const double & dist,
           const double & dir_diff,
           const double & kprate,
           const double & bsize,
           const double & psize,
           const double & kmargin )
{
    return kprate * ( 1.0
                      - 0.25 * std::fabs( dir_diff ) / 180.0
                      - 0.25 * ( dist - bsize - psize ) / kmargin );
}
 
// dash command related
 
/*-------------------------------------------------------------------*/
inline
double
dir_rate( const double & dir,
          const double & back_dash_rate,
          const double & side_dash_rate )
{
    return ( std::fabs( dir ) > 90.0
             ? back_dash_rate - ( ( back_dash_rate - side_dash_rate )
                                  * ( 1.0 - ( std::fabs( dir ) - 90.0 ) / 90.0 )
                                  )
             : side_dash_rate + ( ( 1.0 - side_dash_rate )
                                  * ( 1.0 - std::fabs( dir ) / 90.0 ) )
             );
}
 
// turn command related
 
/*-------------------------------------------------------------------*/
inline
double
effective_turn( const double & turn_moment,
                const double & speed,
                const double & inertiamoment )
{
    return turn_moment / ( 1.0 + inertiamoment * speed );
}
 
// dash command related
 
/*-------------------------------------------------------------------*/
inline
double
final_speed( const double & dash_power,
             const double & dprate,
             const double & effort,
             const double & decay )
{
    // if player continue to run using the same dash power
    // archieved speed at n step later is sum of infinite geometric series
 
    // !! NOTE !!
    // You must compare this value to the PlayerType::playerSpeedMax()
 
    //return ( (dash_power * dprate * effort) // == accel
    //         * (1.0 / (1.0 - decay)) ); // == sum inf geom series
    return ( ( std::fabs( dash_power ) * dprate * effort ) // == accel
             / ( 1.0 - decay ) ); // == sum inf geom series
}
 
/*-------------------------------------------------------------------*/
inline
bool
can_over_speed_max( const double & dash_power,
                    const double & dprate,
                    const double & effort,
                    const double & decay,
                    const double & speed_max )
 
{
    return ( std::fabs( dash_power ) * dprate * effort // max accel
             > speed_max * ( 1.0 - decay ) ); // is over speed decay
}
 
// predictor method for inertia movement
 
/*-------------------------------------------------------------------*/
inline
Vector2D
inertia_n_step_travel( const Vector2D & initial_vel,
                       const int n_step,
                       const double & decay )
{
    return Vector2D( initial_vel )
        *= ( ( 1.0 - std::pow( decay, n_step ) ) / ( 1.0 - decay ) );
}
 
/*-------------------------------------------------------------------*/
inline
Vector2D
inertia_n_step_point( const Vector2D & initial_pos,
                      const Vector2D & initial_vel,
                      const int n_step,
                      const double & decay )
{
    /*
      return
      initial_pos
      + initial_vel * ( (1.0 - std::pow(decay, static_cast<double>(n_step)))
      / (1.0 - decay) );
    */
    /*
      return Vector2D(initial_vel
      * (1.0 - std::pow(decay, n_step) / (1.0 - decay)))
      += initial_pos;
    */
    /*
      double rate = ( (1.0 - std::pow(decay, static_cast<double>(n_step)))
      / (1.0 - decay) );
      return initial_pos + ( initial_vel * rate );
    */
    return Vector2D( initial_pos )
        += inertia_n_step_travel( initial_vel, n_step, decay );
}
 
/*-------------------------------------------------------------------*/
inline
double
inertia_n_step_distance( const double & initial_speed,
                         const int n_step,
                         const double & decay )
{
    return initial_speed
        * ( 1.0 - std::pow( decay, n_step ) )
        / ( 1.0 - decay );
}
 
/*-------------------------------------------------------------------*/
inline
double
inertia_n_step_distance( const double & initial_speed,
                         const double & n_step_real,
                         const double & decay )
{
    return initial_speed
        * ( 1.0 - std::pow( decay, n_step_real ) )
        / ( 1.0 - decay );
}
 
 
/*-------------------------------------------------------------------*/
inline
Vector2D
inertia_final_travel( const Vector2D & initial_vel,
                      const double & decay )
{
    return Vector2D( initial_vel ) /= ( 1.0 - decay );
}
 
/*-------------------------------------------------------------------*/
inline
Vector2D
inertia_final_point( const Vector2D & initial_pos,
                     const Vector2D & initial_vel,
                     const double & decay )
{
    /*
      return Vector2D(initial_vel / (1.0 - decay)) += initial_pos;
    */
    /*
      Vector2D end_pos(initial_vel);
      end_pos /= (1.0 - decay);
      return end_pos += initial_pos;
    */
    /*
      return initial_pos + ( initial_vel / (1.0 - decay) );
    */
    return Vector2D( initial_pos )
        += inertia_final_travel( initial_vel, decay );
}
 
/*-------------------------------------------------------------------*/
inline
double
inertia_final_distance( const double & initial_speed,
                        const double & decay )
{
    return initial_speed / ( 1.0 - decay );
}
 
// localization
 
/*-------------------------------------------------------------------*/
inline
double
quantize( const double & value,
          const double & qstep )
{
    return rint( value / qstep ) * qstep;
}
 
/*-------------------------------------------------------------------*/
inline
double
quantize_dist( const double & unq_dist,
               const double & qstep )
{
    return quantize( std::exp
                     ( quantize( std::log
                                 ( unq_dist + SERVER_EPS ), qstep ) ), 0.1 );
}
 
/*-------------------------------------------------------------------*/
inline
double
unquantize_min( const double & dist,
                const double & qstep )
{
    return ( rint( dist / qstep ) - 0.5 ) * qstep;
}
 
/*-------------------------------------------------------------------*/
inline
double
unquantize_max( const double & dist,
                const double & qstep )
{
    return ( rint( dist / qstep ) + 0.5 ) * qstep;
}
 
/*-------------------------------------------------------------------*/
Vector2D
wind_effect( const double & speed,
             const double & weight,
             const double & wind_force,
             const double & wind_dir,
             const double & wind_weight,
             const double & wind_rand,
             Vector2D * wind_error );
 
/*-------------------------------------------------------------------*/
double
unquantize_error( const double & see_dist,
                  const double & qstep );
 
} // end namespace
 
#endif