chart.h

Go to the documentation of this file.

#ifndef CHARTH
#define CHARTH
 
#include "boolean.h"
#include "parameters.h"
#include "cpolytope.h"
#include "scpolytope.h"
#include "jacobian.h"
 
#include "plot3d.h"
 
#include "geom.h"
 
#include "cuiksystem.h"
#include "world.h"
 
#define COMPUTE_LQRPOLICY_IN_T 1
 
#define PLOT_ON_MANIFOLD 0
 
#define PLOT_AS_POLYGONS 1
 
 
#define MIN_SAMPLING_RADIUS 1e-3
 
typedef struct {
  /* The chart information */
  TAtlasBase *w;      
  unsigned int m;     
  unsigned int k;     
  unsigned int da;    
  unsigned int n;     
  unsigned int nrJ;   
  double error;       
  double eCurv;       
  double r;           
  unsigned int degree;
  boolean collision;  
  boolean frontier;   
  boolean singular;   
  double *center;     
  double *T;          
  boolean *BJ;        
  unsigned int ml;    
  unsigned int nl;    
  unsigned int *l;    
  /* Polytope information (can be full or simple) */
  boolean simple;     
  Tcpolytope *p;      
  Tscpolytope *sp;    
  /* Information about the linearized dynamics (optional) */
  double *A;          
  double *B;          
  double *c;          
  double *d;          
  double *iRBt;       
  double *BiRBt;      
} Tchart;
 
unsigned int InitChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
                       unsigned int m,unsigned int k,double *p,double e, double eCurv,double r,
                       TJacobian *sJ,TAtlasBase *w,
                       Tchart *c);
unsigned int InitPossiblySingularChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
                                       unsigned int m,unsigned int k,double *p,double e, double eCurv,double r,
                                       TJacobian *sJ,TAtlasBase *w,
                                       Tchart *c);
 
unsigned int InitSingularChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
                               unsigned int m,unsigned int k,double *p,double e, double eCurv,double r,
                               TJacobian *sJ,TAtlasBase *w,
                               Tchart *c);
 
unsigned int InitTrustedChart(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
                              unsigned int m,unsigned int k,double *p,double e, double eCurv,double r,
                              TJacobian *sJ,TAtlasBase *w,
                              Tchart *c);
 
unsigned int InitChartWithTangent(Tparameters *pr,boolean simple,Tbox *domain,unsigned int *tp,
                                  unsigned int m,unsigned int k,double *p,double *T,
                                  double e, double eCurv, double r,TJacobian *sJ,
                                  TAtlasBase *w,Tchart *c);
 
void SetLinearizedDynamics(unsigned int da,double *mA,double *mB,double *vc,double *iR,Tchart *c);
 
void GetLinearizedDynamics(double **mA,double **mAt,double **mB,double**vc,double **vd,
                           double **iRBt,double **BiRBt,Tchart *c);
 
void CopyChart(Tchart *c_dst,Tchart *c_src);
 
boolean CompareTangentSpaces(Tchart *c1,Tchart *c2);
 
double MinCosinusBetweenCharts(Tchart *c1,Tchart *c2);
 
TAtlasBase *GetChartWorld(Tchart *c);
 
double *GetChartCenter(Tchart *c);
 
double GetChartRadius(Tchart *c);
 
double GetChartSamplingRadius(Tchart *c);
 
double GetChartMaxError(Tchart *c);
 
double GetChartMaxCurvError(Tchart *c);
 
unsigned int GetChartAmbientDim(Tchart *c);
 
unsigned int GetChartManifoldDim(Tchart *c);
 
double *GetChartTangentSpace(Tchart *c);
 
boolean SingularChart(Tchart *c);
 
boolean *GetChartJacobianBasis(Tchart *c);
 
unsigned int GetChartDegree(Tparameters *pr,double *T,TJacobian *sJ,
                            boolean *singular,Tchart *c);
 
double Manifold2Chart(double *p,unsigned int *tp,double *t,Tchart *c);
 
double Chart2Manifold(Tparameters *pr,TJacobian *sJ,
                      double *t,unsigned int *tp,double *pInit,
                      double *p,Tchart *c);
 
 
void Local2Global(double *t,unsigned int *tp,double *p,Tchart *c);
 
double ChartErrorFromParameters(double *t,double *p,
                                unsigned int *tp,Tchart *c);
 
double Error2Chart(double *p,unsigned int *tp,Tchart *c);
 
boolean CloseCharts(Tparameters *pr,unsigned int *tp,
                    Tchart *c1,Tchart *c2);
 
boolean IntersectCharts(Tparameters *pr,unsigned int *tp,Tbox *ambient,
                        unsigned int id1,Tchart *c1,
                        unsigned int id2,Tchart *c2);
 
boolean CollisionChart(Tchart *c);
 
boolean FrontierChart(Tchart *c);
 
void ChartIsFrontier(Tchart *c);
 
boolean ExpandibleChart(Tchart *c);
 
boolean OpenChart(Tchart *c);
 
void WrongCorner(unsigned int nv,Tchart *c);
 
boolean InsideChartPolytope(double *t,Tchart *c);
 
unsigned int DetermineChartNeighbour(double epsilon,double *t,Tchart *c);
 
void EnlargeChart(double *t,Tchart *c);
 
boolean BoundaryPointFromExternalCorner(boolean rand,unsigned int *nv,double *t,Tchart *c);
 
void BoundaryPointsFromExternalCorners(unsigned int *n,unsigned int **nv,double ***t,Tchart *c);
 
boolean RandomPointOnBoundary(double *t,Tchart *c);
 
boolean RandomPointInChart(Tparameters *pr,double scale,
                           unsigned int *tp,double *t,double *p,Tchart *c);
 
void IncreaseChartSamplingRadius(Tchart *c);
 
void DecreaseChartSamplingRadius(Tchart *c);
 
double ChartMaxVolume(Tchart *c);
 
double ChartVolume(Tparameters *pr,boolean collisionFree,
                   unsigned int *tp,TJacobian *sJ,Tchart *c);
 
boolean FocusedPointOnBoundary(double *p,unsigned int *tp,double *t,Tchart *c);
 
 
boolean PathInChart(Tparameters *pr,double *t,unsigned int *tp,TJacobian *sJ,
                    unsigned int nvs,boolean *systemVars,
                    unsigned int *ms,
                    double *pl,unsigned int *ns,double ***path,
                    Tchart *c);
 
double DistanceOnChart(Tparameters *pr,double *t,unsigned int *tp,TJacobian *sJ,
                       Tchart *c);
 
boolean PointOnChart(Tparameters *pr,TJacobian *sJ,
                     double *p,unsigned int *tp,
                     double *t,Tchart *c);
 
unsigned int ClassifyPointInChart(Tparameters *pr,boolean error,TJacobian *sJ,
                                  double *p,unsigned int *tp,
                                  double *t,Tchart *c);
void LinkCharts(unsigned int id1,Tchart *c1,unsigned int id2,Tchart *c2);
 
void AddBorderConstraint(Tparameters *pr,double *t,unsigned int *tp,Tbox *ambient,Tchart *c);
 
void ForceChartCut(Tparameters *pr,unsigned int *tp,Tbox *ambient,
                   unsigned int id1,Tchart *c1,
                   unsigned int id2,Tchart *c2) ;
 
unsigned int ChartNumNeighbours(Tchart *c);
 
unsigned int ChartNeighbourID(unsigned int n,Tchart *c);
 
void GetChartNeighboursFromVertices(Tparameters *pr,
                                    unsigned int *nn,unsigned int **cID1,unsigned int **cID2,Tchart *c);
 
void PlotChart(Tparameters *pr,FILE *fcost,TJacobian *sJ,
               unsigned int xID,unsigned int yID,unsigned int zID,
               Tplot3d *p3d,Tchart *c);
 
unsigned int ChartMemSize(Tchart *c);
 
void SaveChart(FILE *f,Tchart *c);
 
void LoadChart(FILE *f,TAtlasBase *w,Tchart *c);
 
void PrintChartDefines(FILE *f);
 
void DeleteChart(Tchart *c);
 
#endif