/home/slemaign/softs-local/BioMove3D-git/lightPlanner/proto/lightPlannerApi.h

#ifndef LIGHTPLANNERAPI_H
#define LIGHTPLANNERAPI_H
#include "Planner-pkg.h"
#include "P3d-pkg.h"

class p3d_objectPos {
  
public:
  p3d_objectPos() { };
  p3d_objectPos(p3d_matrix4 m) 
  {
    p3d_mat4ExtractPosReverseOrder(m, &_x, &_y, &_z, &_rx, &_ry, &_rz);
  };
  
  void setFromMatrix(p3d_matrix4 m)
  {
    p3d_mat4ExtractPosReverseOrder(m, &_x, &_y, &_z, &_rx, &_ry, &_rz);
  };
  
  void getMatrix(p3d_matrix4 m)
  {
    p3d_mat4PosReverseOrder( m, _x, _y, _z,
                              _rx, _ry, _rz);
  };  
  
  double _x;
  double _y; 
  double _z; 
  double _rx; 
  double _ry; 
  double _rz; 
};


extern void deactivateCcCntrts(p3d_rob * robot, int cntrtNum);
extern void activateCcCntrts(p3d_rob * robot, int cntrtNum, bool nonUsedCntrtDesactivation);
extern void switchBBActivationForGrasp(void);
extern void setSafetyDistance(p3d_rob* robot, double dist);
extern void getObjectBaseAttachMatrix(p3d_matrix4 base, p3d_matrix4 object, p3d_matrix4 result);
extern void deactivateHandsVsObjectCol(p3d_rob* robot);
extern void activateHandsVsObjectCol(p3d_rob* robot);
extern void deactivateObjectCol(p3d_rob* robot);
extern void activateObjectCol(p3d_rob* robot);
extern double** saveJointSamplingState(p3d_rob* robot);
extern void restoreJointSamplingState(p3d_rob* robot, double** jointSamplingState);
extern void destroyJointSamplingState(p3d_rob* robot, double** jointSamplingState);
extern void fixAllJointsWithoutArm(p3d_rob* robot, int armId);
extern void fixAllJointsExceptBase(p3d_rob * robot);
extern void fixAllJointsExceptBaseAndObject(p3d_rob * robot, configPt conf);
extern void unFixAllJointsExceptBaseAndObject(p3d_rob * robot);
extern void fixJoint(p3d_rob * robot, p3d_jnt * joint, p3d_matrix4 initPos);
extern void unFixJoint(p3d_rob * robot, p3d_jnt * joint);
extern double* getJntDofValue(p3d_rob * robot, p3d_jnt * joint, p3d_matrix4 initPos);
extern p3d_cntrt* setAndActivateTwoJointsFixCntrt(p3d_rob * robot, p3d_jnt* passiveJnt, p3d_jnt* activeJnt);
extern void desactivateTwoJointsFixCntrt(p3d_rob * robot, p3d_jnt* passiveJnt, p3d_jnt* activeJnt);
extern void shootTheObjectInTheWorld(p3d_rob* robot, p3d_jnt* objectJnt);
extern void shootTheObjectArroundTheBase(p3d_rob* robot, p3d_jnt* baseJnt, p3d_jnt* objectJnt, double radius);
extern p3d_cntrt * findTwoJointsFixCntrt(p3d_rob* robot, p3d_jnt* passiveJnt, p3d_jnt* activeJnt);
extern int getGraspingArm(p3d_rob* robot, bool cartesian);
extern int getClosestWristToTheObject(p3d_rob* robot);
extern int getClosestWristToTheObject(p3d_rob* robot, p3d_rob* object);
extern int getClosestWristToTheObject(p3d_rob* robot, p3d_matrix4 objectPos);
#ifdef GRASP_PLANNING
#include "GraspPlanning-pkg.h"
extern int getBetterCollisionFreeGraspAndApproach(p3d_rob* robot, p3d_matrix4 objectPos, gpHand_type handType, p3d_matrix4 tAtt, configPt* graspConfig, configPt* approachConfig, gpGrasp * grasp);
extern int selectHandAndGetGraspApproachConfigs(p3d_rob* robot, p3d_matrix4 objectPos, p3d_matrix4 tAtt, configPt* graspConfig, configPt* approachConfig, gpGrasp* grasp, int* whichArm, bool cartesian);
#endif

#endif