ODEPhysics.hh

Go to the documentation of this file.

/*
 * Copyright 2011 Nate Koenig
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
/* Desc: The ODE physics engine wrapper
 * Author: Nate Koenig
 * Date: 11 June 2007
 */

#ifndef ODEPHYSICS_HH
#define ODEPHYSICS_HH

#include <tbb/spin_mutex.h>
#include <tbb/concurrent_vector.h>
#include <map>
#include <string>
#include <vector>
#include <utility>

#include <boost/thread/thread.hpp>

#include "physics/ode/ode_inc.h"

#include "physics/ode/ODETypes.hh"

#include "physics/PhysicsEngine.hh"
#include "physics/Contact.hh"
#include "physics/Shape.hh"
#include "gazebo_config.h"

namespace gazebo
{
  namespace physics
  {

    class ContactFeedback
    {
      public: Contact contact;
      public: dJointFeedback feedbacks[MAX_CONTACT_JOINTS];
      public: int feedbackCount;
    };

    class ODEPhysics : public PhysicsEngine
    {
      public: ODEPhysics(WorldPtr world);

      public: virtual ~ODEPhysics();

      public: virtual void Load(sdf::ElementPtr _sdf);

      public: virtual void Init();

      public: void Reset();

      public: virtual void InitForThread();

      public: virtual void UpdateCollision();

      public: virtual void UpdatePhysics();

      public: virtual void Fini();

      public: void SetStepTime(double _value);

      public: virtual double GetStepTime();

      public: virtual LinkPtr CreateLink(ModelPtr _parent);

      public: virtual CollisionPtr CreateCollision(
                  const std::string &_shapeType, LinkPtr _parent);

      public: virtual ShapePtr CreateShape(const std::string &_shapeType,
                                           CollisionPtr _collision);

      public: virtual JointPtr CreateJoint(const std::string &type,
                                           ModelPtr _parent);

      public: dSpaceID GetSpaceId() const;

      public: dWorldID GetWorldId();

      public: static void ConvertMass(InertialPtr _interial, void *odeMass);

      public: static void ConvertMass(void *odeMass, InertialPtr _inertial);

      public: virtual std::string GetStepType() const;

      public: virtual void SetStepType(const std::string &_type);

      public: virtual void SetGravity(const gazebo::math::Vector3 &gravity);

      public: math::Vector3 GetGravity() const;

      public: void SetWorldCFM(double cfm);
      public: void SetWorldERP(double erp);
      public: void SetSORPGSPreconIters(unsigned int iters);
      public: void SetSORPGSIters(unsigned int iters);
      public: void SetSORPGSW(double w);
      public: void SetContactMaxCorrectingVel(double vel);
      public: void SetContactSurfaceLayer(double layer_depth);
      public: void SetMaxContacts(unsigned int max_contacts);

      public: double GetWorldCFM();
      public: double GetWorldERP();
      public: int GetSORPGSPreconIters();
      public: int GetSORPGSIters();
      public: double GetSORPGSW();
      public: double GetContactMaxCorrectingVel();
      public: double GetContactSurfaceLayer();
      public: int GetMaxContacts();

      public: void CreateContact(ODECollision *collision1,
                                 ODECollision *collision2);

      private: static void CollisionCallback(void *data,
                                             dGeomID o1, dGeomID o2);

      public: void Collide(ODECollision *collision1, ODECollision *collision2,
                           dContactGeom *contactCollisions);

      public: void ProcessContactFeedback(ContactFeedback* feedback,
                                          msgs::Contact *_msg);

      public: virtual void DebugPrint() const;

      protected: virtual void OnRequest(
                   ConstRequestPtr &/*_msg*/);

      protected: virtual void OnPhysicsMsg(
                   ConstPhysicsPtr &/*_msg*/);

      private: void  AddTrimeshCollider(ODECollision *_collision1,
                                         ODECollision *_collision2);


      private: void  AddCollider(ODECollision *_collision1,
                                  ODECollision *_collision2);

      private: dWorldID worldId;

      private: dSpaceID spaceId;

      private: dJointGroupID contactGroup;

      private: double stepTimeDouble;
      private: std::string stepType;

      private: std::vector<ContactFeedback*> contactFeedbacks;
      private: unsigned int contactFeedbackIndex;

      private: std::map<std::string, dSpaceID> spaces;

      private: std::vector< std::pair<ODECollision*, ODECollision*> > colliders;
      private: std::vector< std::pair<ODECollision*, ODECollision*> >
               trimeshColliders;

      private: unsigned int collidersCount, trimeshCollidersCount;

      private: tbb::spin_mutex collideMutex;

      private: dContactGeom contactCollisions[MAX_COLLIDE_RETURNS];
      private: int (*physicsStepFunc)(dxWorld*, dReal);

      private: int indices[MAX_CONTACT_JOINTS];
    };
    typedef boost::shared_ptr<ODEPhysics> ODEPhysicsPtr;

  }
}
#endif