Revision: 7180 http://playerstage.svn.sourceforge.net/playerstage/?rev=7180&view=rev Author: natepak Date: 2008-11-27 16:37:06 +0000 (Thu, 27 Nov 2008)
Log Message: ----------- Added patch 2281269 Modified Paths: -------------- code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh Modified: code/gazebo/trunk/server/physics/Body.cc =================================================================== --- code/gazebo/trunk/server/physics/Body.cc 2008-11-27 16:30:16 UTC (rev 7179) +++ code/gazebo/trunk/server/physics/Body.cc 2008-11-27 16:37:06 UTC (rev 7180) @@ -70,6 +70,9 @@ this->rpyP = new ParamT<Quatern>("rpy", Quatern(), 0); this->rpyP->Callback( &Body::SetRotation, this ); + + this->dampingFactorP = new ParamT<double>("dampingFactor", 0.03, 0); + Param::End(); } @@ -95,6 +98,7 @@ delete this->xyzP; delete this->rpyP; + delete this->dampingFactorP; } @@ -107,6 +111,7 @@ this->nameP->Load(node); this->xyzP->Load(node); this->rpyP->Load(node); + this->dampingFactorP->Load(node); Pose3d initPose; initPose.pos = **(this->xyzP); @@ -217,7 +222,11 @@ { std::vector< Sensor* >::iterator sensorIter; std::map< std::string, Geom* >::iterator geomIter; + Vector3 vel; + Vector3 avel; + double force; + this->UpdatePose(); if (!this->IsStatic()) @@ -237,6 +246,17 @@ { (*sensorIter)->Update(); } + + if(this->GetId()) + { + force = this->dampingFactorP->GetValue() * this->mass.mass; + vel = this->GetLinearVel(); + dBodyAddForce(this->GetId(), -((vel.x * fabs(vel.x)) * force), -((vel.y * fabs(vel.y)) * force), -((vel.z * fabs(vel.z)) * force)); + + avel = this->GetAngularVel(); + dBodyAddTorque(this->GetId(), -avel.x * force, -avel.y * force, -avel.z * force); + } + } //////////////////////////////////////////////////////////////////////////////// @@ -280,7 +300,7 @@ //this->SetPosition(this->staticPose.pos); //this->SetRotation(this->staticPose.rot); - + for (iter = this->geoms.begin(); iter != this->geoms.end(); iter++) { //newPose = (*iter)->GetPose() - this->staticPose; @@ -702,7 +722,7 @@ { return dynamic_cast<Model*>(this->GetParent()); } - + //////////////////////////////////////////////////////////////////////////////// /// Get a geom by name Geom *Body::GetGeom(const std::string &name) const @@ -719,4 +739,4 @@ return NULL; } } - + Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2008-11-27 16:30:16 UTC (rev 7179) +++ code/gazebo/trunk/server/physics/Body.hh 2008-11-27 16:37:06 UTC (rev 7180) @@ -1,6 +1,6 @@ /* * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 + * Copyright (C) 2003 * Nate Koenig & Andrew Howard * * This program is free software; you can redistribute it and/or modify @@ -47,97 +47,97 @@ /// \addtogroup gazebo_physics /// \brief The body class /// \{ - + /// Body class class Body : public Entity { /// \brief Constructor public: Body(Entity *parent, dWorldID worldId); - + /// \brief Destructor public: virtual ~Body(); - + /// \brief Load the body based on an XMLConfig node /// \param node XMLConfigNode pointer public: virtual void Load(XMLConfigNode *node); - + /// \brief Save the body based on our XMLConfig node public: virtual void Save(std::string &prefix, std::ostream &stream); - + /// \brief Initialize the body public: virtual void Init(); - + /// \brief Finalize the body public: void Fini(); - + /// \brief Update the body public: virtual void Update(); - + /// \brief Attach a geom to this body /// \param geom Geometery to attach to this body public: void AttachGeom( Geom *geom ); - + /// \brief Set the pose of the body /// \param pose New pose of the body public: void SetPose(const Pose3d &pose); - + /// \brief Return the pose of the body /// \return Pose of the body public: Pose3d GetPose() const; - + /// \brief Set the position of the body /// \param pos Vector position public: void SetPosition(const Vector3 &pos); - + /// \brief Set the rotation of the body /// \param rot Quaternion rotation public: void SetRotation(const Quatern &rot); - + /// \brief Return the position of the body /// \return Position vector public: Vector3 GetPosition() const; - + /// \brief Return the rotation /// \return Rotation quaternion public: Quatern GetRotation() const; - + /// \brief Return the ID of this body /// \return ODE body id public: dBodyID GetId() const; - + /// \brief Set whether this body is enabled public: void SetEnabled(bool enable) const; - + /// \brief Update the center of mass public: void UpdateCoM(); - + /// \brief Get the Center of Mass pose public: const Pose3d &GetCoMPose() const; - + /// \brief Set whether gravity affects this body public: void SetGravityMode(bool mode); - + /// \brief Set the linear velocity of the body public: void SetLinearVel(const Vector3 &vel); - + /// \brief Get the linear velocity of the body public: Vector3 GetLinearVel() const; - + /// \brief Set the angular velocity of the body public: void SetAngularVel(const Vector3 &vel); - + /// \brief Get the angular velocity of the body public: Vector3 GetAngularVel() const; - + /// \brief Set the force applied to the body public: void SetForce(const Vector3 &force); - + /// \brief Get the force applied to the body public: Vector3 GetForce() const; - + /// \brief Set the torque applied to the body public: void SetTorque(const Vector3 &force); - + /// \brief Get the torque applied to the body public: Vector3 GetTorque() const; @@ -149,43 +149,48 @@ /// \brief Get the model that this body belongs to public: Model *GetModel() const; - + + /// \brief Get the mass of the body + public: float GetMass() { return mass.mass; } + /// Load a new geom helper function /// \param node XMLConfigNode used to load the geom private: void LoadGeom(XMLConfigNode *node); - + /// Load a new sensor /// \param node XMLConfigNode used to load the geom private: void LoadSensor(XMLConfigNode *node); - + /// \brief Load a renderable private: void LoadVisual(XMLConfigNode *node); /// \brief Update the pose of the body private: void UpdatePose(); - + /// List of geometries attached to this body private: std::map< std::string, Geom* > geoms; - + /// List of attached sensors private: std::vector< Sensor* > sensors; - + /// ODE body handle private: dBodyID bodyId; - + /// Mass properties of the object private: dMass mass; - + private: bool isStatic; - + private: Pose3d comPose; private: Pose3d staticPose; private: Pose3d pose; - + private: ParamT<Vector3> *xyzP; private: ParamT<Quatern> *rpyP; + + private: ParamT<double> *dampingFactorP; }; - + /// \} } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. ------------------------------------------------------------------------- This SF.Net email is sponsored by the Moblin Your Move Developer's challenge Build the coolest Linux based applications with Moblin SDK & win great prizes Grand prize is a trip for two to an Open Source event anywhere in the world http://moblin-contest.org/redirect.php?banner_id=100&url=/ _______________________________________________ Playerstage-commit mailing list Playerstage-commit@lists.sourceforge.net https://lists.sourceforge.net/lists/listinfo/playerstage-commit