atlasrrt.h

Go to the documentation of this file.

#ifndef ATLASRRTH
#define ATLASRRTH

#include "atlas.h"
#include "rrt.h"

#define NEW_BRANCH_EXTENSION 1

#define ATLASRRT_VERBOSE 0

#define GET_ATLASRRT_STATISTICS 1

#define ADJUST_SA 0

#define ATLASRRT_GLOBAL_NN 1

# define GET_ATLASRRT_GLOBAL_CURV_CHECK 0

typedef struct {
  unsigned int n;                      
  unsigned int nBranch;                
  unsigned int nNoEmptyBranch;         
  unsigned int nTreeConnection;        
  unsigned int nNoEmptyTreeConnection; 
  unsigned int nStep;                  
  double dQrand;                       
  /* Reasons to stop a branch extension */
  unsigned int nQrandReached;          
  unsigned int nNotInDomain;           
  unsigned int nCollision;             
  unsigned int nTooLong;               
  unsigned int nTooFar;                
  unsigned int nOverlap;               
  /* Some of the reasons to create a new chart */  
  unsigned int nErrorNewChart;         
  unsigned int nNoConvergent;          
  unsigned int nOutOfChart;            
  unsigned int nLargeCurvature;        
  unsigned int nDirLargeCurvature ;    
  /* Refinements necessary in branch creation */
  unsigned int nStepReduction;         
  unsigned int nSample;                
  unsigned int nChart;                 
  unsigned int nNoConnect;             
  unsigned int nSingular;              
  /* Information about sampling */
  unsigned int nRandom;                
  unsigned int nRejections;            
  /* Information aobut collison checking */
  unsigned int nCollisionChecks;       
} TAtlasRRTStatistics;

typedef struct {
  unsigned int id;            
  double *s;                  
  double *t;                  
  unsigned int pc;            
  unsigned int c;             
  unsigned int generateChart; 
  unsigned int lsc;           
} TSampleInfo;

typedef struct {
  unsigned int lc;      
  unsigned int tree;    
  unsigned int it;      
} TChartInfo;

typedef struct {  
  TAtlasBase *w;         
  unsigned int m;        
  unsigned int k;        
  double e;              
  double ce;             
  double r;              
  boolean birrt;         
  boolean parallel;      
  Tbox ambient;          
  unsigned int *tp;      
  TJacobian J;           
  Trrt rrt;              
  Tatlas atlas;          
  unsigned int ms;       
  unsigned int ns;       
  TSampleInfo **si;      
  unsigned int mc;       
  unsigned int nc;       
  TChartInfo **ci;       
  unsigned int mct1;            
  unsigned int nct1;            
  unsigned int* chartsAtTree1;  
  unsigned int mct2;            
  unsigned int nct2;            
  unsigned int* chartsAtTree2;  
} Tatlasrrt;

/*******************************************************************************/

void InitAtlasRRTStatistics(TAtlasRRTStatistics *arst);

void AccumulateAtlasRRTStatistics(TAtlasRRTStatistics *arst1,TAtlasRRTStatistics *arst2); 

void PrintAtlasRRTStatistics(Tatlasrrt *ar,TAtlasRRTStatistics *arst);

void DeleteAtlasRRTStatistics(TAtlasRRTStatistics *arst);

/*******************************************************************************/


void InitAtlasRRT(Tparameters *pr,boolean parallel,double *ps,unsigned int mode,boolean graph,
                  double *pg,TAtlasBase *w,Tatlasrrt *ar);

boolean AtlasRRTSample(Tparameters *pr,unsigned int samplingMode,
                       unsigned int it,unsigned int tree,
                       double *goal,double scale,boolean *exploration,
                       unsigned int *c_rand,double *t_rand,double *q_rand,
                       TAtlasRRTStatistics *arst,Tatlasrrt *ar);

boolean AtlasRRTValidateSample(Tparameters *pr,double *q_rand,unsigned int tree,boolean expand2goal,
                               unsigned int lastNN2Goal,double *goal,double l,double *h,unsigned int *i_near,
                               TAtlasRRTStatistics *arst,Tatlasrrt *ar);

boolean AtlasRRT(Tparameters *pr,double *pg, 
                 double *time,
                 double *pl,unsigned int *ns,double ***path,
                 TAtlasRRTStatistics *str,Tatlasrrt *ar);


boolean AtlasTRRT(Tparameters *pr,double *pg,double *time,
                  double *pl,double *pc,unsigned int *ns,double ***path, 
                  double (*costF)(Tparameters*,boolean,double*,void*),
                  void *costData,TAtlasRRTStatistics *str,Tatlasrrt *ar);

boolean AtlasRRTstar(Tparameters *pr,double *pg,
                     unsigned int *it,double *times,double *costs,
                     double *planningTime,double *pl,unsigned int *ns,double ***path,
                     TAtlasRRTStatistics *str,Tatlasrrt *ar);

boolean AtlasBiRRTstar(Tparameters *pr,double *pg,
                       unsigned int *it,double *times,double *costs,
                       double *planningTime,double *pl,unsigned int *ns,double ***path,
                       TAtlasRRTStatistics *str,Tatlasrrt *ar);

void PlotAtlasRRT(char *prefix,int argc, char **arg,Tparameters *pr,
                  unsigned int xID,unsigned int yID,unsigned int zID,Tatlasrrt *ar);

unsigned int GetAtlasRRTNumNodes(Tatlasrrt *ar);

unsigned int GetAtlasRRTNumCharts(Tatlasrrt *ar);


boolean RandomPointInAtlasTree(Tparameters *pr,
                               double scale,unsigned int tree,
                               unsigned int *nm,
                               double *t,double *p,
                               Tatlasrrt* ar);

unsigned int GetRRTNNInNeighbourChart(unsigned int tree,
                                      unsigned int c_rand,double *t_rand,double *q_rand, 
                                      Tatlasrrt* ar);


unsigned int GetRRTNNInChart(unsigned int tree,unsigned int chartId,double *q_rand,
                             double t,double*d, Tatlasrrt* ar );

void PlotAtlasRRT(char *prefix,int argc, char **arg,Tparameters *pr,
                  unsigned int xID,unsigned int yID,unsigned int zID,Tatlasrrt *ar);

unsigned int AtlasRRTMemSize(Tatlasrrt *ar);

void SaveAtlasRRT(Tparameters *pr,char *prefix,Tatlasrrt *ar);

void LoadAtlasRRT(Tparameters *pr,char *prefix,TAtlasBase *w,Tatlasrrt *ar);

void DeleteAtlasRRT(Tatlasrrt *ar);


#endif