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

driveeffector.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: driveeffector.cpp,v 1.6 2004/06/19 11:42:33 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 "driveaction.h"
00023 #include "driveeffector.h"
00024 #include <salt/gmath.h>
00025 #include <zeitgeist/logserver/logserver.h>
00026 #include <oxygen/physicsserver/spherecollider.h>
00027 #include <soccer/soccerbase/soccerbase.h>
00028 
00029 using namespace boost;
00030 using namespace oxygen;
00031 using namespace salt;
00032 
00033 DriveEffector::DriveEffector() : oxygen::Effector(),
00034                                  mForceFactor(60.0),
00035                                  mMaxPower(100.0), mConsumption(1.0/18000.0)
00036 {
00037 }
00038 
00039 DriveEffector::~DriveEffector()
00040 {
00041 }
00042 
00043 bool
00044 DriveEffector::Realize(boost::shared_ptr<ActionObject> action)
00045 {
00046     if (mBody.get() == 0)
00047     {
00048         return false;
00049     }
00050 
00051     shared_ptr<BaseNode> parent =
00052         shared_dynamic_cast<BaseNode>(make_shared(GetParent()));
00053 
00054     if (parent.get() == 0)
00055     {
00056         GetLog()->Error()
00057             << "ERROR: (DriveEffector) parent node is not derived from BaseNode\n";
00058         return false;
00059     }
00060 
00061     shared_ptr<DriveAction> driveAction = shared_dynamic_cast<DriveAction>(action);
00062 
00063     if (driveAction.get() == 0)
00064     {
00065         GetLog()->Error()
00066             << "ERROR: (DriveEffector) cannot realize an unknown ActionObject\n";
00067         return false;
00068     }
00069 
00070     mForce = driveAction->GetForce();
00071 
00072     // cut down the drive power vector to maximum length
00073     if (mForce.SquareLength() > mMaxPower * mMaxPower)
00074     {
00075         mForce.Normalize();
00076         mForce *= mMaxPower;
00077     }
00078 
00079     if (mForceErrorRNG.get() != 0)
00080     {
00081         mForce[0] = mForce[0] * (*(mForceErrorRNG.get()))() * mForceFactor;
00082         mForce[1] = mForce[1] * (*(mForceErrorRNG.get()))() * mForceFactor;
00083         mForce[2] = mForce[2] * (*(mForceErrorRNG.get()))() * mForceFactor;
00084     } else {
00085         mForce = mForce * mForceFactor;
00086     }
00087     return true;
00088 }
00089 
00090 shared_ptr<ActionObject>
00091 DriveEffector::GetActionObject(const Predicate& predicate)
00092 {
00093   if (predicate.name != GetPredicate())
00094     {
00095       GetLog()->Error() << "ERROR: (DriveEffector) invalid predicate"
00096                         << predicate.name << "\n";
00097       return shared_ptr<ActionObject>();
00098     }
00099 
00100   Vector3f force;
00101   if (! predicate.GetValue(predicate.begin(), force))
00102   {
00103       GetLog()->Error()
00104           << "ERROR: (DriveEffector) Vector3f parameter expected\n";
00105       return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
00106   }
00107 
00108   return shared_ptr<ActionObject>(new DriveAction(GetPredicate(),force));
00109 }
00110 
00111 void
00112 DriveEffector::OnLink()
00113 {
00114     SoccerBase::GetTransformParent(*this,mTransformParent);
00115     SoccerBase::GetBody(*this,mBody);
00116     SoccerBase::GetAgentState(*this,mAgentState);
00117 
00118     shared_ptr<SphereCollider> geom =
00119         shared_dynamic_cast<SphereCollider>(mTransformParent->GetChild("geometry"));
00120 
00121     mMaxDistance = 0.001;
00122     if (geom.get() == 0)
00123     {
00124         GetLog()->Error() << "ERROR: (DriveEffector) parent node has "
00125                           << "no 'geometry' sphere child\n";
00126     } else {
00127             mMaxDistance += geom->GetRadius();
00128     }
00129 }
00130 
00131 void
00132 DriveEffector::OnUnlink()
00133 {
00134     mForceErrorRNG.reset();
00135     mTransformParent.reset();
00136     mBody.reset();
00137 }
00138 
00139 void
00140 DriveEffector::SetForceFactor(float force_factor)
00141 {
00142     mForceFactor = salt::gAbs(force_factor);
00143 }
00144 
00145 void
00146 DriveEffector::SetSigma(float sigma)
00147 {
00148     NormalRngPtr rng(new salt::NormalRNG<>(1.0,sigma));
00149     mForceErrorRNG = rng;
00150 }
00151 
00152 void
00153 DriveEffector::SetMaxPower(float max_power)
00154 {
00155     mMaxPower = max_power;
00156 }
00157 
00158 void
00159 DriveEffector::PrePhysicsUpdateInternal(float deltaTime)
00160 {
00161     Effector::PrePhysicsUpdateInternal(deltaTime);
00162     if (mBody.get() == 0 ||
00163         mForce.Length() <= std::numeric_limits<float>::epsilon())
00164     {
00165         return;
00166     }
00167 
00168     Vector3f vec = mTransformParent->GetWorldTransform().Pos();
00169     if (vec.z() > mMaxDistance) return;
00170 
00171     if (mAgentState->ReduceBattery(mForce.Length() * mConsumption))
00172     {
00173         mBody->AddForce(SoccerBase::FlipView(mForce,mAgentState->GetTeamIndex()));
00174     }
00175 }
00176 
00177 void
00178 DriveEffector::SetConsumption(float consume_time)
00179 {
00180     mConsumption = 1.0 / consume_time;
00181 }
00182 
00183 

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