object.c

在LINUX下运行的仿真机器人服务器源代码

/* -*- 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, 2001 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:
 */



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

#include "param.h"
#include "types.h"
#include "utility.h"
#include "object.h"
#include "field.h"
#include "random.h"
#include "audio.h"
#include "serializer.h"
#include "referee.h"
#include "visual.h"
#include "random.h"

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

PVector::PVector(const double vx, const double vy)
{
	x = vx ;
	y = vy ;
}

PVector PVector::operator -() const
{
		return PVector ( -x, -y ); 
}

PVector	PVector::operator +() const
{
		return *this;
}

void PVector::operator +=(const PVector& v)
{
	x += v.x ;
	y += v.y ;
}

void PVector::operator -=(const PVector& v)
{
	x -= v.x ;
	y -= v.y ;
}

void PVector::operator *=(const double& a)
{
	x *= a ;
	y *= a ;
}

void PVector::operator /=(const double& a)
{
	x /= a ;
	y /= a ;
}

std::ostream& operator<< (std::ostream& o, const PVector& v)
{
	return o << "#V[" << v.x << "," << v.y << "]" ;
}

PVector operator +(const PVector& a, const PVector& b)
{
	return PVector((a.x + b.x), (a.y + b.y)) ;
}

PVector operator -(const PVector& a, const PVector& b)
{
	return PVector((a.x - b.x), (a.y - b.y)) ;
}

void PVector::normalize(const Value& l) 
{
	*this *= (l/std::max(r(),EPS)) ;
}

Value PVector::distance(const PVector& orig) const
{
	return (*this - orig).r() ;
}

Angle PVector::angle() const
{
	return Atan(y,x) ;
}

Angle PVector::angle(const PVector& dir) const
{
	Angle ang = Atan(dir.y,dir.x) - Atan(y,x) ;

	return normalize_angle(ang) ;
}

void PVector::rotate(const Angle& ang)
{
	Value r1 = r() ;
	Angle th1 = th() ;

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

Angle PVector::vangle(const PVector& target, const PVector& origin) const
{
	PVector axis = origin - *this ;
	PVector dir = target - *this ;
	return axis.angle(dir) ;
}

Angle PVector::vangle(const PVector& target, const Angle& origin) const
{
	PVector dir = target - *this ;
	Angle ang = dir.angle() - origin ;

	return normalize_angle(ang) ;
}

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
 *===================================================================
 */

RArea::RArea(const Value l, const Value r, const Value t, const Value b) 
{
	left = l ; right = r ;
	top = t ; bottom = b ;
}

RArea::RArea(const PVector center,const PVector size)
{
	left = center.x - size.x/2.0 ;
	right = center.x + size.x/2.0 ;
	top = center.y - size.y/2.0 ;
	bottom = center.y + size.y/2.0 ;
}

Logical RArea::inArea(const PVector &p)
{
	return (p.x >= left) && (p.x <= right) && (p.y >= top) && (p.y <= bottom) ;
}

PVector RArea::nearestHEdge(const PVector &p) 
		/* find nearest horizontal line */
{
	static PVector r ;

	r.x = std::min(std::max(p.x,left),right) ;
	r.y = (Abs(p.y - top) < Abs(p.y - bottom)) ? top : bottom ;

	return r ;
}

PVector RArea::nearestVEdge(const PVector &p) 
		/* find nearest vertical line */
{
	static PVector r ;

	r.x = (Abs(p.x - left) < Abs(p.x - right)) ? left : right ;
	r.y = std::min(std::max(p.y,top),bottom) ;

	return r ;
}

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

PVector RArea::randomize() 
{
	static PVector r ;

	r.x = drand(left, right) ;
	r.y = drand(bottom, top) ;

	return r ;
}

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

CArea::CArea(const PVector cnt, const Value radius)
{
	center = cnt ;
	r = radius ;
}

Logical CArea::inArea(const PVector &p)
{
	return r >= center.distance(p) ;
}

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

	dif.normalize(r) ;

	return 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 << "]" ;
}


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.r )
        // 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 = sqrt( dx*dx + dy*dy );
//          std::cout << dr << endl;
        double D = begin.x * end.y - end.x * begin.y;
        double descrim = circle.r*circle.r * 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 = 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 + fabs( dy ) * descrim) / (dr*dr);
            double y2 = (-D*dx - 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.r ), inter ) )
        {
            inter += circle.center;
            return true;
        }
        return false;
    }
}


CArea
nearestPost( const PVector& pos, const Value& size )
{
    PVector nearest_gpost;
    if( pos.y > 0 )
    {
        if( pos.x > 0 )
            nearest_gpost = PVector( PITCH_LENGTH*0.5 
                                     - ServerParam::instance().goalPostRadius(),
                                     ServerParam::instance().gwidth*0.5
                                     + ServerParam::instance().goalPostRadius() );
        else
            nearest_gpost = PVector( -PITCH_LENGTH*0.5 
                                     + ServerParam::instance().goalPostRadius(),
                                     ServerParam::instance().gwidth*0.5
                                     + ServerParam::instance().goalPostRadius() );
    }
    else
    {
        if( pos.x > 0 )
            nearest_gpost = PVector( PITCH_LENGTH*0.5 
                                     - ServerParam::instance().goalPostRadius(),
                                     -ServerParam::instance().gwidth*0.5
                                     - ServerParam::instance().goalPostRadius() );
        else
            nearest_gpost = PVector( -PITCH_LENGTH*0.5 
                                     + ServerParam::instance().goalPostRadius(),
                                     -ServerParam::instance().gwidth*0.5
                                     - ServerParam::instance().goalPostRadius() );
    }
    
    return CArea( nearest_gpost, ServerParam::instance().goalPostRadius() + size );
}

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