Simulator.h

#ifndef WORLD_SIMULATION_H
#define WORLD_SIMULATION_H

#include <Klampt/Modeling/World.h>
#include "ODESimulator.h"
#include "SimRobotController.h"
#include <map>

namespace Klampt {

struct ContactFeedbackInfo
{
  //summary information
  bool accum;      
  int contactCount,separationCount; 
  bool inContact; 
  Vector3 meanForce,meanTorque,meanPoint;

  bool penetrating;  
  int penetrationCount;   

  //full contact information over sub-steps
  bool accumFull;  //set to true if all ODEContactLists should be stored
  vector<double> times;
  vector<ODEContactList> contactLists;
};

class SimulatorHook
{
 public:
  SimulatorHook() : autokill(false) {}
  virtual ~SimulatorHook() {}
  virtual void Step(Real dt) {}
  virtual bool ReadState(File& f) { return true; }
  virtual bool WriteState(File& f) const { return true; }
  bool autokill;
};

class Simulator
{
public:
  Simulator();
  void Init(WorldModel* world);
  void OnAddModel();
  void SetController(int robot,shared_ptr<RobotController> c);
  void Advance(Real dt);
  void AdvanceFake(Real dt);
  void UpdateModel(); 
  void UpdateRobot(int index); 
  bool ReadState(File& f);
  bool WriteState(File& f) const;
  bool ReadState(const string& data);
  bool WriteState(string& data) const;

  //contact querying routines
  void EnableContactFeedback(int aid,int bid,bool accum=true,bool accumFull=false);
  bool InContact(int aid,int bid=-1);
  bool HadContact(int aid,int bid=-1);
  bool HadSeparation(int aid,int bid=-1);
  bool HadPenetration(int aid,int bid=-1);
  ContactFeedbackInfo* GetContactFeedback(int aid,int bid);
  ODEContactList* GetContactList(int aid,int bid);
  Vector3 ContactForce(int aid,int bid=-1);
  Vector3 ContactTorque(int aid,int bid=-1);
  Vector3 MeanContactForce(int aid,int bid=-1);
  Vector3 MeanContactTorque(int aid,int bid=-1);

  //helpers to convert indexing schemes
  int ODEToWorldID(const ODEObjectID& odeid) const;
  ODEObjectID WorldToODEID(int id) const;

  WorldModel* world;
  ODESimulator odesim;
  Real time;
  Real simStep;
  bool fakeSimulation;
  vector<SimRobotController> controlSimulators;
  vector<shared_ptr<RobotController> > robotControllers;
  vector<shared_ptr<SimulatorHook> > hooks;
  typedef map<pair<ODEObjectID,ODEObjectID>,ContactFeedbackInfo> ContactFeedbackMap;
  ContactFeedbackMap contactFeedback;
  ODESimulator::Status worstStatus;
};

class ForceHook : public SimulatorHook
{
 public:
  ForceHook(dBodyID body,const Vector3& worldpt,const Vector3& f);
  virtual void Step(Real dt) override;
  virtual bool ReadState(File& f) override;
  virtual bool WriteState(File& f) const override;

  dBodyID body;
  Vector3 worldpt,f;
};

class LocalForceHook : public SimulatorHook
{
 public:
  LocalForceHook(dBodyID body,const Vector3& localpt,const Vector3& f);
  virtual void Step(Real dt) override;
  virtual bool ReadState(File& f) override;
  virtual bool WriteState(File& f) const override;

  dBodyID body;
  Vector3 localpt,f;
};


class WrenchHook : public SimulatorHook
{
 public:
  WrenchHook(dBodyID body,const Vector3& f,const Vector3& m);
  virtual void Step(Real dt) override;
  virtual bool ReadState(File& f) override;
  virtual bool WriteState(File& f) const override;

  dBodyID body;
  Vector3 f,m;
};

class SpringHook : public SimulatorHook
{
 public:
  SpringHook(dBodyID body,const Vector3& worldpt,const Vector3& target,Real kP,Real kD=0);
  virtual void Step(Real dt) override;
  virtual bool ReadState(File& f) override;
  virtual bool WriteState(File& f) const override;

  dBodyID body;
  Vector3 localpt,target;
  Real kP,kD;
};

class JointForceHook : public SimulatorHook
{
public:
  JointForceHook(ODEJoint* joint,Real f);
  virtual void Step(Real dt) override;
  virtual bool ReadState(File& f) override;
  virtual bool WriteState(File& f) const override;

  ODEJoint* joint;
  Real f;
};

class JointSpringHook : public SimulatorHook
{
public:
  JointSpringHook(ODEJoint* joint,Real target,Real kP,Real kD=0);
  virtual void Step(Real dt) override;
  virtual bool ReadState(File& f) override;
  virtual bool WriteState(File& f) const override;

  ODEJoint* joint;
  Real target;
  Real kP,kD;
};

} //namespace Klampt

#endif