Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

kickeffector.cpp

Go to the documentation of this file.
00001 /* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
00002 
00003    this file is part of rcssserver3D
00004    Fri May 9 2003
00005    Copyright (C) 2002,2003 Koblenz University
00006    Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
00007    $Id: kickeffector.cpp,v 1.11 2006/03/01 15:51:58 fruit Exp $
00008 
00009    This program is free software; you can redistribute it and/or modify
00010    it under the terms of the GNU General Public License as published by
00011    the Free Software Foundation; version 2 of the License.
00012 
00013    This program is distributed in the hope that it will be useful,
00014    but WITHOUT ANY WARRANTY; without even the implied warranty of
00015    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016    GNU General Public License for more details.
00017 
00018    You should have received a copy of the GNU General Public License
00019    along with this program; if not, write to the Free Software
00020    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00021 */
00022 #include "kickaction.h"
00023 #include "kickeffector.h"
00024 #include <salt/random.h>
00025 #include <zeitgeist/logserver/logserver.h>
00026 #include <oxygen/sceneserver/transform.h>
00027 #include <oxygen/physicsserver/spherecollider.h>
00028 #include <soccer/soccerbase/soccerbase.h>
00029 
00030 using namespace boost;
00031 using namespace oxygen;
00032 using namespace salt;
00033 using namespace std;
00034 
00035 KickEffector::KickEffector()
00036     : oxygen::Effector(),
00037       mKickMargin(0.04),mPlayerRadius(0.0),mBallRadius(0.0),
00038       mForceFactor(4.0),mTorqueFactor(0.1),
00039       mMaxPower(100.0),
00040       mMinAngle(0.0),mMaxAngle(50.0), mSteps(10),
00041       mSigmaPhiEnd(0.9), mSigmaPhiMid(4.5)
00042 {
00043 }
00044 
00045 KickEffector::~KickEffector()
00046 {
00047 }
00048 
00049 bool
00050 KickEffector::Realize(boost::shared_ptr<ActionObject> action)
00051 {
00052     // this should also include the case when there is no ball
00053     // (because then there will be no body, neither).
00054     if (mBallBody.get() == 0)
00055     {
00056         return false;
00057     }
00058 
00059     if (mAgent.get() == 0)
00060     {
00061         GetLog()->Error()
00062             << "ERROR: (KickEffector) parent node is not derived from BaseNode\n";
00063         return false;
00064     }
00065 
00066     shared_ptr<KickAction> kickAction =
00067         shared_dynamic_cast<KickAction>(action);
00068 
00069     if (kickAction.get() == 0)
00070     {
00071         GetLog()->Error()
00072             << "ERROR: (KickEffector) cannot realize an unknown ActionObject\n";
00073         return false;
00074     }
00075 
00076     // if the agent doesn't have a body, we're done (this should never happen)
00077     if (mBall.get() == 0) return true;
00078 
00079     Vector3f force =
00080         mBallBody->GetWorldTransform().Pos() -
00081         mAgent->GetWorldTransform().Pos();
00082 
00083     // the ball can be kicked if the distance is
00084     // less then Ball-Radius + Player-Radius + KickMargin AND
00085     // the player is close to the ground
00086     if (mAgent->GetWorldTransform().Pos().z() > mPlayerRadius + 0.01 ||
00087         force.Length() > mPlayerRadius + mBallRadius + mKickMargin)
00088     {
00089         // ball is out of reach, or player is in the air:
00090         // kick has no effect
00091         return true;
00092     }
00093 
00094     // get the kick angle in the horizontal plane
00095     double theta = salt::gArcTan2(force[1], force[0]);
00096     if (mThetaErrorRNG.get() != 0)
00097     {
00098         theta += (*(mThetaErrorRNG.get()))();
00099     }
00100 
00101     float phi = salt::gMin(salt::gMax(kickAction->GetAngle(), mMinAngle), mMaxAngle);
00102     if (mSigmaPhiEnd > 0.0 || mSigmaPhiMid > 0.0)
00103     {
00104         // f will be close to 0.0 if the angle is near the minimum or the maximum.
00105         // f will be close to 1.0 if the angle is somewhere in the middle of the range.
00106         float f = 1.0 - 2.0 * salt::gAbs((phi - mMinAngle) / (mMaxAngle - mMinAngle) - 0.5);
00107         // f is set to a number between mSigmaPhiEnd and mSigmaPhiMid
00108         f = salt::gMax(mSigmaPhiEnd + f * (mSigmaPhiMid-mSigmaPhiEnd), 0.0);
00109         phi = salt::NormalRNG<>(phi,f)();
00110     }
00111     phi = salt::gDegToRad(90.0-phi);
00112 
00113     // x = r * cos(theta) * sin(90 - phi), with r = 1.0
00114     force[0] = salt::gCos(theta) * salt::gSin(phi);
00115     // y = r * sin(theta) * sin(90 - phi), with r = 1.0
00116     force[1] = salt::gSin(theta) * salt::gSin(phi);
00117     // z = r * cos(90 - phi),              with r = 1.0
00118     force[2] = salt::gCos(phi);
00119 
00120     float kick_power = salt::gMin(salt::gMax(kickAction->GetPower(), 1.0f), mMaxPower);
00121     if (mForceErrorRNG.get() != 0)
00122     {
00123         kick_power += (*(mForceErrorRNG.get()))();
00124     }
00125 
00126     force *= (mForceFactor * kick_power);
00127 
00128     const Vector3f torque(mTorqueFactor*force[1]/salt::g2PI,
00129                           mTorqueFactor*force[0]/salt::g2PI,
00130                           0.0);
00131 
00132     mBall->SetAcceleration(mSteps,force,torque,mAgent);
00133     return true;
00134 }
00135 
00136 shared_ptr<ActionObject>
00137 KickEffector::GetActionObject(const Predicate& predicate)
00138 {
00139   do
00140   {
00141       if (predicate.name != GetPredicate())
00142           {
00143               GetLog()->Error() << "ERROR: (KickEffector) invalid predicate"
00144                                 << predicate.name << "\n";
00145               break;
00146           }
00147 
00148       Predicate::Iterator iter = predicate.begin();
00149 
00150       float angle;
00151       if (! predicate.AdvanceValue(iter, angle))
00152       {
00153           GetLog()->Error()
00154               << "ERROR: (KickEffector) kick angle parameter expected\n";
00155           break;
00156       }
00157 
00158       float power;
00159       if (! predicate.AdvanceValue(iter, power))
00160           {
00161               GetLog()->Error()
00162                   << "ERROR: (KickEffector) kick power expected\n";
00163               break;
00164           }
00165 
00166       // construct the KickAction object
00167       return shared_ptr<KickAction>(new KickAction(GetPredicate(),angle,power));
00168 
00169   } while (0);
00170 
00171   // some error happened
00172   return shared_ptr<ActionObject>();
00173 }
00174 
00175 void
00176 KickEffector::OnLink()
00177 {
00178     SoccerBase::GetBall(*this,mBall);
00179     SoccerBase::GetBallBody(*this,mBallBody);
00180 
00181     mAgent = shared_dynamic_cast<AgentAspect>(make_shared(GetParent()));
00182 
00183     if (mAgent.get() == 0)
00184     {
00185         GetLog()->Error()
00186             << "ERROR: (KickEffector) parent node is not derived from AgentAspect\n";
00187         return;
00188     }
00189 
00190     shared_ptr<SphereCollider> geom =
00191         shared_dynamic_cast<SphereCollider>(mAgent->GetChild("geometry"));
00192     if (geom.get() == 0)
00193     {
00194         GetLog()->Error()
00195             << "ERROR: (KickEffector) parent node has no SphereCollider child\n";
00196     } else
00197         {
00198             mPlayerRadius = geom->GetRadius();
00199         }
00200 
00201     if (! SoccerBase::GetBallCollider(*this,geom))
00202     {
00203         GetLog()->Error()
00204             << "ERROR: (KickEffector) ball node has no SphereCollider child\n";
00205     } else
00206     {
00207         mBallRadius = geom->GetRadius();
00208     }
00209 }
00210 
00211 void
00212 KickEffector::OnUnlink()
00213 {
00214     mForceErrorRNG.reset();
00215     mThetaErrorRNG.reset();
00216     mBallBody.reset();
00217     mAgent.reset();
00218 }
00219 
00220 void
00221 KickEffector::SetKickMargin(float margin)
00222 {
00223     mKickMargin = margin;
00224 }
00225 
00226 void
00227 KickEffector::SetNoiseParams(double sigma_force, double sigma_theta,
00228                              double sigma_phi_end, double sigma_phi_mid)
00229 {
00230     NormalRngPtr rng(new salt::NormalRNG<>(0.0,sigma_force));
00231     mForceErrorRNG = rng;
00232     NormalRngPtr rng2(new salt::NormalRNG<>(0.0,sigma_theta));
00233     mThetaErrorRNG = rng2;
00234     mSigmaPhiEnd = sigma_phi_end;
00235     mSigmaPhiMid = sigma_phi_mid;
00236 }
00237 
00238 void
00239 KickEffector::SetForceFactor(float force_factor)
00240 {
00241     mForceFactor = force_factor;
00242 }
00243 
00244 void
00245 KickEffector::SetTorqueFactor(float torque_factor)
00246 {
00247     mTorqueFactor = torque_factor;
00248 }
00249 
00250 void
00251 KickEffector::SetSteps(int steps)
00252 {
00253     mSteps = steps;
00254 }
00255 
00256 void
00257 KickEffector::SetMaxPower(float max_power)
00258 {
00259     mMaxPower = max_power;
00260 }
00261 
00262 void
00263 KickEffector::SetAngleRange(float min, float max)
00264 {
00265     if (max <= min)
00266     {
00267         GetLog()->Error()
00268             << "ERROR: (KickEffector) min. kick angle should be < max kick angle\n";
00269         return;
00270     }
00271     mMinAngle = min;
00272     mMaxAngle = max;
00273 }

Generated on Thu Apr 6 15:25:38 2006 for rcssserver3d by  doxygen 1.4.4