AnalyticIK.h

#ifndef ANALYTICAL_IK_SOLVERS_H
#define ANALYTICAL_IK_SOLVERS_H

#include "RobotKinematics3D.h"
#include "IK.h"
#include <memory>
#include <vector>
#include <map>


struct AnalyticIKSolution
{
  AnalyticIKSolution();
  void DeleteMinimalDistance();

  std::vector<Vector> solutions;   
  std::vector<bool> minimalDistance;  
  bool infinite;            
};

class AnalyticalIKSolver
{
 public:
  AnalyticalIKSolver() {}
  ~AnalyticalIKSolver() {}
  virtual bool Solve(int base,int ee,
                     const RigidTransform& T,
                     AnalyticIKSolution& solution) const
  { return false; }
};

class AnalyticIKMap
{
 public:
  void GetIndices(const RobotKinematics3D& robot,
                  const IKGoal& goal,
                  std::vector<int>& indices) const;
  bool Solve(RobotKinematics3D& robot,
             const IKGoal& goal,
             AnalyticIKSolution& solution) const;
  bool Solve(int base,int ee,
             const RigidTransform& T,
             AnalyticIKSolution& solution) const;

  void UpdateRobot(int base,int ee,
                   const AnalyticIKSolution& solution,
                   int index,RobotKinematics3D& robot) const;

  std::map<std::pair<int,int>,std::shared_ptr<AnalyticalIKSolver> > solvers;
};

#endif