object.cpp

2009 ROBOCUP 仿真2DSERVER 源码

    return CArea( nearest_gpost, ServerParam::instance().goalPostRadius() + size );
}
}

/*
 *===================================================================
 *Part: PObject
 *===================================================================
 */

int PObject::S_object_count = 0;

/* pfr 06/07/200 added short name support */
PObject::PObject( const std::string & name,
                  const std::string & short_name,
                  const std::string & close_name,
                  const std::string & short_close_name,
                  const PVector & p,
                  const double & v )
    : M_id( S_object_count ),
      M_name( name ),
      M_short_name( short_name ),
      M_close_name( close_name ),
      M_short_close_name( short_close_name ),
      M_object_version( v ),
      M_size( 1.0 ),
      M_pos( p ),
      M_enable( true )
{
    ++S_object_count;
}

std::ostream &
PObject::print( std::ostream & o ) const
{
    o << "#Ob[" << this->id();
    if( ! name().empty() )
        o << ":" << name() << "";
    return o << ":pos=" << this->M_pos
             << ",size=" << this->M_size
        //<< ",angle=" << this->M_angle
             << "]";
}


/*
 *===================================================================
 *Part: MPObject
 *===================================================================
 */

/* pfr 06/07/200 added short name support */
MPObject::MPObject( Stadium & stadium,
                    const std::string & name,
                    const std::string & short_name,
                    const std::string & close_name,
                    const std::string & short_close_name )
    : PObject( name, short_name,
               close_name, short_close_name )
    , M_stadium( stadium )
    , M_vel( 0.0,0.0 )
    , M_accel( 0.0,0.0 )
{
    //assert( stadium );
    //M_weather = &( stadium->weather() );
}

std::ostream &
MPObject::print( std::ostream & o ) const
{
    o << "#Ob[" << this->id();
    if( ! name().empty() )
        o << ":" << name() << "";
    return o << ":pos=" << this->M_pos
             << ",size=" << this->M_size
        //<< ",angle=" << Rad2IDegRound( this->M_angle )
             << ",vel=" << this->M_vel
             << ",acc=" << this->M_accel
             << ",decay=" << this->M_decay
             << ",randp=" << this->M_randp
             << "]";
}

void
MPObject::moveTo( const PVector & pos,
                  //const Angle & angle,
                  const PVector & vel,
                  const PVector & accel )
{
    M_pos = pos;
    //M_angle = angle;
    M_vel = vel;
    M_accel = accel;
}

void
MPObject::setConstant( const double & size,
                       const double & decay,
                       const double & randp,
                       const double & weight,
                       const double & max_speed,
                       const double & max_accel )
{
    M_size = size;
    M_decay = decay;
    M_randp = randp;
    M_weight = weight;
    M_max_speed = max_speed;
    M_max_accel = max_accel;
}

PVector
MPObject::noise()
{
    double maxrnd = M_randp * vel().r();
    return PVector::fromPolar( drand( 0.0, maxrnd ),
                               drand( -M_PI, M_PI ) );
    //return PVector( drand( -maxrnd, maxrnd ),
    //                drand( -maxrnd, maxrnd ) );
}

PVector
MPObject::wind()
{
    const Weather & w = M_stadium.weather();

    //if ( ! M_weather
    //|| M_weather->wind_rand < EPS )
    if ( w.wind_rand < EPS )
    {
        return PVector( 0.0, 0.0 );
    }

    //     return PVector( M_vel.r() * ( M_weather->wind_vector.x +
    //                                   drand( - M_weather->wind_rand, M_weather->wind_rand) ) /
    //                     (M_weight * WIND_WEIGHT), M_vel.r() * ( M_weather->wind_vector.y +
    //                                                             drand( - M_weather->wind_rand, M_weather->wind_rand ) ) /
    //                     (M_weight * WIND_WEIGHT));
    const double speed = M_vel.r();
    return PVector( speed * ( w.wind_vector.x +
                              drand( - w.wind_rand, + w.wind_rand) ) /
                    ( M_weight * ServerParam::instance().windWeight() ),
                    speed * ( w.wind_vector.y +
                              drand( - w.wind_rand, + w.wind_rand ) ) /
                    ( M_weight * ServerParam::instance().windWeight() ));
}

void
MPObject::_inc()
{
    if ( M_accel.x || M_accel.y )
	  {
        double max_a = maxAccel();
        double max_s = maxSpeed();

        double tmp = M_accel.r();
        if ( tmp > max_a )
            M_accel *= (max_a / tmp);
        M_vel += M_accel;
        tmp = M_vel.r();
        if ( tmp > max_s )
            M_vel *= (max_s / tmp);
    }

    updateAngle();

    M_vel += noise();
    M_vel += wind();

    CArea post = nearestPost( pos(), M_size );

    //      std::cout << "pos = " << pos << endl;
    //      std::cout << "nearest post = " << post << endl;
    //      std::cout << "dist = " << (pos - post.center).r() << endl;
    while ( ( pos() - post.center() ).r() < post.radius() )
    {
        //          std::cout << "In post\n";
        // then the ball has overlapped the post.  Either it was moved
        // there or "pushed".  Either way, we just move the ball away
        // from the post
        PVector diff = pos() - post.center();
        if ( diff == PVector() )
        {
            diff = PVector::fromPolar( post.radius(),
                                       drand( -M_PI, +M_PI ) );
        }
        else
        {
            diff.normalize( post.radius() );
        }

        M_pos = post.center() + diff;

        while ( ( pos() - post.center() ).r() < post.radius() )
        {
            // noise keeps it inside the post, move it a bit further out
            diff.normalize( diff.r() * 1.01 );
            M_pos = post.center() + diff;
        }

        if ( M_vel.x != 0.0 || M_vel.y != 0.0 )
        {
            PVector pos2center = post.center() - pos();
            M_vel.rotate( -pos2center.th() );
            M_vel.x = - M_vel.x;
            M_vel.rotate( pos2center.th() );
        }

        post = nearestPost( pos(), M_size );
        std::cout << M_stadium.time() << ": Colliding with post\n"
                  << "  pos = " << pos() << '\n'
                  << "  vel = " << vel() << '\n'
                  << "  nearest post = " << post.center() << '\n'
                  << "  dist = " << ( pos() - post.center() ).r() << std::endl;

        collidedWithPost();
    }

    PVector new_pos = pos() + M_vel;
    CArea second_post = nearestPost( new_pos, M_size );
    PVector inter;
    bool second = false;

    //      std::cout << "vel = " << vel << endl;
    //      std::cout << "new_pos = " << new_pos << endl;
    //int loop_count = 0;
    while ( pos() != new_pos
            && ( ( intersect( pos(), new_pos, post, inter ) )
                 || ( post != second_post
                      && ( second = intersect( pos(), new_pos, second_post, inter ) )
                      )
                 )
            )
    {
        //         ++loop_count;
        //         std::cout << M_stadium.time() <<": Collision: " << loop_count << "\n"
        //                   << "  pos=" << pos() << '\n';

        // handle collision
        M_pos = inter;

        //         std::cout << "  intersect=" << pos() << '\n';

        PVector rem = new_pos - pos();
        PVector coll_2_circle;
        if ( second )
        {
            coll_2_circle = second_post.center() - pos();
        }
        else
        {
            coll_2_circle = post.center() - pos();
        }

        // 2008-05-22 akiyama
        // fixed endless-loop bug.
        // If this small vector is not added to M_pos, intersect() may still
        // return pos() as the intersect point.
        M_pos += PVector::fromPolar( EPS, coll_2_circle.th() + 180.0 );

        //         std::cout << "  rem = " << rem << '\n';

        rem.rotate( -coll_2_circle.th() );
        rem.x = -rem.x;
        rem.rotate( coll_2_circle.th() );

        new_pos = pos() + rem;
        //         std::cout << "  rem = " << rem << '\n';

        // setup post and second post for next loop
        post = nearestPost( pos(), M_size );
        second_post = nearestPost( new_pos, M_size );

        //         std::cout << "  pos = " << pos() << '\n'
        //                   << "  new_pos = " << new_pos << '\n'
        //                   << "  nearest post = " << post.center() << '\n'
        //                   << "  dist = " << ( pos() - post.center() ).r() << '\n';

        // setup vel so it will decay normally.  The collisions are
        // elastic, so the maginitude does not change, but the heading
        // does
        M_vel = PVector::fromPolar( M_vel.r(), rem.th() );
        //         std::cout << "  vel = " << vel() << std::endl;

        second = false;

        collidedWithPost();
    }

    M_pos = new_pos;
    M_vel *= M_decay;
    M_accel *= 0.0;
}

// void MPObject::collide(MPObject& obj)
// {
//     double r = size + obj.size;
//     PVector dif = (pos - obj.pos);
//     double d = pos.distance(obj.pos);
//     Angle th = fabs(dif.angle(vel));
//     double l1 = d * cos(th);
//     double h = d * sin(th);
//     double cosp = h / r;
//     double sinp = sqrt(1.0 - square(cosp));
//     double l2 = r * sinp;
//     PVector dv = vel;

//     dv.normalize(-(l1 + l2));

//     pos += dv;
// }


void
MPObject::moveToCollPos()
{
    if ( this->col_count > 0 )
    {
        /*        cout << "oldpos = " << obj->pos << std::endl; */
        /*        cout << "colpos = " << obj->post_col_pos << std::endl; */
        /*        cout << "colcount = " << obj->col_count << std::endl; */
        this->post_col_pos /= this->col_count;
        this->M_pos = this->post_col_pos;
        /*        cout << "newpos = " << obj->pos << std::endl; */
    }

    this->post_col_pos = PVector( 0, 0 );
    this->col_count = 0;
}


Ball::Ball( Stadium & stadium )
    : MPObject( stadium,
                BALL_NAME, BALL_NAME_SHORT,
                O_TYPE_BALL_NAME, O_TYPE_BALL_NAME_SHORT )
{

}