ODESimulator.h

#ifndef ODE_INTERFACE_SIMULATOR_H
#define ODE_INTERFACE_SIMULATOR_H

#include "ODERobot.h"
#include "ODERigidObject.h"
#include "ODESurface.h"
#include <Klampt/Modeling/Terrain.h>
#include <Klampt/Modeling/RigidObject.h>
#include <KrisLibrary/robotics/Contact.h>
#include <ode/contact.h>
#include <map>

namespace Klampt {

struct ODEObjectID;
struct ODEContactList;
struct ODEContactResult;
struct ODEJoint;

struct ODESimulatorSettings
{
  ODESimulatorSettings();

  double gravity[3];
  bool autoDisable;
  double defaultEnvPadding;
  ODESurfaceProperties defaultEnvSurface;

  //collision checking settings
  bool boundaryLayerCollisions;
  bool rigidObjectCollisions;
  bool robotSelfCollisions;
  bool robotRobotCollisions;
  bool adaptiveTimeStepping;
  double minimumAdaptiveTimeStep;

  //contact detection settings
  int maxContacts;
  double clusterNormalScale;

  //ODE constants, mostly relevant to tightness of robot constraints
  double errorReductionParameter;
  double dampedLeastSquaresParameter;

  //Instability detection / correction parameters
  double instabilityConstantEnergyThreshold,instabilityLinearEnergyThreshold;
  double instabilityMaxEnergyThreshold;
  double instabilityPostCorrectionEnergy;
};



class ODESimulator
{
 public:
  enum Status { StatusNormal=0, StatusAdaptiveTimeStepping=1, StatusContactUnreliable=2, StatusUnstable=3, StatusError=4 };

  ODESimulator();
  virtual ~ODESimulator();
  void SetGravity(const Vector3& g);
  void SetAutoDisable(bool autoDisable=true);  //global auto disable -- see ODE docs
  void SetERP(double erp);   //global error reduction  -- see ODE docs
  void SetCFM(double erp);   //global constraint force mixing -- see ODE docs
  ODESimulatorSettings& GetSettings() { return settings; }
  Status GetStatus() const; 
  void GetStatusHistory(vector<Status>& statuses,vector<Real>& statusChangeTimes) const;
  void AddTerrain(TerrainModel& terr);
  void AddRobot(RobotModel& robot);
  void AddObject(RigidObjectModel& object);
  bool CheckObjectOverlap(vector<pair<ODEObjectID,ODEObjectID> >& overlaps);
  void Step(Real dt);
  void StepDynamics(Real dt);
  bool ReadState(File& f);
  bool WriteState(File& f) const;

  size_t numTerrains() const { return terrains.size(); }
  size_t numRobots() const { return robots.size(); }
  size_t numObjects() const { return objects.size(); }
  inline dWorldID world() const { return worldID; }
  const TerrainModel* terrain(int i) const { return terrains[i]; }
  ODEGeometry* terrainGeom(int i) const { return terrainGeoms[i]; }
  ODERobot* robot(int i) const { return robots[i]; }
  ODERigidObject* object(int i) const { return objects[i]; }

  string ObjectName(const ODEObjectID& obj) const;
  dBodyID ObjectBody(const ODEObjectID& obj) const;
  dGeomID ObjectGeom(const ODEObjectID& obj) const;
  void EnableContactFeedback(const ODEObjectID& a,const ODEObjectID& b);
  ODEContactList* GetContactFeedback(const ODEObjectID& a,const ODEObjectID& b);
  void GetContactFeedback(const ODEObjectID& a,vector<ODEContactList*>& contacts);
  void ClearContactFeedback();
  bool InContact(const ODEObjectID& a) const;
  bool InContact(const ODEObjectID& a,const ODEObjectID& b) const;
  void DisableInstabilityCorrection();
  void DisableInstabilityCorrection(const ODEObjectID& obj);

  ODEJoint* AddJoint(const ODEObjectID& obj);
  ODEJoint* AddJoint(const ODEObjectID& a,const ODEObjectID& b);
  void RemoveJoint(ODEJoint*);
  void RemoveJoints(const ODEObjectID& obj);
  void RemoveJoints(const ODEObjectID& a,const ODEObjectID& b);

  //used internally
  bool ReadState_Internal(File& f);
  bool WriteState_Internal(File& f) const;
  void DetectCollisions();
  void SetupContactResponse(); 
  void SetupContactResponse(const ODEObjectID& a,const ODEObjectID& b,int feedbackIndex,ODEContactResult& c);
  void ClearCollisions();
  bool InstabilityCorrection();
    
  //overload this to have custom parameters for surface pairs
  virtual void GetSurfaceParameters(const ODEObjectID& a,const ODEObjectID& b,dSurfaceParameters& surface) const;

 private:
  vector<pair<Status,Real> > statusHistory;
  ODESimulatorSettings settings;
  dWorldID worldID;
  dSpaceID envSpaceID;
  vector<ODEGeometry*> terrainGeoms;
  vector<const TerrainModel*> terrains;
  vector<ODERobot*> robots;
  vector<ODERigidObject*> objects;
  map<pair<ODEObjectID,ODEObjectID>,ODEContactList> contactList;
  dJointGroupID contactGroupID;
  Real timestep;
  Real simTime;
  map<ODEObjectID,Real> energies;

public:
  //for adaptive time stepping
  File lastState;
  Real lastStateTimestep;
  map<pair<ODEObjectID,ODEObjectID>,double> lastMarginsRemaining;
  //joints
  list<ODEJoint> joints;
};


struct ODEObjectID
{
  inline ODEObjectID(int _t=-1,int _i=-1,int _b=-1)
    :type(_t),index(_i),bodyIndex(_b) {}
  inline void SetEnv(int _index=0) { type = 0; index = _index; }
  inline void SetRobot(int _index=0) { type = 1; index = _index; bodyIndex = -1; }
  inline void SetRobotBody(int _index,int _bodyIndex=-1) { type = 1; index = _index; bodyIndex = _bodyIndex; }
  inline void SetRigidObject(int _index=0) { type = 2; index = _index; }
  inline bool IsEnv() const { return type == 0; }
  inline bool IsRobot() const { return type == 1; }
  inline bool IsRigidObject() const { return type == 2; }
  inline bool operator == (const ODEObjectID& rhs) const {
    if(type != rhs.type) return false;
    if(index != rhs.index) return false;
    if(type == 1 && bodyIndex!=rhs.bodyIndex) return false;
    return true;
  }
  inline bool operator < (const ODEObjectID& rhs) const {
    if(type < rhs.type) return true;
    else if(type > rhs.type) return false;
    if(index < rhs.index) return true;
    else if(index > rhs.index) return false;
    return (bodyIndex<rhs.bodyIndex);
  }

  int type;       //0: static environment, 1: robot, 2: movable object
  int index;      //index in world
  int bodyIndex;  //for robots, this identifies a link
};

struct ODEContactList
{
  ODEObjectID o1,o2;
  //the contact points
  vector<ContactPoint> points;
  vector<Vector3> forces;
  //whether the contact detector found excessive penetration
  bool penetrating;   

  vector<int> feedbackIndices;           //internally used
};

struct ODEJoint
{
  ODEJoint();
  ~ODEJoint();
  void MakeFixed();
  void MakeHinge(const Vector3& pt,const Vector3& axis);
  void MakeSlider(const Vector3& dir);
  void Destroy();
  
  Real GetPosition();
  Real GetVelocity();

  void SetLimits(Real min,Real max);
  void SetBounce(Real coeff);
  void SetFriction(Real coeff);
  void SetFixedVelocity(Real vel,Real fmax);
  void AddForce(Real f);
  void GetConstraintForces(Vector3& f1,Vector3& t1,Vector3& f2,Vector3& t2);

  int type; //<-1 not initialized, 0 fixed, 1 hinge, 2 slider
  ODEObjectID o1,o2;
  ODESimulator* sim;
  dJointID joint;
  dJointFeedback feedback;
};

} //namespace Klampt

#endif