JointStructure.h

#ifndef ROBOTICS_JOINT_STRUCTURE
#define ROBOTICS_JOINT_STRUCTURE

#include "RobotKinematics3D.h"
#include "IK.h"
#include "WorkspaceBound.h"
using namespace std;


struct JointStructure
{
  JointStructure(const RobotKinematics3D& robot);

  void Init();
  bool IsFeasible() const;
  void SolveWorkspaceBounds(const IKGoal& constraint);
  void IntersectWorkspaceBounds(const IKGoal& constraint);
  void SolveWorkspaceBounds(const vector<IKGoal>& constraints);
  void SolveWorkspaceBounds(int link0);
  void IntersectWorkspaceBounds(int link0);
  void SolveRootBounds();
  void IntersectRootBounds();

  void SolveBoundsIter(int link,int parent);
  void GetLinkWorkspaceBounds(vector<Real>& bounds) const;

  void Expand(int n,int p);
  Real GetJointDist(int link1, int link2) const;

  static void SetInitialBounds(WorkspaceBound& b,const IKGoal& c,const Vector3& plocal);

  const RobotKinematics3D& robot;
  vector<vector<int> > children;
  vector<vector<Vector3> > linkPoints; 

  vector<WorkspaceBound> bounds;    
  vector<vector<WorkspaceBound> > pointBounds; 
  //temp
  vector<bool> reached;
};

#endif