ODEPhysics.hh

/*
 * Copyright 2011 Nate Koenig & Andrew Howard
 *
 * 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 "physics/ode/ode_inc.h"

#include <tbb/spin_mutex.h>
#include <tbb/concurrent_vector.h>

#include <boost/thread/thread.hpp>

#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 SetUpdateRate(double _value);

      public: virtual double GetUpdateRate();
      public: virtual double GetUpdatePeriod();

      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);
    
      public: dSpaceID GetSpaceId() const;
    
      public: dWorldID GetWorldId();
    
      public: static void ConvertMass(InertialPtr &_interial, void *odeMass);
    
      public: static void ConvertMass(void *odeMass, const 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 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 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);

      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 updateRateDouble; 

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

      private: std::vector<ContactFeedback*> contactFeedbacks;
    
      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_DCOLLIDE_RETURNS];
      private: int (*physicsStepFunc)(dxWorld*, dReal);

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