RobotWithGeometry.h

#ifndef ROBOT_WITH_GEOMETRY_H
#define ROBOT_WITH_GEOMETRY_H

#include "RobotDynamics3D.h"
#include <KrisLibrary/structs/array2d.h>
#include <KrisLibrary/geometry/AnyGeometry.h>
#include <memory>

class RobotWithGeometry : public RobotDynamics3D
{
public:
  typedef Geometry::AnyCollisionGeometry3D CollisionGeometry;
  typedef Geometry::AnyCollisionQuery CollisionQuery;
  
  RobotWithGeometry();
  RobotWithGeometry(const RobotDynamics3D& rhs);
  RobotWithGeometry(const RobotWithGeometry& rhs);
  virtual ~RobotWithGeometry();

  void Initialize(int numLinks);
  bool LoadGeometry(int i,const char* file);
  bool SaveGeometry(int i,const char* file);
  bool IsGeometryEmpty(int i);
  void InitCollisions();
  void CleanupCollisions();
  void InitAllSelfCollisions();
  void InitSelfCollisionPair(int i,int j);
  void InitSelfCollisionPairs(const Array2D<bool>& collision);
  void GetSelfCollisionPairs(Array2D<bool>& collision) const;
  void CleanupSelfCollisions();

  void Merge(const std::vector<RobotWithGeometry*>& robots);

  const RobotWithGeometry& operator = (const RobotWithGeometry& rhs);
  const RobotWithGeometry& operator = (const RobotDynamics3D& rhs);

  virtual void UpdateGeometry();
  virtual void UpdateGeometry(int i);
  virtual void InitMeshCollision(CollisionGeometry& mesh);

  virtual bool SelfCollision(Real distance=0);
  virtual bool SelfCollision(const std::vector<int>& bodies, Real distance=0);
  virtual bool SelfCollision(const std::vector<int>& set1,const std::vector<int>& set2,Real distance=0);
  virtual bool SelfCollision(int i, int j, Real distance=0); 
  virtual void SelfCollisions(std::vector<std::pair<int,int> >& pairs,Real distance=0);

  virtual bool MeshCollision(CollisionGeometry& mesh);
  virtual bool MeshCollision(int i,Real distance=0);

  virtual void DrawGL();
  virtual void DrawLinkGL(int i);

  std::vector<std::shared_ptr<CollisionGeometry> > geometry;
  Array2D<CollisionQuery*> selfCollisions;
  std::vector<CollisionQuery*> envCollisions;
};

#endif