simspark-cvs

[Oliver O. <fr...@us...>]

Update of /cvsroot/simspark/simspark/simulations/soccer/plugin/catcheffector
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv24718/catcheffector

Added Files:
      Tag: projectx
	catchaction.h catcheffector.cpp catcheffector.h 
	catcheffector_c.cpp 
Log Message:
soccer plugins from rcssserver3D

--- NEW FILE: catchaction.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef CATCHACTION_H
#define CATCHACTION_H

#include <oxygen/gamecontrolserver/actionobject.h>
#include <salt/vector.h>

class CatchAction : public oxygen::ActionObject
{
public:
    CatchAction(const std::string& predicate)
        : ActionObject(predicate){}

    virtual ~CatchAction() {}
};

#endif // CATCHACTION_H

--- NEW FILE: catcheffector.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "catchaction.h"
#include "catcheffector.h"
#include <salt/random.h>
#include <zeitgeist/logserver/logserver.h>
#include <oxygen/sceneserver/transform.h>
#include <oxygen/physicsserver/spherecollider.h>
#include <oxygen/agentaspect/agentaspect.h>
#include <oxygen/physicsserver/body.h>
#include <oxygen/gamecontrolserver/gamecontrolserver.h>
#include <soccer/ballstateaspect/ballstateaspect.h>
#include <soccer/agentstate/agentstate.h>
#include <soccer/soccerbase/soccerbase.h>
#include <soccer/soccercontrolaspect/soccercontrolaspect.h>
#include <soccer/soccerruleaspect/soccerruleaspect.h>

using namespace boost;
using namespace oxygen;
using namespace salt;
using namespace std;

CatchEffector::CatchEffector()
    : oxygen::Effector(),
      mCatchMargin(1.00),mPlayerRadius(0.0),mBallRadius(0.0)
{
}

CatchEffector::~CatchEffector()
{
}

void
CatchEffector::MoveBall(const Vector3f& pos)
{
    mBallBody->SetPosition(pos);
    mBallBody->SetVelocity(Vector3f(0,0,0));
    mBallBody->SetAngularVelocity(Vector3f(0,0,0));
}

bool
CatchEffector::Realize(boost::shared_ptr<ActionObject> action)
{
    // this should also include the case when there is no ball
    // (because then there will be no body, neither).
    if (mBallBody.get() == 0)
    {
        return false;
    }

    if (mAgent.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (CatchEffector) parent node is not derived "
            << "from BaseNode\n";
        return false;
    }

    if (mAgentState.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (CatchEffector) parent node is not derived "
            << "from BaseNode\n";
        return false;
    }

    shared_ptr<CatchAction> catchAction =
        shared_dynamic_cast<CatchAction>(action);

    if (catchAction.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (CatchEffector) cannot realize an unknown "
            << "ActionObject\n";
        return false;
    }

    if (mAgentState->GetUniformNumber() != 1)
    {
        return true;
    }

    Vector3f pos = mBallBody->GetWorldTransform().Pos();
    if ( mAgentState->GetTeamIndex() == TI_LEFT )
    {
        if (! mLeftPenaltyArea.Contains(Vector2f(pos[0], pos[1])))
        {
            return true;
        }
    }
    else
    {
        if (! mRightPenaltyArea.Contains(Vector2f(pos[0], pos[1])))
        {
            return true;
        }
    }

    Vector3f ballVec =
        mBallBody->GetWorldTransform().Pos() -
        mAgent->GetWorldTransform().Pos();

    // the ball can be catched if the distance is
    // less then Ball-Radius + Player-Radius + CatchMargin AND
    // the player is close to the ground
    if (mAgent->GetWorldTransform().Pos().z() > mPlayerRadius + 0.01 ||
        ballVec.Length() > mPlayerRadius + mBallRadius + mCatchMargin)
    {
        // ball is out of reach, or player is in the air:
        // catch has no effect
        return true;
    }

    Vector3f ballPos = mAgent->GetWorldTransform().Pos();
    ballPos[2]= mBallRadius;
    if (mAgentState->GetTeamIndex() == TI_LEFT)
    {
        ballPos[0] += mBallRadius + mPlayerRadius + 0.07;
    }
    else
    {
        ballPos[0] -= mBallRadius + mPlayerRadius + 0.07;
    }

    mSoccerRule->ClearPlayersWithException(ballPos,
        2.0, 5.0, TI_LEFT, mAgentState);
    mSoccerRule->ClearPlayersWithException(ballPos,
        2.0, 5.0, TI_RIGHT, mAgentState);

    MoveBall(ballPos);

    return true;
}

shared_ptr<ActionObject>
CatchEffector::GetActionObject(const Predicate& predicate)
{
  do
  {
      if (predicate.name != GetPredicate())
          {
              GetLog()->Error() << "ERROR: (CatchEffector) invalid predicate"
                                << predicate.name << "\n";
              break;
          }

      // construct the CatchAction object
      return shared_ptr<CatchAction>(new CatchAction(GetPredicate()));

  } while (0);

  // some error happened
  return shared_ptr<ActionObject>();
}

void
CatchEffector::OnLink()
{
    SoccerBase::GetBallBody(*this,mBallBody);
    SoccerBase::GetAgentState(*this,mAgentState);

    SoccerBase::GetSoccerRuleAspect(*this,mSoccerRule);

    mAgent = shared_dynamic_cast<AgentAspect>(make_shared(GetParent()));

    if (mAgent.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (CatchEffector) parent node is not derived "
            << "from AgentAspect\n";
        return;
    }

    shared_ptr<SphereCollider> geom =
        shared_dynamic_cast<SphereCollider>(mAgent->GetChild("geometry"));
    if (geom.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (CatchEffector) parent node has no SphereCollider "
            << "child\n";
    } 
    else
    {
        mPlayerRadius = geom->GetRadius();
    }

    if (! SoccerBase::GetBallCollider(*this,geom))
    {
        GetLog()->Error()
            << "ERROR: (CatchEffector) ball node has no SphereCollider "
            << "child\n";
    } 
    else
    {
        mBallRadius = geom->GetRadius();
    }

    SoccerBase::GetSoccerVar(*this,"FieldLength",mFieldLength);
    SoccerBase::GetSoccerVar(*this,"GoalWidth",mGoalWidth);

    // the penalty areas (exact sizes)
    mRightPenaltyArea = salt::AABB2(
                     Vector2f(mFieldLength/2.0 - 16.5, -16.5 - mGoalWidth/2.0),
                     Vector2f(mFieldLength/2.0 , 16.5 + mGoalWidth/2.0));
    mLeftPenaltyArea = salt::AABB2(
                     Vector2f(-mFieldLength/2.0 + 16.5, -16.5 - mGoalWidth/2.0),
                     Vector2f(-mFieldLength/2.0, 16.5 + mGoalWidth/2.0));
}

void
CatchEffector::OnUnlink()
{
    mSoccerRule.reset();
    mBallBody.reset();
    mAgent.reset();
    mAgentState.reset();
}

void
CatchEffector::SetCatchMargin(float margin)
{
    mCatchMargin = margin;
}

--- NEW FILE: catcheffector.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef CATCHEFFECTOR_H
#define CATCHEFFECTOR_H

#include <oxygen/agentaspect/effector.h>
#include <oxygen/physicsserver/body.h>
#include <soccer/ball/ball.h>
#include <soccer/ballstateaspect/ballstateaspect.h>

namespace salt
{
    class AABB2;
}

namespace oxygen
{
    class Body;
    class AgentAspect;
}

class AgentState;

class SoccerRuleAspect;

class CatchEffector : public oxygen::Effector
{
public:
    CatchEffector();
    virtual ~CatchEffector();

    /** realizes the action described by the ActionObject */
    virtual bool Realize(boost::shared_ptr<oxygen::ActionObject> action);

    /** returns the name of the predicate this effector implements. */
    virtual std::string GetPredicate() { return "catch"; }

    /** constructs an Actionobject, describing a predicate */
    virtual boost::shared_ptr<oxygen::ActionObject>
    GetActionObject(const oxygen::Predicate& predicate);

    /** setup the reference to the ball body node */
    virtual void OnLink();

    /** remove the reference to the ball body node */
    virtual void OnUnlink();

    /** set the catch margin (the area within objects are catchable) */
    void SetCatchMargin(float margin);

protected:
    /** moves the ball to pos setting its linear and angular velocity to 0 */
    void MoveBall(const salt::Vector3f& pos);
    /** reference to the soccer rule aspect */
    boost::shared_ptr<SoccerRuleAspect> mSoccerRule;

protected:
    /** reference to the body node of the ball */
    boost::shared_ptr<oxygen::Body> mBallBody;
    /** reference to the agent aspect */
    boost::shared_ptr<oxygen::AgentAspect> mAgent;
    /** reference to the agentstate */
    boost::shared_ptr<AgentState> mAgentState;

    /** bounding box for the right penalty area */
    salt::AABB2 mRightPenaltyArea;
    /** bounding box for the left penalty area */
    salt::AABB2 mLeftPenaltyArea;

    /** the field length (in meters) */
    float mFieldLength;
    /** the goal width (in meters) */
    float mGoalWidth;

private:
    /** the margin where objects can be catched */
    float mCatchMargin;
    /** radius of the player */
    float mPlayerRadius;
    /** radius of the ball */
    float mBallRadius;
};

DECLARE_CLASS(CatchEffector);

#endif // CATCHEFFECTOR_H

--- NEW FILE: catcheffector_c.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program 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 General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "catcheffector.h"

using namespace oxygen;

FUNCTION(CatchEffector,setCatchMargin)
{
    float inMargin;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inMargin))
        )
        {
            return false;
        }

    obj->SetCatchMargin(inMargin);
    return true;
}

void CLASS(CatchEffector)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Effector);
    DEFINE_FUNCTION(setCatchMargin);
}