00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
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