00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00053
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
00077 if (mBall.get() == 0) return true;
00078
00079 Vector3f force =
00080 mBallBody->GetWorldTransform().Pos() -
00081 mAgent->GetWorldTransform().Pos();
00082
00083
00084
00085
00086 if (mAgent->GetWorldTransform().Pos().z() > mPlayerRadius + 0.01 ||
00087 force.Length() > mPlayerRadius + mBallRadius + mKickMargin)
00088 {
00089
00090
00091 return true;
00092 }
00093
00094
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
00105
00106 float f = 1.0 - 2.0 * salt::gAbs((phi - mMinAngle) / (mMaxAngle - mMinAngle) - 0.5);
00107
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
00114 force[0] = salt::gCos(theta) * salt::gSin(phi);
00115
00116 force[1] = salt::gSin(theta) * salt::gSin(phi);
00117
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
00167 return shared_ptr<KickAction>(new KickAction(GetPredicate(),angle,power));
00168
00169 } while (0);
00170
00171
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 }