object.cpp

2009 ROBOCUP 仿真2DSERVER 源码

/* -*- Mode: C++ -*-
 *Header:
 *File: object.C (for C++ & cc)
 *Author: Noda Itsuki
 *Date: 1995/02/21
 *EndHeader:
 */

/*
 *Copyright:

 Copyright (C) 1996-2000 Electrotechnical Laboratory.
 Itsuki Noda, Yasuo Kuniyoshi and Hitoshi Matsubara.
 Copyright (C) 2000-2007 RoboCup Soccer Server Maintainance Group.
 Patrick Riley, Tom Howard, Daniel Polani, Itsuki Noda,
 Mikhail Prokopenko, Jan Wendler

 This file is a part of SoccerServer.

 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 2.1 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:
 */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "object.h"

//#include "audio.h"
#include "field.h"
#include "param.h"
#include "player.h"
#include "random.h"
#include "types.h"
#include "utility.h"

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <cassert>

/*
 *===================================================================
 *Part: Plane Vector
 *===================================================================
 */

const
PVector &
PVector::rotate(const double & ang)
{
    double r1 = r();
    double th1 = th();

    x = r1 * std::cos( th1 + ang );
    y = r1 * std::sin( th1 + ang );

    return *this;
}

bool
PVector::between( const PVector & begin,
                  const PVector & end ) const
{
    if( begin.x > end.x )
        return between( end, begin );

    if( begin.x <= x && x <= end.x )
    {
        if( begin.y < end.y )
        {
            return begin.y <= y && y <= end.y;
        }
        else
        {
            return begin.y >= y && y >= end.y;
        }
    }
    //      std::cout << begin.x << " > " << x
    //                << "\n|| " << x << " > " << end.x;
    //      if( begin.y < end.y )
    //      {
    //          std::cout << "\n|| " << begin.y << " > " << y
    //                    << "\n|| " << y << " > " << end.y << std::endl;
    //      }
    //      else
    //      {
    //          std::cout << "\n|| " << begin.y << " < " << y
    //                    << "\n|| " << y << " < " << end.y << std::endl;
    //      }
    return false;
}
/*
 *===================================================================
 *Part: Area
 *===================================================================
 */

PVector
RArea::nearestHEdge( const PVector & p ) const
{
    return PVector( std::min( std::max( p.x, left ), right ),
                    ( std::fabs( p.y - top ) < std::fabs( p.y - bottom )
                      ? top : bottom ) );
}

PVector
RArea::nearestVEdge( const PVector & p ) const
{
    return PVector( ( std::fabs( p.x - left ) < std::fabs( p.x - right )
                      ? left : right ),
                    std::min( std::max( p.y, top ), bottom ) );
}

PVector
RArea::nearestEdge( const PVector & p ) const
{
    if ( std::min( std::fabs( p.x - left ), std::fabs( p.x - right ) )
         < std::min( std::fabs( p.y - top ), std::fabs( p.y - bottom ) ) )
    {
        return nearestVEdge( p );
    }
    else
    {
        return nearestHEdge( p );
    }
}

PVector
RArea::randomize() const
{
    return PVector( drand( left, right ),
                    drand( bottom, top ) );
}

std::ostream &
RArea::print( std::ostream& o ) const
{
    return o << "#A[h:" << left << "~" << right
             << ",v:" << top << "~" << bottom << "]";
}




PVector
CArea::nearestEdge( const PVector & p ) const
{
    PVector dif = p - M_center;
    if ( dif.x == 0.0 && dif.y == 0.0 )
        dif = PVector( EPS, EPS );

    dif.normalize( M_radius );

    return M_center + dif;
}

//std::ostream &
//operator<<( std::ostream & o, const CArea & a )
//{
//    return o << "#A[x:" << a.center.x <<
//				",y:" << a.center.y <<
//				",r:" << a.r << "]";
//}

namespace {
bool
intersect( const PVector & begin,
           const PVector & end,
           const CArea & circle,
           PVector & inter )
{
    if ( begin == end )
        return false;

    if ( ( begin - end ).r() < ( begin - circle.center() ).r() - circle.radius() )
        // object wont get within circles range
        return false;

    if ( circle.center() == PVector() )
    {
        double dx = end.x - begin.x;
        double dy = end.y - begin.y;
        //          std::cout << dx << endl;
        //          std::cout << dy << endl;
        double dr = std::sqrt( dx*dx + dy*dy );
        //          std::cout << dr << endl;
        double D = begin.x * end.y - end.x * begin.y;
        double descrim = circle.radius() * circle.radius() * dr*dr - D*D;
        //          std::cout << descrim << endl;
        if ( descrim <= 0.0 )
        {
            // no collision of tagent
            //              std::cout << "Descrim < 0\n";
            return false;
        }
        else
        {
            descrim = std::sqrt( descrim );
            //              std::cout << descrim << endl;

            double x1 = (D*dy + dx * descrim) / (dr*dr);
            double x2 = (D*dy - dx * descrim) / (dr*dr);
            double y1 = (-D*dx + std::fabs( dy ) * descrim) / (dr*dr);
            double y2 = (-D*dx - std::fabs( dy ) * descrim) / (dr*dr);
            //              std::cout << x1 << endl;
            //              std::cout << x2 << endl;
            //              std::cout << y1 << endl;
            //              std::cout << y2 << endl;
            PVector first, second;
            if ( dy < 0 )
            {
                first = PVector( x2, y1 );
                second = PVector( x1, y2 );
            }
            else
            {
                first = PVector( x1, y1 );
                second = PVector( x2, y2 );
            }

            if ( ! first.between( begin, end )
                 && ! second.between( begin, end ) )
            {
                // intersections are not between the end points
                //                  std::cout << "Coll outside of end points\n"
                //                            << "begin = " << begin << std::endl
                //                            << "end = " << end << std::endl
                //                            << "first = " << first << std::endl
                //                            << "second = " << second << std::endl;

                return false;
            }

            if ( ! first.between( begin, end ) )
            {
                inter = second;
                second = first;
            }
            else if ( ! second.between( begin, end ) )
            {
                inter = first;
            }
            else
            {
                if ( ( begin - first ).r() < ( begin - second ).r() )
                {
                    inter = first;
                }
                else
                {
                    inter = second;
                    second = first;
                }
            }

            if ( inter == begin
                 && ! second.between( begin, end ) )
            {
                //                  std::cout << "Fake collision\n"
                //                            << "begin = " << begin << std::endl
                //                            << "end = " << end << std::endl
                //                            << "first = " << inter << std::endl
                //                            << "second = " << second << std::endl;
                // fake collision.  Object is tagent to the circle and moving away
                return false;
            }
            return true;
        }
    }
    else
    {
        if ( intersect( begin - circle.center(), end - circle.center(),
                        CArea( PVector(), circle.radius() ), inter ) )
        {
            inter += circle.center();
            return true;
        }
        return false;
    }
}
}

namespace {
CArea
nearestPost( const PVector & pos,
             const double & size )
{
    PVector nearest_gpost;

    if ( pos.y > 0 )
    {
        if ( pos.x > 0 )
        {
            nearest_gpost = PVector( ServerParam::PITCH_LENGTH*0.5
                                     - ServerParam::instance().goalPostRadius(),
                                     ServerParam::instance().goalWidth()*0.5
                                     + ServerParam::instance().goalPostRadius() );
        }
        else
        {
            nearest_gpost = PVector( - ServerParam::PITCH_LENGTH*0.5
                                     + ServerParam::instance().goalPostRadius(),
                                     ServerParam::instance().goalWidth()*0.5
                                     + ServerParam::instance().goalPostRadius() );
        }
    }
    else
    {
        if ( pos.x > 0 )
        {
            nearest_gpost = PVector( ServerParam::PITCH_LENGTH*0.5
                                     - ServerParam::instance().goalPostRadius(),
                                     - ServerParam::instance().goalWidth()*0.5
                                     - ServerParam::instance().goalPostRadius() );
        }
        else
        {
            nearest_gpost = PVector( - ServerParam::PITCH_LENGTH*0.5
                                     + ServerParam::instance().goalPostRadius(),
                                     - ServerParam::instance().goalWidth()*0.5
                                     - ServerParam::instance().goalPostRadius() );
        }
    }