chart.c File Reference

Detailed Description

Implementation of a local chart on a point on a manifold.

This basically follows the implementation of Dr. Henderson for MultiFario but adapted to our requirements.

See Also
Tchart,chart.h

Definition in file chart.c.

Functions

unsigned int InitChartInt (boolean trusted, boolean singular, boolean forceRS, Tparameters *pr, boolean simple, Tbox *domain, unsigned int *tp, unsigned int m, unsigned int k, double *p, double e, double eCurv, double r, double *T, TJacobian *sJ, TAtlasBase *w, Tchart *c)
 Constructor. More...
 
unsigned int ComputeJacobianKernelBasis (double epsilon, TJacobian *sJ, Tchart *c)
 Computes the kernel of the Jacobian and, optionally its basis. More...
 
void LinkChart (unsigned int id, Tchart *c)
 Add a map indentifier to the list of linked charts. More...
 
boolean IntersectChartsInt (Tparameters *pr, boolean cut, unsigned int *tp, Tbox *ambient, unsigned int id1, Tchart *c1, unsigned int id2, Tchart *c2)
 Intersects two local charts. More...
 
void PlotChartAsPolygon (Tparameters *pr, FILE *fcost, TJacobian *sJ, unsigned int xID, unsigned int yID, unsigned int zID, Tplot3d *p3d, Tchart *c)
 Plots a 3d projection of a local chart as a filled polygon. More...
 
void PlotChartAsBox (Tparameters *pr, unsigned int xID, unsigned int yID, unsigned int zID, Tplot3d *p3d, Tchart *c)
 Plots a 3d chart as a box. More...
 
void AddBorderConstraint (Tparameters *pr, double *t, unsigned int *tp, Tbox *ambient, Tchart *c)
 Crops the domain for a given chart. More...
 
void ForceChartCut (Tparameters *pr, unsigned int *tp, Tbox *ambient, unsigned int id1, Tchart *c1, unsigned int id2, Tchart *c2)
 Intersect two charts that might be non-neighbours. More...
 
void InitCSWDFromFile (Tparameters *pr, char *name, TAtlasBase *wcs)
 Initializes a world or a CuikSystem structre. More...
 
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)
 Constructor. More...
 
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)
 Constructor. More...
 
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)
 Constructor. More...
 
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)
 Constructor. More...
 
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)
 Constructor. More...
 
void CopyChart (Tchart *c_dst, Tchart *c_src)
 Copy constructor. More...
 
boolean CompareTangentSpaces (Tchart *c1, Tchart *c2)
 Checks if the tangent spaces are similar. More...
 
double MinCosinusBetweenCharts (Tchart *c1, Tchart *c2)
 Computes the angle between the tangent spaces in the charts. More...
 
TAtlasBaseGetChartWorld (Tchart *c)
 Returns the world defining the manifold. More...
 
double * GetChartCenter (Tchart *c)
 Returns the center of the chart. More...
 
double GetChartRadius (Tchart *c)
 Returns de range of the chart. More...
 
double GetChartSamplingRadius (Tchart *c)
 Returns de sampling range of the chart. More...
 
double GetChartMaxError (Tchart *c)
 Returns the maximum error between the chart and the manifold. More...
 
double GetChartMaxCurvError (Tchart *c)
 Returns the maximum oriented curvature error between the chart and the manifold. More...
 
unsigned int GetChartAmbientDim (Tchart *c)
 Dimensionality of the ambient space. More...
 
unsigned int GetChartManifoldDim (Tchart *c)
 Dimensionality of the manifold space. More...
 
double * GetChartTangentSpace (Tchart *c)
 Gets the chart tangent space. More...
 
booleanGetChartJacobianBasis (Tchart *c)
 Gets the index of the basis of the Jacobian vectors forming a basis. More...
 
boolean SingularChart (Tchart *c)
 Identify charts defined on singularities. More...
 
unsigned int GetChartDegree (Tparameters *pr, double *T, TJacobian *sJ, boolean *singular, Tchart *c)
 Returns the chart degree. More...
 
double Manifold2Chart (double *p, unsigned int *tp, double *t, Tchart *c)
 Returns the parametrization of a point. More...
 
double Chart2Manifold (Tparameters *pr, TJacobian *sJ, double *t, unsigned int *tp, double *pInit, double *p, Tchart *c)
 Returns the point in the manifold for a given set of parameteres. More...
 
void Local2Global (double *t, unsigned int *tp, double *p, Tchart *c)
 Transforms a parameter in tangent space to a point in ambient space. More...
 
double ChartErrorFromParameters (double *t, double *p, unsigned int *tp, Tchart *c)
 Identifies points inside the defined error. More...
 
double Error2Chart (double *p, unsigned int *tp, Tchart *c)
 Distance from the manifold to the tangent space. More...
 
boolean CloseCharts (Tparameters *pr, unsigned int *tp, Tchart *c1, Tchart *c2)
 Identifies close local charts. More...
 
boolean IntersectCharts (Tparameters *pr, unsigned int *tp, Tbox *ambient, unsigned int id1, Tchart *c1, unsigned int id2, Tchart *c2)
 Intersects two local charts. More...
 
boolean CollisionChart (Tchart *c)
 Identifies collision charts. More...
 
boolean FrontierChart (Tchart *c)
 Identifies frontier charts. More...
 
void ChartIsFrontier (Tchart *c)
 Marks a chart as a frontier chart. More...
 
boolean ExpandibleChart (Tchart *c)
 Identifies boundary charts. More...
 
boolean OpenChart (Tchart *c)
 Identifies charts not fully sorrounded by other charts. More...
 
void WrongCorner (unsigned int nv, Tchart *c)
 Mark a vertex as wrong. More...
 
boolean InsideChartPolytope (double *t, Tchart *c)
 Checks if a parameter point is inside the chart polytope. More...
 
unsigned int DetermineChartNeighbour (double epsilon, double *t, Tchart *c)
 Determines the neighbouring chart containing a given point. More...
 
void EnlargeChart (double *t, Tchart *c)
 Ensures that a chart includes a given point. More...
 
boolean BoundaryPointFromExternalCorner (boolean rand, unsigned int *nv, double *t, Tchart *c)
 Random point on the chart boundary from the polytope vetices. More...
 
void BoundaryPointsFromExternalCorners (unsigned int *n, unsigned int **nv, double ***t, Tchart *c)
 All the possible points on the chart's boundary from polytope vertices. More...
 
boolean RandomPointOnBoundary (double *t, Tchart *c)
 Random point on the boundary of the chart. More...
 
boolean RandomPointInChart (Tparameters *pr, double scale, unsigned int *tp, double *t, double *p, Tchart *c)
 Samples a random point in the area covered by the chart. More...
 
void IncreaseChartSamplingRadius (Tchart *c)
 Increase the sampling radious of the chart. More...
 
void DecreaseChartSamplingRadius (Tchart *c)
 Decrease the sampling radious of the chart. More...
 
double ChartMaxVolume (Tchart *c)
 Estimate the volume of a chart. More...
 
double ChartVolume (Tparameters *pr, boolean collisionFree, unsigned int *tp, TJacobian *sJ, Tchart *c)
 Estimate the volume of a chart. More...
 
boolean FocusedPointOnBoundary (double *p, unsigned int *tp, double *t, Tchart *c)
 Generates point on the boundary towards a given goal. More...
 
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)
 Defines the path to a point in the chart. More...
 
double DistanceOnChart (Tparameters *pr, double *t, unsigned int *tp, TJacobian *sJ, Tchart *c)
 Distance between the center of a chart and a point on this chart. More...
 
boolean PointOnChart (Tparameters *pr, TJacobian *sJ, double *p, unsigned int *tp, double *t, Tchart *c)
 Identify points on a chart. More...
 
unsigned int ClassifyPointInChart (Tparameters *pr, boolean error, TJacobian *sJ, double *p, unsigned int *tp, double *t, Tchart *c)
 Identifies the position of a point w.r.t. a given chart. More...
 
void LinkCharts (unsigned int id1, Tchart *c1, unsigned int id2, Tchart *c2)
 Connect charts at singularities. More...
 
unsigned int ChartNumNeighbours (Tchart *c)
 Number of neighbours of the chart. More...
 
unsigned int ChartNeighbourID (unsigned int n, Tchart *c)
 Returns the identifier of one of the neighbours of a chart. More...
 
void GetChartNeighboursFromVertices (Tparameters *pr, unsigned int *nn, unsigned int **cID1, unsigned int **cID2, Tchart *c)
 Returns the identifier of neighbouring charts coincident at a vertex. More...
 
void PlotChart (Tparameters *pr, FILE *fcost, TJacobian *sJ, unsigned int xID, unsigned int yID, unsigned int zID, Tplot3d *p3d, Tchart *c)
 Plots a 3d projection of a local chart. More...
 
unsigned int ChartMemSize (Tchart *c)
 Memory used by a given chart. More...
 
void SaveChart (FILE *f, Tchart *c)
 Stores the chart information on a file. More...
 
void LoadChart (FILE *f, TAtlasBase *w, Tchart *c)
 Defines a chart from the information on a file. More...
 
void DeleteChart (Tchart *c)
 Destructor. More...
 

Function Documentation

unsigned int InitChartInt ( boolean  trusted,
boolean  singular,
boolean  forceRS,
Tparameters pr,
boolean  simple,
Tbox domain,
unsigned int *  tp,
unsigned int  m,
unsigned int  k,
double *  p,
double  e,
double  eCurv,
double  r,
double *  T,
TJacobian sJ,
TAtlasBase w,
Tchart c 
)

Shared code between InitChart and InitSingularChart The only difference is that the second does not check if the given point is singular (and generates some structures to link the singular chart with other chart, at a bifurcation).

Parameters
trustedTRUE if the given point is for sure on the manifold and collision free. This is so during the AtlasRRT construction.
singularTRUE if we expect the point to be singular. In this case, the chart is created even if the point is singular, i.e., when the return values are 0 or 1. If singular is FALSE, the chart is only created if the return code is 0. Otherwise no memory is actually allocated (and, thus, no memory need to be realesed).
forceRSIf TRUE raises an error if a point is singular and is not expected to be so (i.e. singular==FALSE). If singular is TRUE we just enforce the chart to be singular even if it is not detected as so due to numerical issues.
prThe set of parameters.
simpleTRUE if we want to use a simple polytope.
domainThe box defining the domain over which the manifold is defined (or of interest).
tpTopology for each variable.
mThe dimension of the ambient space.
kThe dimension of the manifold
pA point on the manifold.
eNaximum distance respect to the manifold.
eCurvMaximum curvature error respecto to the manifold.
rThe radius of validity of the local parametrization.
TThe tangent space. Only provided for some charts defined at a bifurcation. When provided it is not computed. If given, it implies singular=TRUE and forceRS=TRUE.
sJSymbolic Jacobian.
wThe world with the equations and obstacles.
cThe chart to create. Note that no chart is actually created if an error is detected. Any ouput different from 0 means that the chart is not created, except when singular is TRUE where the chart is also created when the return code is 1.
Returns
0 if we could actually define the chart, 1 if there the kernel is too large (i.e., the given point is singular), 2 if the chart could not be defined since the kernel is too small at the given point, 3 if there is an error while performing QR decomposition, and 4 if the error w.r.t. the manifold is too large. Most of these outputs come directly from AnalyzeKernel.

Definition at line 651 of file chart.c.

References Tchart::BJ, Tchart::center, Tchart::collision, ComputeJacobianKernelBasis(), CS_WD_ERROR_IN_SIMP_EQUATIONS, CS_WD_IN_COLLISION, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_EPSILON, CT_SR, Tchart::degree, DeleteChart(), Tchart::eCurv, Tchart::error, Error(), FALSE, Tchart::frontier, GetJacobianSize(), GetParameter(), InitEmptyPolytope(), InitEmptySPolytope(), Tchart::k, Tchart::l, Tchart::m, Tchart::ml, Tchart::n, NEW, Tchart::nl, NO_UINT, Tchart::nrJ, Tchart::p, PointInBoxTopology(), Tchart::r, Tchart::simple, Tchart::singular, Tchart::sp, Tchart::T, TRUE, and Tchart::w.

Referenced by InitChart(), InitChartWithTangent(), InitPossiblySingularChart(), InitSingularChart(), and InitTrustedChart().

unsigned int ComputeJacobianKernelBasis ( double  epsilon,
TJacobian sJ,
Tchart c 
)

Computes the Jacobian kernel. For wide matrices (matrices with more columns than rows, it also computes the Jacobian basis. This function initializes the T field of charts and, optionally, the BJ field too.

For wide matrices we use QR and for tall matrices we use SVD.

This is an auxiliary function of InitChart. To have a separate function is more inefficient but we defined it just to keep the code understandable.

Parameters
epsilonNumerical accuracy.
sJSymbolic Jacobian.
cThe chart with the point where to compute the Jacobian (and the parameters like the dimensionality of the kernel).
Returns
0 if there was no error when computing the kernel. 3 if there is a numerical error in computing the QR of the SVD decomposition, 2 if the kernel is larger than expected, and 1 if it is smaller than expected.

Definition at line 193 of file chart.c.

References Tchart::BJ, Tchart::center, EvaluateTransposedJacobianInVector(), FindKernelAndIndependentRows(), Tchart::k, Tchart::m, NEW, Tchart::nrJ, PrintTMatrix(), PrintVector(), Tchart::singular, and Tchart::T.

Referenced by InitChartInt().

void LinkChart ( unsigned int  id,
Tchart c 
)

Auxiliary function of LinkCharts.

Parameters
idMap identifer to add to the list.
cMap where to add the identifier as a linked chart.

Definition at line 217 of file chart.c.

References Error(), Tchart::l, MEM_DUP, Tchart::ml, Tchart::nl, and Tchart::singular.

Referenced by LinkCharts().

boolean IntersectChartsInt ( Tparameters pr,
boolean  cut,
unsigned int *  tp,
Tbox ambient,
unsigned int  id1,
Tchart c1,
unsigned int  id2,
Tchart c2 
)

This is a function that implements the common functionality for CloseCharts and IntersectCharts.

Parameters
prThe set of parameters (basically to use CT_EPSILON)
cutIf TRUE we implement the IntersectCharts behavior (we do nt only detect if two charts are close but we also cut their polytopes).
tpThe topology for each variable.
ambientThe ambient space.
id1Identifier of the first chart to modify. Not used if cut is FALSE.
c1The first chart to modify.
id2Identifier of the second chart to modify. Not used if cut is FALSE.
c2The second chart to modify.
Returns
TRUE if c1 and c2 are neighbours.

Definition at line 287 of file chart.c.

References Tchart::center, CompareTangentSpaces(), CutPolytope(), CutSPolytope(), DistanceTopology(), Error(), FALSE, Tchart::k, Tchart::m, Manifold2Chart(), Tchart::n, NEW, Tchart::p, Tchart::r, Tchart::simple, Tchart::sp, TRUE, Tchart::w, and ZERO.

Referenced by CloseCharts(), and IntersectCharts().

void PlotChartAsPolygon ( Tparameters pr,
FILE *  fcost,
TJacobian sJ,
unsigned int  xID,
unsigned int  yID,
unsigned int  zID,
Tplot3d p3d,
Tchart c 
)

Plots a 3d projection of a local chart as a filled polygon.

In only works for 2D manifolds.

In 2D the chart area is bounded by a polygon. In PlotChart we plot the edges defining this polygon. In this function we fill the polygon and plot it all and not only its boundaries.

Observe that this function does not take into account the topology of the variables in order to avoid plotting weird charts. When plotting on variables with topology TOPOLOGY_S a cut is introduced at pi and since plots are in generated in R. Charts over pi will produce vertices larger/smaller than pi/-pi. The resulting plot won't be continuous.

Parameters
prThe set of parameters.
fcostFile from where to get the cost associated with this chart. This translates to a different color for the chart in function of the cost. Set to NULL if the chart is to have a uniform color.
sJSymbolic Jacobian. This is only used when ploting the edges after projecting them on the manifold (to check the overlap between them).
xIDFirst dimension for the projection (in the original system including system vars and dummies).
yIDSecond dimension for the projection (in the original system including system vars and dummies).
zIDThird dimension for the projection (in the original system including system vars and dummies).
p3dThe 3d plot where to add the
cThe chart to plot.

Definition at line 402 of file chart.c.

References Chart2Manifold(), CostColor(), CS_WD_REGENERATE_ORIGINAL_POINT, CT_CUT_X, CT_CUT_Y, CT_CUT_Z, CT_REPRESENTATION, DeletePolytope(), Error(), FALSE, Tchart::frontier, GetParameter(), GetPolytopeEdges(), GetPolytopeVertices(), INF, Tchart::k, Local2Global(), Tchart::m, M_2PI, NEW, Tchart::p, Plot3dObject(), Plot3dObjectWithColor(), PLOT_ON_MANIFOLD, REP_JOINTS, Tchart::simple, Tchart::sp, SPolytope2Polytope(), TRUE, Tchart::w, and Warning().

Referenced by PlotChart().

void PlotChartAsBox ( Tparameters pr,
unsigned int  xID,
unsigned int  yID,
unsigned int  zID,
Tplot3d p3d,
Tchart c 
)

Plots a 3d chart as a box, i.e. the cube defining the maximum applicability area for the chart.

Parameters
prThe set of parameters.
xIDFirst dimension for the projection (in the original system including system vars and dummies).
yIDSecond dimension for the projection (in the original system including system vars and dummies).
zIDThird dimension for the projection (in the original system including system vars and dummies).
p3dThe 3d plot where to add the
cThe chart to plot.

Definition at line 599 of file chart.c.

References Tchart::center, CS_WD_REGENERATE_ORIGINAL_POINT, Error(), Tchart::k, PlotBox3d(), Tchart::r, and Tchart::w.

Referenced by PlotChart().

void AddBorderConstraint ( Tparameters pr,
double *  t,
unsigned int *  tp,
Tbox ambient,
Tchart c 
)

Crops the domain of a given chart with a constraint derived form the global domain. This is used for charts that are at the border of the global domain.

Parameters
prThe set of parameters.
tParameters of the point that are just outside the domain.
tpThe topology of the ambient space.
ambientThe ambient space box.
cThe chart to modify.

Definition at line 229 of file chart.c.

References CutPolytopeWithFace(), CutSPolytopeWithFace(), GeneralDotProduct(), Tchart::k, Tchart::m, NEW, NO_UINT, Norm(), Tchart::p, ScaleVector(), Tchart::simple, Tchart::sp, and Tchart::w.

Referenced by ExtendAtlasFromPoint().

void ForceChartCut ( Tparameters pr,
unsigned int *  tp,
Tbox ambient,
unsigned int  id1,
Tchart c1,
unsigned int  id2,
Tchart c2 
)

Intersects two charts without checking first if they are actually neighbours. This is used in the AtlasRRT when using loose controls aboud curvature. In this case we can end up generating a chart that is not neighbour with its parent (but it must be almost neighbour). In this situation we enforce the neighbouring relation between them.

Parameters
prThe set of parameters.
tpThe topology of the ambient space.
ambientThe ambient space.
id1The identifier of the first chart.
c1The first chart.
id2The identifier of the second chart.
c2The second chart.

Definition at line 265 of file chart.c.

References Tchart::center, CutPolytope(), CutSPolytope(), Tchart::k, Tchart::m, Manifold2Chart(), NEW, Tchart::p, Tchart::r, Tchart::simple, Tchart::sp, and Tchart::w.

Referenced by AddChart2AtlasRRT().

void InitCSWDFromFile ( Tparameters pr,
char *  name,
TAtlasBase wcs 
)

Initializes the equations from a file. First we try to read the equations from cuik file. If this fails, we try to read them from a world file. If not possible, we trigger and error.

The cuik files are typically derived from the world ones (using cuikequations). So if the cuik file has been generated, then it probably means you want to work with it. Otherwise just delete (or rename) it and we will read the world file instead.

We have two main set of equation. One (the original system) is the one as given in the given file. The other (the simplified system) is the same after introducing trivial simplifications (removing variables with constant values, replacing variables by equivalent ones,...). Typically we operate with the simplified system but input and output points are given in the original system. Therefore we have to translate between them when necessary. But this is out of the scope of this function.

See Also
InitCuikSystemFromFile, InitWorldFromFile

Definition at line 618 of file chart.c.

References CreateFileName(), TAtlasBase::cs, CUIK_EXT, DeleteFileName(), FALSE, GetFileFullName(), InitCuikSystemFromFile(), InitWorldFromFile(), TAtlasBase::isCS, NEW, TRUE, TAtlasBase::w, and WORLD_EXT.

Referenced by main().

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 
)

Defines a new chart that locally parametrizes a manifold. This chart is supposed to be on a regular point. Otherwise an error is triggered.

We do not extract m and k from the number of variables/equations in the world since our systems are overconstrained (we include redundant equations).

Parameters
prThe set of parameters.
simpleTRUE if we want to use a simple polytope.
domainThe box defining the domain over which the manifold is defined (or of interest).
tpTopology for each variable.
mThe dimension of the ambient space.
kThe dimension of the manifold
pA point on the manifold.
eNaximum distance respect to the manifold.
eCurvMaximum curvature error respecto to the manifold.
rThe radius of validity of the local parametrization.
sJSymbolic Jacobian.
wThe world with the equations and obstacles.
cThe chart to create.The chart is actually created (and must be deleted by the caller) when the return code is 0.
Returns
0 if we could actually define the chart, 1 if there the kernel is too large (i.e., the given point is singular), 2 if the chart could not be defined since the kernel is too small at the given point, 3 if there is an error while performing QR decomposition, and 4 if the error w.r.t. the manifold is too large.

Definition at line 792 of file chart.c.

References FALSE, InitChartInt(), and TRUE.

Referenced by AddChart2Atlas(), ConnectSamplesChart(), ExtendAtlasFromPoint(), ExtendAtlasTowardPoint(), GradientSmooth(), InitAtlasFromPoint(), main(), NewChartFromPoint(), and NewTemptativeSample().

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 
)

The same as InitChart but for charts possibly defined from singular points. No error is triggered if the point is not regular and charts are not forced to be singular.

Parameters
prThe set of parameters.
simpleTRUE if we want to use a simple polytope.
domainThe box defining the domain over which the manifold is defined (or of interest).
tpTopology for each variable.
mThe dimension of the ambient space.
kThe dimension of the manifold
pA point on the manifold.
eNaximum distance respect to the manifold.
eCurvMaximum curvature error respecto to the manifold.
rThe radius of validity of the local parametrization.
sJSymbolic Jacobian.
wThe world with the equations and obstacles.
cThe chart to create. The chart is actually created (and must be deleted by the caller) when the return code is 0 or 1.
Returns
0 if we could actually define the chart, 1 if there the kernel is too large (i.e., the given point is singular), 2 if the chart could not be defined since the kernel is too small at the given point, 3 if there is an error while performing QR decomposition, and 4 if the error w.r.t. the manifold is too large.

Definition at line 800 of file chart.c.

References FALSE, InitChartInt(), and TRUE.

Referenced by FindSingularPoint().

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 
)

The same as InitChart but for charts defined from singular points. Charts as marked as singular even if numerically they are determined to be regular.

Parameters
prThe set of parameters.
simpleTRUE if we want to use a simple polytope.
domainThe box defining the domain over which the manifold is defined (or of interest).
tpTopology for each variable.
mThe dimension of the ambient space.
kThe dimension of the manifold
pA point on the manifold.
eNaximum distance respect to the manifold.
eCurvMaximum curvature error respecto to the manifold.
rThe radius of validity of the local parametrization.
sJSymbolic Jacobian.
wThe world with the equations and obstacles.
cThe chart to create. The chart is actually created (and must be deleted by the caller) when the return code is 0 or 1.
Returns
0 if we could actually define the chart, 1 if there the kernel is too large (i.e., the given point is singular), 2 if the chart could not be defined since the kernel is too small at the given point, 3 if there is an error while performing QR decomposition, and 4 if the error w.r.t. the manifold is too large.

Definition at line 808 of file chart.c.

References FALSE, InitChartInt(), and TRUE.

Referenced by DefineChartsAtBifurcation().

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 
)

The same as InitChart but for points that are for sure on the manifold and collision free.

Parameters
prThe set of parameters.
simpleTRUE if we want to use a simple polytope.
domainThe box defining the domain over which the manifold is defined (or of interest).
tpTopology for each variable.
mThe dimension of the ambient space.
kThe dimension of the manifold
pA point on the manifold.
eNaximum distance respect to the manifold.
eCurvMaximum curvature error respecto to the manifold.
rThe radius of validity of the local parametrization.
sJSymbolic Jacobian.
wThe world with the equations and obstacles.
cThe chart to create. The chart is actually created (and must be deleted by the caller) when the return code is 0 or 1.
Returns
0 if we could actually define the chart, 1 if there the kernel is too large (i.e., the given point is singular), 2 if the chart could not be defined since the kernel is too small at the given point, 3 if there is an error while performing QR decomposition, and 4 if the error w.r.t. the manifold is too large.

Definition at line 816 of file chart.c.

References InitChartInt(), and TRUE.

Referenced by AddTrustedChart2Atlas().

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 
)

Defines a new chart that locally parametrizes a manifold.

This is like InitChart but here the tangent space is given instead of computed from the Jacobian. This is used during branch switching where the tangent space is computed externally.

Parameters
prThe set of parameters.
simpleTRUE if we want to use a simple polytope.
domainThe box defining the domain over which the manifold is defined (or of interest).
tpTopology for each variable.
mThe dimension of the ambient space.
kThe dimension of the manifold
pA point on the manifold.
TThe tangent space at this point.
eEpsilon.
eCurvmaximum curvature error
rThe radius of validity of the local parametrization.
sJSymbolic Jacobian.
wThe world with the equations and obstacles.
cThe chart to create. The chart is actually created (and must be deleted by the caller) when the return code is 0 or 1.
Returns
0 if we could actually define the chart, 1 if there the kernel is too large (i.e., the given point is singular), 2 if the chart could not be defined since the kernel is too small at the given point, 3 if there is an error while performing QR decomposition, and 4 if the error w.r.t. the manifold is too large.

Definition at line 824 of file chart.c.

References FALSE, InitChartInt(), and TRUE.

Referenced by DefineChartsAtBifurcation().

void CopyChart ( Tchart c_dst,
Tchart c_src 
)

Copies one chart into another.

Parameters
c_dstThe chart to define.
c_srcThe chart from where to copy.

Definition at line 833 of file chart.c.

References Tchart::BJ, Tchart::center, CopyPolytope(), CopySPolytope(), Tchart::degree, Tchart::eCurv, Tchart::error, Tchart::frontier, Tchart::k, Tchart::l, Tchart::m, Tchart::ml, Tchart::n, NEW, Tchart::nl, Tchart::nrJ, Tchart::p, Tchart::r, Tchart::simple, Tchart::singular, Tchart::sp, Tchart::T, and Tchart::w.

Referenced by main().

boolean CompareTangentSpaces ( Tchart c1,
Tchart c2 
)

Computes the norm of the product of the tangent spaces on two charts. If they are similar, the norm of the resulting matrix must be below epsilon (the error).

The comparison between tangent spaces is based on measuring the principal angles between the sub-spaces. Intuitively, similar sub-spaces have angles between them close to 0 or to pi.

Parameters
c1The first chart to compare.
c2The second chart to compare.
Returns
TRUE if norm(mp1->T^{trans}*mp2->T-I)<=epsilon

Definition at line 901 of file chart.c.

References Tchart::eCurv, Error(), Tchart::k, Tchart::m, MatrixDeterminant(), NEW, Tchart::T, TMatrixMatrixProduct(), and Tchart::w.

Referenced by FindSingularPoint(), and IntersectChartsInt().

double MinCosinusBetweenCharts ( Tchart c1,
Tchart c2 
)

This is a wrapper for MinCosinusBetweenSubSpaces that can be used with charts.

Parameters
c1The first chart.
c2The second chart.
Returns
The cosinus of the maximum angle (i.e., the minimum cosinus) between the tangent spaces of the input charts.

Definition at line 920 of file chart.c.

References Error(), Tchart::k, Tchart::m, MinCosinusBetweenSubSpaces(), Tchart::T, and Tchart::w.

Referenced by DefineChartsAtBifurcation(), and FindSingularPoint().

TAtlasBase* GetChartWorld ( Tchart c)

Returns a pointer to the world structure with the system of equations defining the manifold.

This is used to determine is two charts are defined on the same manifold (i.e., the same world).

Parameters
cThe chart to query
Returns
Pointer to the world on which the chart is defined.

Definition at line 928 of file chart.c.

References Tchart::w.

double* GetChartCenter ( Tchart c)

Returns a pointer to the point from which the local chart is defined

Parameters
cThe chart to query
Returns
The center of the chart.

Definition at line 933 of file chart.c.

References Tchart::center.

Referenced by AddChart2Btree(), AtlasAStar(), AtlasGBF(), BuildAtlasFromPoint(), ExtendAtlasTowardPoint(), FindSingularPoint(), GeodesicDistance(), InitBTree(), MinimizeOnAtlas(), PlotAtlas(), PlotBifurcations(), PlotQrand(), ReconstructAtlasPath(), RefineSingularPoint(), SaveChartCenters(), SaveSingularCharts(), SearchInBtree(), and TriangulateAtlas().

double GetChartRadius ( Tchart c)

Returns the radious of influence for the chart.

Parameters
cThe chart to query.
Returns
The radius of the chart.

Definition at line 938 of file chart.c.

References Tchart::r.

Referenced by ExtendAtlasTowardPoint(), and InitBTree().

double GetChartSamplingRadius ( Tchart c)

Returns the adjusted radious of influence for the chart.

Right now there is no difference between the radius and the sampling radious. This can change in the future.

Parameters
cThe chart to query.
Returns
The sampling radius of the chart.

Definition at line 943 of file chart.c.

References Error(), Tchart::simple, Tchart::sp, and SPolytopeGetSamplingRadius().

Referenced by PrintAtlasRRTStatistics(), and RandomPointInAtlasTree().

double GetChartMaxError ( Tchart c)

Returns the maximum error between the chart and the manifold.

Parameters
cThe chart to query
Returns
The maximum error between the chart and the manifold.

Definition at line 950 of file chart.c.

References Tchart::error.

double GetChartMaxCurvError ( Tchart c)

Returns the maximum oriented curvature error between the chart and the manifold.

Parameters
cThe chart to query
Returns
The maximum oriented curvature error between the chart and the manifold.

Definition at line 955 of file chart.c.

References Tchart::eCurv.

unsigned int GetChartAmbientDim ( Tchart c)

Returns the dimensionality of the ambient space where the manifold is defined

Parameters
cThe chart to query.
Returns
m (see Tchart)

Definition at line 960 of file chart.c.

References Tchart::m.

Referenced by InitBTree().

unsigned int GetChartManifoldDim ( Tchart c)

Returns the dimensionality of the manifold.

Parameters
cThe chart to query.
Returns
k (see Tchart)

Definition at line 965 of file chart.c.

References Tchart::k.

double* GetChartTangentSpace ( Tchart c)

Returns a pointer to the matrix defining a basis of the chart tangent space. This matrix is m x k.

Parameters
cThe chart to query.
Returns
The pointer to the matrix.

Definition at line 970 of file chart.c.

References Tchart::T.

Referenced by DefineChartsAtBifurcation(), DetectBifurcation(), FindRightNullVector(), FindSingularPoint(), GradientSmooth(), and RefineSingularPoint().

boolean* GetChartJacobianBasis ( Tchart c)

Returns a pointer to an array of booleans indicating the linearly independent vectors of the Jacobian. This can be NULL if the Jacobian basis is not yet computed.

Parameters
cThe chart to query.
Returns
The pointer to array of boolean flags.

Definition at line 975 of file chart.c.

References Tchart::BJ.

boolean SingularChart ( Tchart c)

Returns TRUE for charts defined on singularities (i.e., points where the Jacobian is not full rang).

Parameters
cThem ap to query.
Returns
TRUE if the chart is defined on a singularity.

Definition at line 980 of file chart.c.

References Tchart::singular.

Referenced by DetectBifurcation(), ExtendAtlasFromPoint(), ExtendAtlasTowardPoint(), FindSingularPoint(), PlotAtlas(), and SaveSingularCharts().

unsigned int GetChartDegree ( Tparameters pr,
double *  T,
TJacobian sJ,
boolean singular,
Tchart c 
)

The degree the modulo 2 of the number of non-negative real eigenvalues (i.e., possitive, or complex) of the squared matrix formed by the Jacobian in the top rows and the transposed tangent space (given in the function call) in the bottom rows.

The degree of a chart is the degree for the center of this chart (i.e., the Jacobian is evaluated at the center of the chart).

The degree is used to detect bifurcations (a change in the degree between two points on the manifold indicates that the line connecting the points crosses a bifurcation.

Note that for this procedure to work effectively, we have to use the same tangent space to complement the Jacobian of the two points to be tested. That is, we use the tangent space of one of the points to evaluate the degrees of the two points. That is, the degree is not something intrinsec of a chart that can be computed and stored forever but depends on couples of charts (that use the same tangent space). When computing the degree for a pair of charts (a,b) we use the tangent space of a for the two evaluations. We use the cached degree for a and compute that of b from scratch using the tangent space of a. If we evaluate the possible existance of a bifurcation in between (b,a) we re-use the cached degree in b and we recompute that for a using the tangent space of b.

Parameters
prThe set of parameters.
TThe tangent space to complement the Jacobian basis. If this is the same as the tangent space in the chart, the result is cached in the chart for future uses.
sJSymbolic Jacobian.
singularIs set to TRUE if we realize that the chart is defined on an almost singular point. In some cases (when very close to the singularity but not exacty on it) this could not be detected during chart initilization but is detected here.
cThe chart to query.
Returns
The degree for the center of the chart.

Definition at line 985 of file chart.c.

References Tchart::BJ, Tchart::center, CT_EPSILON, Tchart::degree, Error(), EvaluateJacobianSubSetInVector(), FALSE, GetParameter(), Tchart::k, Tchart::m, MatrixDeterminantSgn(), Tchart::n, NEW, NO_UINT, PrintMatrix(), PrintVector(), Tchart::singular, SubMatrixFromTMatrix(), Tchart::T, and TRUE.

Referenced by DetectBifurcation(), and FindSingularPoint().

double Manifold2Chart ( double *  p,
unsigned int *  tp,
double *  t,
Tchart c 
)

Returns the parametrization on the tangent space of a point on the manifold.

In principle any point can be projected to the tangent space, even those given parameters out of the radius of the chart.

This corresponds to the logarithmic map in differential geometry.

In some cases we have to take into account the topology of the ambient space and in others not. When trying to determine which charts are neighbours, we take it into account so that we obtain the closest projection of a point to the current chart center. When defining RRT branches on the tangnent space the topology is not considered so that we define the correct advance direction towards the random/goal sample.

We directly project the points to the tangent space, without transforming the point to project into canonical form (i.e,. move it in the range -pi,pi for the dimensions with circular topology). It is the user option to transform the point to project before actually projecting it.

Observe that transforming a point before projecting it can actually generate a set of parameters that do not give the shortest displacement to the projected sample. This is critical when defining RRT-like structures using the charts.

Parameters
pThe point to parametrize.
tpThe topology of the ambient space.
tThe output set of parameters.
cThe chart to use to get the parametrization.
Returns
The norm of the parameter vector t (i.e., how far is t from the center of the chart).

Definition at line 1036 of file chart.c.

References ArrayPi2Pi(), Tchart::center, DifferenceVector(), DifferenceVectorTopology(), Tchart::k, Tchart::m, Tchart::n, NEW, Norm(), Tchart::T, and TMatrixVectorProduct().

Referenced by AddBranchToAtlasRRT(), AddChart2AtlasRRT(), AddSample2AtlasRRT(), ClassifyPointInChart(), ConnectSamplesChart(), Error2Chart(), FindSingularPoint(), FocusedPointOnBoundary(), ForceChartCut(), IntersectChartsInt(), main(), NewTemptativeSample(), PointOnChart(), PointTowardRandSample(), PopulateWithSamples(), ReconstructAtlasPath(), and SaveChartCenters().

double Chart2Manifold ( Tparameters pr,
TJacobian sJ,
double *  t,
unsigned int *  tp,
double *  pInit,
double *  p,
Tchart c 
)

Returns a point in the mainifold corresponding to a set of parameters in a given chart.

Note that not all parameters can be savely mapped to the manifold. The implicit function theorem guarantees the mapping to be valid for a ball around the linealization point. In principle this ball should reach the singularity locus possibly sourronding the central point. However, this mapping is implemented using a numerical procedure than can fail to converge even far from the singularity frontier.

This corresponds to the exponential map in differential geometry.

In many cases, this function not applies the pi2pi mapping since the output points can be used with many purposes (not only creating new charts). Thus, this function can produce points larger/smaller than pi/-pi in circular dimensions. The trick here is:

  • When creating a new chart, enforce the [-pi,pi] range.
  • Detect that points around the border +/-pi are neighbours (the kd-tree takes care of this). This avoids an atlas to expand repetitively over a circular range.

To skip the pi2pi mapping just call the function with tp set to NULL.

Parameters
prThe set of parameters.
sJSymbolic Jacobian.
tThe set of parameters.
tpThe topology of the variables. NULL if all are TOPOLOGY_R.
pInitInitial estimation for the expected output p. Set to NULL to start the process from the the point on the tangent space given by t.
pThe output point on the manifold.
cThe mpa to use.
Returns
If the process converges, the distance between the point on the manifold and the tangent space, if not INF.

Definition at line 1066 of file chart.c.

References ArrayPi2Pi(), Tchart::BJ, Tchart::center, CS_WD_EVALUATE_SIMP_EQUATIONS, CS_WD_EVALUATE_SUBSET_SIMP_EQUATIONS, CT_EPSILON, CT_MAX_NEWTON_ITERATIONS, DeleteLS(), DifferenceVectorTopology(), DistanceTopology(), Error(), EvaluateJacobianInVector(), EvaluateJacobianSubSetInVector(), FALSE, GetLSMatrixBuffer(), GetLSRHBuffer(), GetLSSolutionBuffer(), GetParameter(), INF, InitLS(), Tchart::k, Local2Global(), LSSolve(), Tchart::m, Tchart::n, NEW, Norm(), Tchart::nrJ, PI2PI, SubMatrixFromTMatrix(), SumVector(), Tchart::T, TMatrixVectorProduct(), TOPOLOGY_S, TRUE, and Tchart::w.

Referenced by ChartVolume(), ClassifyPointInChart(), ConnectSamplesChart(), DistanceOnChart(), ExtendAtlasFromPoint(), ExtendAtlasTowardPoint(), FindSingularPoint(), GradientSmooth(), main(), MinimizeOnAtlas(), NewChartFromPoint(), NewTemptativeSample(), PathInChart(), PlotChart(), PlotChartAsPolygon(), PointOnChart(), and SaveChartCenters().

void Local2Global ( double *  t,
unsigned int *  tp,
double *  p,
Tchart c 
)

Uses the center and the base of the tangent space to generate a point in ambient space from a parameter set in tangent space.

Parameters
tThe parameters in tangent space.
tpTopology of the ambient space.
pThe output point in ambient space.
cThe chart to use

Definition at line 1212 of file chart.c.

References AccumulateVector(), ArrayPi2Pi(), Tchart::center, Tchart::k, Tchart::m, MatrixVectorProduct(), Tchart::n, SumVector(), and Tchart::T.

Referenced by Chart2Manifold(), ChartErrorFromParameters(), CutPolytopeWithFace(), PlotAtlasRRT(), PlotChart(), PlotChartAsPolygon(), PlotQrand(), PointTowardRandSample(), and RandomPointInChart().

double ChartErrorFromParameters ( double *  t,
double *  p,
unsigned int *  tp,
Tchart c 
)

Transforms p to the ambient space (via Local2Global) and compute the distance to p.

The point p and the paramters t are supposed to correspond each other (via the Chart2Manifold and Manifold2Chart mappings).

Parameters
tpThe topology. One entry per variable: TOPOLOGY_R or TOPOLOGY_S.
tThe parameters of the point to compare.
pThe second point to compare.
cTha chart to query.
Returns
The norm of Local2Global(t)-p.
See Also
Local2Global, ChartError.

Definition at line 1229 of file chart.c.

References DistanceTopology(), Local2Global(), Tchart::m, Tchart::n, and NEW.

Referenced by Error2Chart().

double Error2Chart ( double *  p,
unsigned int *  tp,
Tchart c 
)

Basically combines Manifold2Chart with ChartErrorFromParameters.

Parameters
tpThe topology. One entry per variable: TOPOLOGY_R or TOPOLOGY_S.
pThe point in the manifold to project and compare.
tpThe topology of the variables.
cTha chart to query.
Returns
The distance of p to the tangent space.
See Also
Manifold2Chart, ChartErrorFromParameters.

Definition at line 1247 of file chart.c.

References ChartErrorFromParameters(), Tchart::k, Manifold2Chart(), Tchart::n, and NEW.

boolean CloseCharts ( Tparameters pr,
unsigned int *  tp,
Tchart c1,
Tchart c2 
)
inline

Determines if two local charts are close and have similar tangent spaces.

Parameters
prThe set of parameters (basically to use CT_EPSILON).
tpThe topology for each variable.
c1The first chart to modify.
c2The second chart to modify.
Returns
TRUE if mp1 and mp2 are neighbours.

Definition at line 1264 of file chart.c.

References FALSE, IntersectChartsInt(), and NO_UINT.

Referenced by ExtendAtlasFromPoint(), ExtendAtlasTowardPoint(), NewChartFromPoint(), and NewTemptativeSample().

boolean IntersectCharts ( Tparameters pr,
unsigned int *  tp,
Tbox ambient,
unsigned int  id1,
Tchart c1,
unsigned int  id2,
Tchart c2 
)

Determines if two chart are neighbours and intersect the corresponding polytopes.

Parameters
prThe set of parameters (basically to use CT_EPSILON).
tpThe topology for each variable.
ambientThe ambient space.
id1Identifier of the first chart to modify.
c1The first chart to modify.
id2Identifier of the second chart to modify.
c2The second chart to modify.
Returns
TRUE if mp1 and mp2 are neighbours.

Definition at line 1270 of file chart.c.

References IntersectChartsInt(), and TRUE.

Referenced by DetermineChartNeighbours().

boolean CollisionChart ( Tchart c)
inline

Identifies charts that are in collision

Parameters
cThe chart to query.
Returns
TRUE for colliding charts.

Definition at line 1277 of file chart.c.

References Tchart::collision.

Referenced by ExtendAtlasFromPoint(), ExtendAtlasTowardPoint(), and NewChartFromPoint().

boolean FrontierChart ( Tchart c)
inline

Identifies charts that are at the borders of the domain.

Parameters
cThe chart to query.
Returns
TRUE for boundary charts.

Definition at line 1282 of file chart.c.

References Tchart::frontier.

Referenced by AtlasVolume(), InitAtlasFromPoint(), PlotAtlas(), and SaveChartCenters().

void ChartIsFrontier ( Tchart c)
inline

Marks a chart as a frontier chart. Frontier charts are note expanded.

Parameters
cThe chart to modify.

Definition at line 1287 of file chart.c.

References Tchart::frontier, and TRUE.

Referenced by ExtendAtlasFromPoint(), and NewChartFromPoint().

boolean ExpandibleChart ( Tchart c)
inline

TRUE for external (frontier) charts, i.e., charts that can be used to expand the atlas.

If the chart is based on simple polytope this function will trigger an error since simple polytopes can not identify open charts.

Parameters
cThe chart to query.
Returns
TRUE for external (frontier) charts.

Definition at line 1292 of file chart.c.

References Error(), ExpandiblePolytope(), Tchart::frontier, Tchart::p, and Tchart::simple.

Referenced by AtlasAStar(), AtlasGBF(), FocusedPointOnBoundary(), and PlotAtlas().

boolean OpenChart ( Tchart c)
inline

TRUE for charts with polytope vertices still outside the ball. Those vertices can still be used to generate neighbours for the chart.

If the chart is based on simple polytope this function will return TRUE always since for simple polytopes it is not possible to determine if a chart is closed or not.

This is equivalent to ExpandibleChart but this function does not trigger error for charts based on simple polytopes.

Parameters
cThe chart to query.
Returns
TRUE for charts with vertices out of the ball.

Definition at line 1300 of file chart.c.

References ExpandiblePolytope(), Tchart::p, and Tchart::simple.

Referenced by BuildAtlasFromPoint().

void WrongCorner ( unsigned int  nv,
Tchart c 
)

Wrapper for WrongPolytopeCorner.

Parameters
nvThe vertex to mark
cThe chart.

Definition at line 1305 of file chart.c.

References Error(), Tchart::p, Tchart::simple, and WrongPolytopeCorner().

Referenced by AtlasAStar(), and BuildAtlasFromPoint().

boolean InsideChartPolytope ( double *  t,
Tchart c 
)

Wrapper for InsidePolytope (or InsideSPolytope).

Parameters
tThe parameters.
cThe chart.
Returns
TRUE if the point is inside the polytope defining the area of influence of the chart.

Definition at line 1313 of file chart.c.

References InsidePolytope(), InsideSPolytope(), Tchart::p, Tchart::simple, and Tchart::sp.

Referenced by ClassifyPointInChart(), NewTemptativeSample(), and PointOnChart().

unsigned int DetermineChartNeighbour ( double  epsilon,
double *  t,
Tchart c 
)

This function determines the neighbouring chart that includes a point inside the chart radius but outside the polytope. This is implemented determining the chart borders crossed when moving to the point.

Parameters
epsilonNumerical accuracy.
tThe point.
cThe chart.

Definition at line 1321 of file chart.c.

References DetermineSPolytopeNeighbour(), Error(), NO_UINT, Tchart::simple, and Tchart::sp.

Referenced by NewTemptativeSample(), and PopulateWithSamples().

void EnlargeChart ( double *  t,
Tchart c 
)

Ensures that a chart includes a given point possibly modifying the faces enlarging the polytope.

Note that this function is not implemented for normal polytpes since moving a face will represent to recompute all vertices and this is too expensive. Thus, this function is only used from AtlasRRT where charts are defined with simple polytopes.

Parameters
tThe point.
cThe chart.

Definition at line 1332 of file chart.c.

References EnlargeSPolytope(), Error(), Tchart::simple, and Tchart::sp.

Referenced by AddChart2AtlasRRT(), NewTemptativeSample(), and PopulateWithSamples().

boolean BoundaryPointFromExternalCorner ( boolean  rand,
unsigned int *  nv,
double *  t,
Tchart c 
)

Wapper for PolytopeBoundaryPointFromExternalCorner.

Parameters
randTRUE if the external corner has to be selected at random from all external ones of FALSE if we have to just pick the first one.
nvThe identifier of the selected vertex. This is set to NO_UINT if no boundary point can be actually returned.
tThe output point on the chart ball boundary (if any)
cThe chart to sample.
Returns
TRUE if the sampled point is actually on the boundary of the atlas.

Definition at line 1340 of file chart.c.

References Error(), Tchart::p, PolytopeBoundaryPointFromExternalCorner(), Tchart::r, and Tchart::simple.

Referenced by AtlasAStar(), and BuildAtlasFromPoint().

void BoundaryPointsFromExternalCorners ( unsigned int *  n,
unsigned int **  nv,
double ***  t,
Tchart c 
)

This is the same as BoundaryPointFromExternalCorner but instead of selecting one vertex returns points for all the possible vertices.

This is used when parallelizing the generation of children charts from a given chart.

Parameters
nThe number of returned points.
nvArray with the vertex identifiers. The space is allocated internally, but must be deallocated by the caller. This is set to NO_UINT if no boundary point can be actually returned.
tArray with the returned points. Each point is of size c->k. The space is allocated internally, but must be deallocated by the caller.
cThe chart to query.

Definition at line 1348 of file chart.c.

References Error(), Tchart::p, PolytopeBoundaryPointsFromExternalCorners(), Tchart::r, and Tchart::simple.

boolean RandomPointOnBoundary ( double *  t,
Tchart c 
)

Wrapper for PolytopeRandomPointOnBoundary.

Parameters
tThe output point on the chart ball boundary (if any)
cThe chart to sample.
Returns
TRUE if the sampled point is actually on the boundary of the chart.

Definition at line 1356 of file chart.c.

References Tchart::p, PolytopeRandomPointOnBoundary(), Tchart::r, Tchart::simple, Tchart::sp, and SPolytopeRandomPointOnBoundary().

Referenced by AtlasGBF().

boolean RandomPointInChart ( Tparameters pr,
double  scale,
unsigned int *  tp,
double *  t,
double *  p,
Tchart c 
)

Selects a point on a chart with uniform distribution.

Points are given in the tangent space associated with the chart and not on the manifold. However, given the chart manifold, a point on the tangent is supposed to give a point on the manifold.

This is basically a wrapper RandomPointInPolytope plus a conversion of local points to global.

Parameters
prThe set of parameters.
scaleGlobal scale factor for the sampling area for each chart. This is only used simple charts. The default value is 1. See RandomPointInSPolytope.
tpTopology of the ambient space.
tParameters (in the chart) for the output point.
pThe output point.
cThe chart to sample.
Returns
TRUE if we could actually sample a point in the chart.

Definition at line 1364 of file chart.c.

References Local2Global(), Tchart::p, RandomPointInPolytope(), RandomPointInSPolytope(), Tchart::simple, and Tchart::sp.

Referenced by RandomPointInAtlas(), and RandomPointInAtlasTree().

void IncreaseChartSamplingRadius ( Tchart c)

Valid sampling on a chart leads to an increment of the sampling area.

Parameters
cThe chart whose sampling radius we have to adjust.

Definition at line 1379 of file chart.c.

References Tchart::simple, Tchart::sp, and SPolytopeIncreaseSamplingRadius().

Referenced by AtlasRRT(), and AtlasRRTSample().

void DecreaseChartSamplingRadius ( Tchart c)

Rejection sampling on a chart leads to a reduction of the sampling area.

Parameters
cThe chart whose sampling radius we have to adjust.

Definition at line 1385 of file chart.c.

References Tchart::simple, Tchart::sp, and SPolytopeDecreaseSamplingRadius().

Referenced by AtlasRRT(), and AtlasRRTSample().

double ChartMaxVolume ( Tchart c)

This is a simplified version of ChartVolume where collisions are not taken into account. Thus this returns an upper bound approximation of the collision free volume of the chart.

This is used to fix some thresholds in RRT*.

Parameters
cThe chart to measure.
Returns
The chart volume.

Definition at line 1391 of file chart.c.

References Tchart::p, PolytopeVolume(), Tchart::simple, Tchart::sp, and SPolytopeVolume().

Referenced by ChartVolume().

double ChartVolume ( Tparameters pr,
boolean  collisionFree,
unsigned int *  tp,
TJacobian sJ,
Tchart c 
)

Estimate the volume of part of the manifold covered by a chart. The volume is estimated via MonteCarlo on the tangent space.

Note that this function replicates PolytopeVolume and SPolytopeVolume but including the collision detection, which is not computable at the level of polytopes.

Parameters
prThe set of parameters. Only used if collisionFree is TRUE.
collisionFreeTRUE if only the collision free volume has to be computed.
tpThe topology for each variable. Only used if collisionFree is TRUE.
sJThe simbolic Jacobian. Only used if collisionFree is TRUE.
cThe chart to measure.
Returns
The chart volume.

Definition at line 1406 of file chart.c.

References Chart2Manifold(), ChartMaxVolume(), CS_WD_IN_COLLISION, INF, Tchart::k, Tchart::m, NEW, Tchart::p, PolytopeMaxVolume(), RandomPointInPolytope(), RandomPointInSPolytope(), Tchart::simple, Tchart::sp, SPolytopeMaxVolume(), and Tchart::w.

Referenced by AtlasVolume().

boolean FocusedPointOnBoundary ( double *  p,
unsigned int *  tp,
double *  t,
Tchart c 
)

Tries to generate a point on the chart boundary that is aligned with a given point on the manifold.

Parameters
pThe goal point (on the manifold, with ambient dimension).
tpThe ambient space topology.
tThe output point on the chart ball boundary (if any)
cThe chart to sample.
Returns
TRUE if the sampled point is actually on the boundary of the atlas.

Definition at line 1458 of file chart.c.

References Error(), ExpandibleChart(), FALSE, InsidePolytope(), Tchart::k, Manifold2Chart(), Tchart::p, Tchart::r, ScaleVector(), and Tchart::simple.

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 
)

Defines the path to a point in the chart. The samples added to the path are those resulting from using the chart projection moving at steps of size delta towards the given set of parameters. The final point is also added to the path.

This is used when reconstructing the path from the start point to the goal and the segment of path on this chart is concatenated to the path already found.

Parameters
prThe set of parameters.
tThe parameters for the point.
tpThe topology of the variables.
sJSymbolic Jacobian.
nvsNumber of system variables.
systemVarsFlags indicating the sytem varibles. Only those are added to the path.
msMaximum number of samples in the path.
plAccumulated path lenght.
nsCurrent number of samples in the path.
pathThe path where to add the samples. Samples found on this chart are added at the end of this path.
cThe chart.
Returns
TRUE if there is no collision along the path (i.e., if the determined path is actually valid).

Definition at line 1477 of file chart.c.

References AddSample2Samples(), Tchart::center, Chart2Manifold(), CS_WD_ORIGINAL_IN_COLLISION, CS_WD_REGENERATE_ORIGINAL_POINT, CT_DELTA, Distance(), DistanceTopology(), Tchart::error, FALSE, GetParameter(), Tchart::k, Tchart::m, NEW, Norm(), ScaleVector(), TRUE, and Tchart::w.

Referenced by ReconstructAtlasPath().

double DistanceOnChart ( Tparameters pr,
double *  t,
unsigned int *  tp,
TJacobian sJ,
Tchart c 
)

Approximates the geodesic distance between the center of a chart and a point on this chart. This is like GeodesicDistance but this function only operates on points on the same chart.

If the path is not collision free, this function returns INF.

Parameters
prThe set of parameters.
tThe parameters for the point.
tpThe topology of the variables.
sJSymbolic Jacobian.
cThe chart.

Definition at line 1546 of file chart.c.

References Tchart::center, Chart2Manifold(), CS_WD_IN_COLLISION, CT_DELTA, Distance(), DistanceTopology(), Tchart::error, FALSE, GetParameter(), INF, Tchart::k, Tchart::m, NEW, Norm(), ScaleVector(), TRUE, and Tchart::w.

Referenced by AtlasAStar(), and AtlasGBF().

boolean PointOnChart ( Tparameters pr,
TJacobian sJ,
double *  p,
unsigned int *  tp,
double *  t,
Tchart c 
)

Identifies points that are inside the area of influence of a given chart.

Parameters
prThe set of parameters (to get the numerical tolerance).
sJSymbolic Jacobian.
pThe point.
tpThe topology of the variables. NULL if all are TOPOLOGY_R.
tThe parameters of the point on the chart. Only holds the correct value if the point is actually on this chart.
cThe chart that should include the point.
Returns
TRUE if the point belongs to the chart.

Definition at line 1602 of file chart.c.

References Chart2Manifold(), CT_EPSILON, DistanceTopology(), Tchart::error, FALSE, GetParameter(), InsideChartPolytope(), Tchart::m, Manifold2Chart(), NEW, and Tchart::r.

Referenced by AtlasAStar(), and AtlasGBF().

unsigned int ClassifyPointInChart ( Tparameters pr,
boolean  error,
TJacobian sJ,
double *  p,
unsigned int *  tp,
double *  t,
Tchart c 
)

Computes the relative position of a point with respect to a given chart. The point can be

  • 4: The point is completely out of the scope of the chart.
  • 3: The point is outside the scope due to convergence problems.
  • 2: The point is outside the scope, but inside the ball. This means that the point must be inside the scope of one of the neighbouring charts.
  • 1: The point is inside the scope of the chart, but outside the ball.
  • 0: The point is inside the scope of the chart.

Codes 0 and 1 correspont to points inside the scope of the chart, although in different circumstances. In the first case the point wil for sure move to a neighbouring chart as new charts are created and in the second this is less likely to occur (but not impossible).

Parameters
prThe set of parameters (to get the numerical tolerance).
errorTRUE if the error between the chart and the manifold for the given point should be also considered. If this is set to FALSE this function never returns 3.
sJSymbolic Jacobian.
pThe point.
tpThe topology of the variables. NULL if all are TOPOLOGY_R.
tThe parameters of the point on the chart. Only holds the correct value if the point is actually on this chart.
cThe chart that should include the point.
Returns
A code (see above) with the position of the point with respect to the chart.

Definition at line 1629 of file chart.c.

References Chart2Manifold(), CT_EPSILON, DistanceTopology(), Tchart::error, GetParameter(), InsideChartPolytope(), Tchart::m, Manifold2Chart(), NEW, and Tchart::r.

void LinkCharts ( unsigned int  id1,
Tchart c1,
unsigned int  id2,
Tchart c2 
)

At bifurcation points we generate two charts, one at the singular point with tangent space aligned with the charts that allowed to detect the bifurcation and a chart at the other branch of the manifold. These two charts overlap because they are almost defined at the same point but with different tangent spaces. Since they have different tangent spaces they will not be considered neighbours by IntersectCharts and they have to be linked explicitly using this function.

Explicitly linked chats also appear in the list of neighbours as retrieved using ChartNumNeighbours or ChartNeighbourID.

Parameters
id1Identifier of the first chart to link.
c1The first charts to link.
id2Identifier of the second chart to link.
c2The second chart to link.

Definition at line 1669 of file chart.c.

References LinkChart().

Referenced by DefineChartsAtBifurcation().

unsigned int ChartNumNeighbours ( Tchart c)

Returns the number of neighbouring charts.

Note that for charts at the border of the ambient space, this returns and overestimation of the number of neighbours: it includes "virtual" neighbours generated by cropping the chart with the ambient space borders. These virtual neighbours have NO_UINT as identifier.

Parameters
cThe chart to query.
Returns
The number of neighbours of the chart.

Definition at line 1675 of file chart.c.

References Tchart::nl, Tchart::p, PolytopeNumNeighbours(), Tchart::simple, Tchart::sp, and SPolytopeNumNeighbours().

Referenced by AddChart2AtlasRRT(), AtlasAStar(), DetermineChartNeighbours(), GetRRTNNInNeighbourChart(), PlotAtlas(), PopulateWithSamples(), and SaveChartCenters().

unsigned int ChartNeighbourID ( unsigned int  n,
Tchart c 
)

Gives the identifer for one of the neighbours of a chart. For the virtual neighbours eventually generated due to the intersection of the chart with the ambient space domain, NO_UINT is returned. The caller must take care of not using output of this function in these special cases.

Parameters
nThe number of neighbour to query (numbered from 0).
cThe chart to query.
Returns
The neighbour identifer. NO_UINT if i is larger thatn the actual number of neighbours of the chart.

Definition at line 1687 of file chart.c.

References Tchart::l, Tchart::nl, Tchart::p, PolytopeNeighbourID(), Tchart::simple, Tchart::sp, and SPolytopeNeighbourID().

Referenced by AddChart2AtlasRRT(), AtlasAStar(), GetRRTNNInNeighbourChart(), PlotAtlas(), PopulateWithSamples(), and SaveChartCenters().

void GetChartNeighboursFromVertices ( Tparameters pr,
unsigned int *  nn,
unsigned int **  cID1,
unsigned int **  cID2,
Tchart c 
)

When triangulating an atlas we want to define a triangle for each vertex of the atlas. Thus, for each vertex, we identify the three coincident charts so that we can connect their centers forming a triangle. The charts are the one defining the polytope whose vertex we are analyzing plust the two charts defining the edges coincident at that vertex.

This only operates for bidimendional manifolds and does not return the neighbours through singularities.

This is only used when triangulating atlas.

Parameters
prThe set of parameters.
nnNumber of neighbours (output).
cID1For each vertex, the identifier of the chart neighbouring by one of the two edges defining this vertex. The space for this array is allocated internally (output).
cID2For each vertex, the identifier of the chart neighbouring by other edge defining this vertex. The space for this array is allocated internally (output).
cThe chart to query.

Definition at line 1704 of file chart.c.

References DeletePolytope(), GetPolytopeNeighboursFromVertices(), NEW, Tchart::p, Tchart::simple, Tchart::sp, and SPolytope2Polytope().

Referenced by TriangulateAtlas().

void PlotChart ( Tparameters pr,
FILE *  fcost,
TJacobian sJ,
unsigned int  xID,
unsigned int  yID,
unsigned int  zID,
Tplot3d p3d,
Tchart c 
)

Plots a 3d projection of a local chart.

In only works for 2D or 3D manifolds.

We plot the edges delimiting the chart.

Observe that this function does not take into account the topology of the variables in order to avoid plotting weird charts. When plotting on variables with topology TOPOLOGY_S a cut is introduced at pi and since plots are in generated in R. Charts over pi will produce vertices larger/smaller than pi/-pi. The resulting plot won't be continuous.

Todo:
Speed up this function caching the vertex projection on the manifold instead as recomputing them as many times as needed.
Parameters
prThe set of parameters.
fcostFile from where to get the cost associated with this chart. This translates to a different color for the chart in function of the cost. Set to NULL if the chart is to have a uniform color. This is only used if the chart is plotted as a polygon, otherwise it makes no sense to color the chart.
sJSymbolic Jacobian. This is only used when ploting the edges after projecting them on the manifold (to check the overlap between them).
xIDFirst dimension for the projection (in the original system including system vars and dummies).
yIDSecond dimension for the projection (in the original system including system vars and dummies).
zIDThird dimension for the projection (in the original system including system vars and dummies).
p3dThe 3d plot where to add the
cThe chart to plot.

Definition at line 1726 of file chart.c.

References Chart2Manifold(), CS_WD_REGENERATE_ORIGINAL_POINT, CT_CUT_X, CT_CUT_Y, CT_CUT_Z, DeletePolytope(), Error(), GetParameter(), GetPolytopeEdges(), GetPolytopeVertices(), INF, Tchart::k, Local2Global(), Tchart::m, M_2PI, NEW, Tchart::p, PLOT_AS_POLYGONS, PLOT_ON_MANIFOLD, PlotChartAsBox(), PlotChartAsPolygon(), PlotVect3d(), Tchart::simple, Tchart::sp, SPolytope2Polytope(), and Tchart::w.

Referenced by PlotAtlas().

unsigned int ChartMemSize ( Tchart c)

Returns the approximated memory used (in bytes) by a given chart.

Parameters
cThe chart.
Returns
The size of the chart in bytes.

Definition at line 1862 of file chart.c.

References Tchart::k, Tchart::m, Tchart::p, PolytopeMemSize(), Tchart::simple, Tchart::sp, and SPolytopeMemSize().

Referenced by AtlasMemSize().

void SaveChart ( FILE *  f,
Tchart c 
)

Stores the chart information on a file.

Parameters
fThe file where to store the information.
cThe chart to store.

Definition at line 1877 of file chart.c.

References Tchart::BJ, Tchart::center, Tchart::degree, Tchart::eCurv, Tchart::error, Tchart::frontier, Tchart::k, Tchart::l, Tchart::m, Tchart::ml, Tchart::n, Tchart::nl, Tchart::nrJ, Tchart::p, Tchart::r, SavePolytope(), SaveSPolytope(), Tchart::simple, Tchart::singular, Tchart::sp, and Tchart::T.

Referenced by SaveAtlas().

void LoadChart ( FILE *  f,
TAtlasBase w,
Tchart c 
)

Chart constructor from the information on a file. The information is to be generated with SaveChart.

Parameters
fThe file from where to read the data.
wThe world to which the chart is refereed.
cThe chart to define.

Definition at line 1932 of file chart.c.

References Tchart::BJ, Tchart::center, Tchart::degree, Tchart::eCurv, Tchart::error, Tchart::frontier, Tchart::k, Tchart::l, LoadPolytope(), LoadSPolytope(), Tchart::m, Tchart::ml, Tchart::n, NEW, Tchart::nl, Tchart::nrJ, Tchart::p, Tchart::r, Tchart::simple, Tchart::singular, Tchart::sp, Tchart::T, and Tchart::w.

Referenced by LoadAtlas().

void DeleteChart ( Tchart c)