00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "catchaction.h"
00022 #include "catcheffector.h"
00023 #include <salt/random.h>
00024 #include <zeitgeist/logserver/logserver.h>
00025 #include <oxygen/sceneserver/transform.h>
00026 #include <oxygen/physicsserver/spherecollider.h>
00027 #include <oxygen/agentaspect/agentaspect.h>
00028 #include <oxygen/physicsserver/body.h>
00029 #include <oxygen/gamecontrolserver/gamecontrolserver.h>
00030 #include <soccer/ballstateaspect/ballstateaspect.h>
00031 #include <soccer/agentstate/agentstate.h>
00032 #include <soccer/soccerbase/soccerbase.h>
00033 #include <soccer/soccercontrolaspect/soccercontrolaspect.h>
00034 #include <soccer/soccerruleaspect/soccerruleaspect.h>
00035
00036 using namespace boost;
00037 using namespace oxygen;
00038 using namespace salt;
00039 using namespace std;
00040
00041 CatchEffector::CatchEffector()
00042 : oxygen::Effector(),
00043 mCatchMargin(2.00),mPlayerRadius(0.0),mBallRadius(0.0)
00044 {
00045 }
00046
00047 CatchEffector::~CatchEffector()
00048 {
00049 }
00050
00051 void
00052 CatchEffector::MoveBall(const Vector3f& pos)
00053 {
00054 mBallBody->SetPosition(pos);
00055 mBallBody->SetVelocity(Vector3f(0,0,0));
00056 mBallBody->SetAngularVelocity(Vector3f(0,0,0));
00057 }
00058
00059 bool
00060 CatchEffector::Realize(boost::shared_ptr<ActionObject> action)
00061 {
00062
00063
00064 if (mBallBody.get() == 0)
00065 {
00066 return false;
00067 }
00068
00069 if (mAgent.get() == 0)
00070 {
00071 GetLog()->Error()
00072 << "ERROR: (CatchEffector) parent node is not derived "
00073 << "from BaseNode\n";
00074 return false;
00075 }
00076
00077 if (mAgentState.get() == 0)
00078 {
00079 GetLog()->Error()
00080 << "ERROR: (CatchEffector) parent node is not derived "
00081 << "from BaseNode\n";
00082 return false;
00083 }
00084
00085 shared_ptr<CatchAction> catchAction =
00086 shared_dynamic_cast<CatchAction>(action);
00087
00088 if (catchAction.get() == 0)
00089 {
00090 GetLog()->Error()
00091 << "ERROR: (CatchEffector) cannot realize an unknown "
00092 << "ActionObject\n";
00093 return false;
00094 }
00095
00096 if (mAgentState->GetUniformNumber() != 1)
00097 {
00098 return true;
00099 }
00100
00101 Vector3f pos = mBallBody->GetWorldTransform().Pos();
00102 if ( mAgentState->GetTeamIndex() == TI_LEFT )
00103 {
00104 if (! mLeftPenaltyArea.Contains(Vector2f(pos[0], pos[1])))
00105 {
00106 return true;
00107 }
00108 }
00109 else
00110 {
00111 if (! mRightPenaltyArea.Contains(Vector2f(pos[0], pos[1])))
00112 {
00113 return true;
00114 }
00115 }
00116
00117 Vector3f ballVec =
00118 mBallBody->GetWorldTransform().Pos() -
00119 mAgent->GetWorldTransform().Pos();
00120
00121
00122
00123
00124 if (mAgent->GetWorldTransform().Pos().z() > mPlayerRadius + 0.01 ||
00125 ballVec.Length() > mPlayerRadius + mBallRadius + mCatchMargin)
00126 {
00127
00128
00129 return true;
00130 }
00131
00132 Vector3f ballPos = mAgent->GetWorldTransform().Pos();
00133 ballPos[2]= mBallRadius;
00134 if (mAgentState->GetTeamIndex() == TI_LEFT)
00135 {
00136 ballPos[0] += mBallRadius + mPlayerRadius + 0.07;
00137 }
00138 else
00139 {
00140 ballPos[0] -= mBallRadius + mPlayerRadius + 0.07;
00141 }
00142
00143 mSoccerRule->ClearPlayersWithException(ballPos,
00144 2.0, 5.0, TI_LEFT, mAgentState);
00145 mSoccerRule->ClearPlayersWithException(ballPos,
00146 2.0, 5.0, TI_RIGHT, mAgentState);
00147
00148 MoveBall(ballPos);
00149
00150 return true;
00151 }
00152
00153 shared_ptr<ActionObject>
00154 CatchEffector::GetActionObject(const Predicate& predicate)
00155 {
00156 do
00157 {
00158 if (predicate.name != GetPredicate())
00159 {
00160 GetLog()->Error() << "ERROR: (CatchEffector) invalid predicate"
00161 << predicate.name << "\n";
00162 break;
00163 }
00164
00165
00166 return shared_ptr<CatchAction>(new CatchAction(GetPredicate()));
00167
00168 } while (0);
00169
00170
00171 return shared_ptr<ActionObject>();
00172 }
00173
00174 void
00175 CatchEffector::OnLink()
00176 {
00177 SoccerBase::GetBallBody(*this,mBallBody);
00178 SoccerBase::GetAgentState(*this,mAgentState);
00179
00180 SoccerBase::GetSoccerRuleAspect(*this,mSoccerRule);
00181
00182 mAgent = shared_dynamic_cast<AgentAspect>(make_shared(GetParent()));
00183
00184 if (mAgent.get() == 0)
00185 {
00186 GetLog()->Error()
00187 << "ERROR: (CatchEffector) parent node is not derived "
00188 << "from AgentAspect\n";
00189 return;
00190 }
00191
00192 shared_ptr<SphereCollider> geom =
00193 shared_dynamic_cast<SphereCollider>(mAgent->GetChild("geometry"));
00194 if (geom.get() == 0)
00195 {
00196 GetLog()->Error()
00197 << "ERROR: (CatchEffector) parent node has no SphereCollider "
00198 << "child\n";
00199 }
00200 else
00201 {
00202 mPlayerRadius = geom->GetRadius();
00203 }
00204
00205 if (! SoccerBase::GetBallCollider(*this,geom))
00206 {
00207 GetLog()->Error()
00208 << "ERROR: (CatchEffector) ball node has no SphereCollider "
00209 << "child\n";
00210 }
00211 else
00212 {
00213 mBallRadius = geom->GetRadius();
00214 }
00215
00216 SoccerBase::GetSoccerVar(*this,"FieldLength",mFieldLength);
00217 SoccerBase::GetSoccerVar(*this,"GoalWidth",mGoalWidth);
00218
00219
00220 mRightPenaltyArea = salt::AABB2(
00221 Vector2f(mFieldLength/2.0 - 16.5, -16.5 - mGoalWidth/2.0),
00222 Vector2f(mFieldLength/2.0 , 16.5 + mGoalWidth/2.0));
00223 mLeftPenaltyArea = salt::AABB2(
00224 Vector2f(-mFieldLength/2.0 + 16.5, -16.5 - mGoalWidth/2.0),
00225 Vector2f(-mFieldLength/2.0, 16.5 + mGoalWidth/2.0));
00226 }
00227
00228 void
00229 CatchEffector::OnUnlink()
00230 {
00231 mSoccerRule.reset();
00232 mBallBody.reset();
00233 mAgent.reset();
00234 mAgentState.reset();
00235 }
00236
00237 void
00238 CatchEffector::SetCatchMargin(float margin)
00239 {
00240 mCatchMargin = margin;
00241 }