Implementation of the auxiliary functions to deal with sets of samples.
 See Also
 samples.h
Definition in file samples.c.

int  cmpUInt (const void *a, const void *b) 
 Compares two unsigned in integers. More...


double  PathLength (unsigned int *tp, unsigned int m, unsigned int np, double **point) 
 Length of a path formed by a set of samples. More...


void  ShortcutSmooth (Tparameters *pr, Tstatistics *stime, unsigned int *np, double ***point, TAtlasBase *w) 
 Fixedcut based smoothing. More...


void  RandomSmooth (Tparameters *pr, unsigned int nCores, unsigned int maxIterations, Tstatistics *stime, unsigned int *np, double ***point, TAtlasBase *w) 
 Randombased path smoothing. More...


void  GradientSmooth (Tparameters *pr, unsigned int nCores, unsigned int maxIterations, Tstatistics *stime, unsigned int *np, double ***point, TAtlasBase *w) 
 Gradientbased path smoothing. More...


void  SaveSamplesInt (Tfilename *fpath, unsigned int nvs, unsigned int ns, double **path) 
 Internal function to save a set of samples to a file. More...


void  InitSamples (unsigned int *ms, unsigned int *ns, double ***path) 
 Initializes a set of samples. More...


void  AddSample2Samples (unsigned int nv, double *sample, unsigned int nvs, boolean *systemVars, unsigned int *ms, unsigned int *ns, double ***path) 
 Adds a sample to a set of samples. More...


double  ConnectSamplesChart (Tparameters *pr, unsigned int *tp, Tbox *domain, unsigned int m, unsigned int n, double *s1, double *s2, double md, boolean checkCollisions, boolean *reached, boolean *collision, double *lastSample, unsigned int *ns, double ***path, TAtlasBase *w) 
 Determines the connection between two points on the manifold. More...


double  ConnectSamples (Tparameters *pr, unsigned int *tp, Tbox *domain, unsigned int m, unsigned int n, double *s1, double *s2, double md, boolean checkCollisions, boolean *reached, boolean *collision, double *lastSample, unsigned int *ns, double ***path, TAtlasBase *w) 
 Determines the connection between two points on the manifold. More...


void  SmoothSamples (Tparameters *pr, boolean parallel, int mode, unsigned int maxIterations, unsigned int ns, double **path, unsigned int *sns, double ***spath, TAtlasBase *w) 
 Path smoothing. More...


void  ConcatSamples (unsigned int nvs, unsigned int ns1, double **path1, unsigned int ns2, double **path2, unsigned int *ns, double ***path) 
 Concats two path. More...


void  ReverseConcatSamples (unsigned int nvs, unsigned int ns1, double **path1, unsigned int ns2, double **path2, unsigned int *ns, double ***path) 
 Reverses and concats a path. More...


void  ReverseSamples (unsigned int ns, double **path) 
 Reverses a set of samples. More...


unsigned int  ReadOneSample (Tparameters *p, char *fname, unsigned int nvs, double **s) 
 Reads one sample from a file. More...


unsigned int  ReadTwoSamples (Tparameters *p, char *fname, unsigned int nvs, double **s1, double **s2) 
 Reads two samples from a file. More...


void  SaveSamples (char *fname, boolean smooth, unsigned int nvs, unsigned int ns, double **path) 
 Saves a set of samples to a file. More...


void  SaveSamplesN (char *fname, boolean smooth, unsigned int n, unsigned int nvs, unsigned int ns, double **path) 
 Saves a set of samples to a file. More...


boolean  LoadSamples (Tfilename fname, unsigned int *nvs, unsigned int *ns, double ***path) 
 Reads a set of samples from file. More...


void  PlotSamples (Tparameters *p, Tplot3d *p3d, unsigned int xID, unsigned int yID, unsigned int zID, unsigned int ns, double **path) 
 Plots a 3D projection of a path. More...


void  DeleteSamples (unsigned int ns, double **path) 
 Deletes the space used by a set of samples. More...


int cmpUInt 
( 
const void * 
a, 


const void * 
b 

) 
 
Auxiliary function used for qsort inside RandomSmooth when we exploit parallelism to speed up the smoothing.
 Parameters

a  Pointer to the first unsigned integer. 
b  Pointer to the second unsigned integer. 
 Returns
 1 if a<b, 0 if a==b, or 1 if a>b.
Definition at line 574 of file samples.c.
Referenced by RandomSmooth().
double PathLength 
( 
unsigned int * 
tp, 


unsigned int 
m, 


unsigned int 
np, 


double ** 
point 

) 
 
Computes the length of a path defined by a set of points. IMPORTANT: this is for internal use only and the points are defined in the simplified system and not in the original system as most of the functions on samples.
 Parameters

tp  The topology of the variables. 
m  The size of each point. 
np  Number of points in the path. 
point  The set of points. 
 Returns
 The path length.
Definition at line 561 of file samples.c.
References DistanceTopology().
Referenced by GradientSmooth(), RandomSmooth(), ShortcutSmooth(), and SmoothSamples().
This is like the RandomSmooth but the tentative shortcuts are selected systematically: from the first sample as far as possible and so on.
 Parameters

pr  The set of parameters. 
stime  Statistics initilized at the beginning of the smoothing process. 
np  Number of points defining the path. 
point  The points defining the path. 
w  The world/cuiksystem on which the sample is defined. 
Definition at line 590 of file samples.c.
References ConnectSamplesChart(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_INIT_CD, CT_N_DOF, DeleteBox(), FALSE, GetElapsedTime(), GetParameter(), PathLength(), and TRUE.
Referenced by SmoothSamples().
void RandomSmooth 
( 
Tparameters * 
pr, 


unsigned int 
nCores, 


unsigned int 
maxIterations, 


Tstatistics * 
stime, 


unsigned int * 
np, 


double *** 
point, 


TAtlasBase * 
w 

) 
 
Tries to reduce the lenght of a path by ramdomly selecting points on the path and verifying if a direct collision free path between them exists. If so, the new path replaces a segment of the previous path.
IMPORTANT: this is a subprocess of SmoothSamples and it is intended for internal use only and the points are defined in the simplified system and not in the original system as most of the functions on samples.
 Parameters

pr  The set of parameters. 
nCores  Number of computing cores available. 
maxIterations  Maximum number of iterations. To be scaled by the number of steps in the path. 
stime  Statistics initilized at the beginning of the smoothing process. 
np  Number of points defining the path. 
point  The points defining the path. 
w  The world/cuiksystem on which the sample is defined. 
Definition at line 688 of file samples.c.
References cmpUInt(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_INIT_CD, CT_N_DOF, DEFAULT_CONNECT, DeleteBox(), DeleteSamples(), FALSE, GetElapsedTime(), GetParameter(), NEW, PathLength(), randomMax(), and TRUE.
Referenced by SmoothSamples().
void GradientSmooth 
( 
Tparameters * 
pr, 


unsigned int 
nCores, 


unsigned int 
maxIterations, 


Tstatistics * 
stime, 


unsigned int * 
np, 


double *** 
point, 


TAtlasBase * 
w 

) 
 
Reduces the length of a path by applying a movement to the points forming the path according the the gradient of the path lenght for w.r.t. the parameters of this points.
IMPORTANT: this is a subprocess of SmoothSamples and it is intended for internal use only and the points are defined in the simplified system and not in the original system as most of the functions on samples.
 Parameters

pr  The set of parameters. 
nCores  Number of computing cores available. Up to now this is not used. 
maxIterations  Maximum number of iterations. To be scaled by the number of steps in the path. 
stime  Statistics initilized at the beginning of the smoothing process. 
np  Number of points defining the path. 
point  The points defining the path. 
w  The world/cuiksystem on which the sample is defined. 
 Todo:
 Implement a parallel version of the GradientSmooth function.
Definition at line 909 of file samples.c.
References Chart2Manifold(), CS_WD_GENERATE_SIMP_INITIAL_BOX, CS_WD_GET_SIMP_JACOBIAN, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_IN_COLLISION, CS_WD_INIT_CD, CT_CE, CT_DELTA, CT_E, CT_N_DOF, CT_R, DeleteBox(), DeleteChart(), DeleteJacobian(), DistanceTopology(), FALSE, GetChartTangentSpace(), GetElapsedTime(), GetJacobianSize(), GetParameter(), InitChart(), NEW, Norm(), PathLength(), PointInBoxTopology(), ScaleVector(), SubtractVector(), TMatrixVectorProduct(), and TRUE.
Referenced by SmoothSamples().
void SaveSamplesInt 
( 
Tfilename * 
fpath, 


unsigned int 
nvs, 


unsigned int 
ns, 


double ** 
path 

) 
 
void InitSamples 
( 
unsigned int * 
ms, 


unsigned int * 
ns, 


double *** 
path 

) 
 
void AddSample2Samples 
( 
unsigned int 
nv, 


double * 
sample, 


unsigned int 
nvs, 


boolean * 
systemVars, 


unsigned int * 
ms, 


unsigned int * 
ns, 


double *** 
path 

) 
 
Adds a sample to a set of samples. The sample is given in full form (including all variables) but we only store the system variables.
 Parameters

nv  Number of variables. 
sample  The sample to add. 
nvs  Number of system variables. 
systemVars  Array to identify the system variables. Use NULL if all variables are to be included in the sample. 
ms  Max number of samples in the set. This will be automatically enlarged in AddSample2Samples if necessary. 
ns  Number of samples in the set. 0 after initialization. 
path  The sample set. 
Definition at line 150 of file samples.c.
References MEM_DUP, and NEW.
Referenced by AddBranchToAtlasRRT(), ConnectSamples(), ConnectSamplesChart(), MinimizeOnAtlas(), PathInChart(), ReconstructAtlasPath(), ReconstructAtlasRRTPath(), SmoothSamples(), Steps2PathinAtlasRRT(), and Steps2PathinRRT().
double ConnectSamplesChart 
( 
Tparameters * 
pr, 


unsigned int * 
tp, 


Tbox * 
domain, 


unsigned int 
m, 


unsigned int 
n, 


double * 
s1, 


double * 
s2, 


double 
md, 


boolean 
checkCollisions, 


boolean * 
reached, 


boolean * 
collision, 


double * 
lastSample, 


unsigned int * 
ns, 


double *** 
path, 


TAtlasBase * 
w 

) 
 
Determines if two points on the manifold can be connected with a path of length below a given threshold.
This is the same as ConnectSamples but using a chartbased way to move over the manifold from s1 to s2.
This can be seen as a simply version of AddBranchToAtlasRRT. The main difference is that here we do not maintain an atlas: we generate charts as we advance over the manifold but we release them when they are not any longer useful.
IMPORTANT: here samples are suposed to be given in the simplified form and not in the nonsimplified one, as it is the case of many of the functions on samples.
 Parameters

pr  The set of parameters. 
tp  The topology for the variables. 
domain  The domain for the variables. 
m  The size of each sample. IMPORTANT: here samples are suposed to be given in the simplified form and not in the nonsimplified one, as it is the case of many of the functions on samples. 
n  The number of equations in the problem. This is actually not used in this function but we keep this parameter for compatibility with ConnectSamples. 
s1  The first sample to connect. 
s2  The second sample to connect. 
md  Maximum displacement between the two samples. 
checkCollisions  TRUE if the path has to be collision free. 
reached  (output) TRUE if s2 can be reached form s1. 
collision  (output) TRUE if the connection can not be established due to a collision. 
lastSample  Buffer to store the last sample generated along the path from s1 to s2. Set to NULL if this sample is not requiered. 
ns  Number of steps of the path connecting the two samples. 
path  Samples along the path connecting the two samples. Set ns or path to NULL if you are not interested in the path but only in verifying that the two samples can be connected. Note that the two given samples are NOT included in this output path. 
w  The The world/cuiksystem on which the sample is defined. 
 Returns
 The maximum distance travelled form s1 to s2. The distance from s1 to the sample stored in 'lastSample', if any.
Definition at line 172 of file samples.c.
References AccumulateVector(), AddSample2Samples(), ArrayPi2Pi(), Chart2Manifold(), CS_WD_GET_SIMP_JACOBIAN, CS_WD_IN_COLLISION, CS_WD_SIMP_INEQUALITIES_HOLD, CT_CE, CT_DELTA, CT_E, CT_EPSILON, CT_N_DOF, CT_R, DeleteChart(), DeleteJacobian(), DeleteSamples(), DifferenceVector(), DifferenceVectorTopology(), DistanceTopology(), Error(), FALSE, GetJacobianSize(), GetParameter(), InitChart(), InitSamples(), Manifold2Chart(), NEW, Norm(), PointInBoxTopology(), ScaleVector(), SumVectorScale(), and TRUE.
Referenced by ShortcutSmooth(), and WireAtlasRRTstar().
double ConnectSamples 
( 
Tparameters * 
pr, 


unsigned int * 
tp, 


Tbox * 
domain, 


unsigned int 
m, 


unsigned int 
n, 


double * 
s1, 


double * 
s2, 


double 
md, 


boolean 
checkCollisions, 


boolean * 
reached, 


boolean * 
collision, 


double * 
lastSample, 


unsigned int * 
ns, 


double *** 
path, 


TAtlasBase * 
w 

) 
 
Determines if two points on the manifold can be connected with a path of length below a given threshold.
The method used to determine the path does not return the sortest path and it can fail in many cases, specially when the samples are far away.
IMPORTANT: here samples are suposed to be given in the simplified form and not in the nonsimplified one, as it is the case of many of the functions on samples.
 Parameters

pr  The set of parameters. 
tp  The topology for the variables. 
domain  The domain for the variables. 
m  The size of each sample. IMPORTANT: here samples are suposed to be given in the simplified form and not in the nonsimplified one, as it is the case of many of the functions on samples. 
n  The number of equations in the problem. 
s1  The first sample to connect. 
s2  The second sample to connect. 
md  Maximum displacement between the two samples. 
checkCollisions  TRUE if the path has to be collision free. 
reached  (output) TRUE if s2 can be reached form s1. 
collision  (output) TRUE if the connection can not be established due to a collision. 
lastSample  Buffer to store the last sample generated along the path from s1 to s2. Set to NULL if this sample is not requiered. 
ns  Number of steps of the path connecting the two samples. 
path  Samples along the path connecting the two samples. Set ns or path to NULL if you are not interested in the path but only in verifying that the two samples can be connected. Note that the two given samples are NOT included in this output path. 
w  The The world/cuiksystem on which the sample is defined. 
 Returns
 The maximum distance travelled form s1 to s2. The distance from s1 to the sample stored in 'lastSample', if any.
Definition at line 434 of file samples.c.
References AddSample2Samples(), ArrayPi2Pi(), CS_WD_IN_COLLISION, CS_WD_NEWTON_IN_SIMP, CS_WD_SIMP_INEQUALITIES_HOLD, CT_DELTA, CT_EPSILON, DeleteSamples(), DifferenceVectorTopology(), DistanceTopology(), DIVERGED, FALSE, GetParameter(), InitSamples(), NEW, Norm(), PointInBoxTopology(), SumVectorScale(), and TRUE.
Referenced by AddStepToRRTstar(), BiRRTstar(), ReWireRRTstar(), Steps2PathinRRT(), and WireRRTstar().
void SmoothSamples 
( 
Tparameters * 
pr, 


boolean 
parallel, 


int 
mode, 


unsigned int 
maxIterations, 


unsigned int 
ns, 


double ** 
path, 


unsigned int * 
sns, 


double *** 
spath, 


TAtlasBase * 
w 

) 
 
Produces a path that (locally) minimizes the length and connects the two extremes of the given path.
This implements a basic smoothing structure transforming the input samples (in the original system) to the simplified system and then call a speciallized smoothing procedure.
Only path with more than 2 steps are suitable of optimization. Otherwise this function triggers an error.
 Parameters

pr  The set of parameters. 
parallel  TRUE if we have to execute a parallel vesion of smoothing process. Not relevant in SMOOTH_CUT. 
mode  Smoothing mode to be used: SMOOTH_RANDOM, SMOOTH_GRADIENT, or SMOOTH_CUT. 
maxIterations  Maximum number of iterations. To be scaled by the number of steps in the path. 
ns  The numer of samples in the input path. 
path  The samples in the input path. 
sns  The number of samples in the output (smoothed) path. 
spath  The samples in the output (smoothed) path. 
w  The The world/cuiksystem on which the sample is defined. 
Definition at line 1033 of file samples.c.
References AddSample2Samples(), CS_WD_GENERATE_SIMPLIFIED_POINT, CS_WD_GET_SIMP_TOPOLOGY, CS_WD_GET_SYSTEM_VARS, CS_WD_REGENERATE_ORIGINAL_POINT, CS_WD_REGENERATE_SOLUTION_POINT, CT_DELTA, DeleteStatistics(), Error(), GetElapsedTime(), GetParameter(), GradientSmooth(), InitStatistics(), NEW, PathLength(), RandomSmooth(), ShortcutSmooth(), SMOOTH_GRADIENT, SMOOTH_RANDOM, and SMOOTH_SHORTCUT.
Referenced by main().
void ConcatSamples 
( 
unsigned int 
nvs, 


unsigned int 
ns1, 


double ** 
path1, 


unsigned int 
ns2, 


double ** 
path2, 


unsigned int * 
ns, 


double *** 
path 

) 
 
Produces a path that is the concatenation of two paths.
 Parameters

nvs  The number of values per sample. 
ns1  The number of samples defining the first path. 
path1  The collection of samples defining the first path. 
ns2  The number of samples defining the second path. 
path2  The collection of samples defining the second path. 
ns  The number of samples defining the output path. 
path  The collection of samples defining the output path. 
Definition at line 1150 of file samples.c.
References NEW.
void ReverseConcatSamples 
( 
unsigned int 
nvs, 


unsigned int 
ns1, 


double ** 
path1, 


unsigned int 
ns2, 


double ** 
path2, 


unsigned int * 
ns, 


double *** 
path 

) 
 
Produces a path that is the concatenation of a path and the reverse of a second path. This is basically used to reconstruct paths for bidirectional (Atlas)RRTs.
 Parameters

nvs  The number of values per sample. 
ns1  The number of samples defining the first path. 
path1  The collection of samples defining the first path. 
ns2  The number of samples defining the second path. 
path2  The collection of samples defining the second path. 
ns  The number of samples defining the output path. 
path  The collection of samples defining the output path. 
Definition at line 1171 of file samples.c.
References NEW.
Referenced by PathStart2GoalInRRT().
void ReverseSamples 
( 
unsigned int 
ns, 


double ** 
path 

) 
 
unsigned int ReadOneSample 
( 
Tparameters * 
p, 


char * 
fname, 


unsigned int 
nvs, 


double ** 
s 

) 
 
unsigned int ReadTwoSamples 
( 
Tparameters * 
p, 


char * 
fname, 


unsigned int 
nvs, 


double ** 
s1, 


double ** 
s2 

) 
 
Reads two sample from a file.
If the mechanisms representation is DOFbased (see REP_JOINTS) the samples are read from a .dof file. Otherwise they are read from a .samples file.
 Parameters

p  The set of parameters. 
fname  File from where to read the sample. 
nvs  Number of system variables in the problem = Number of values in the sample. 
s1  Where to store the first sample. The space is allocated internally. 
s2  Where to store the second sample. The space is allocated internally. 
 Returns
 The dimensionality of the sample. Number of system variables of w.
Definition at line 1247 of file samples.c.
References CreateFileName(), CT_REPRESENTATION, DeleteFileName(), Error(), GetFileFullName(), GetParameter(), JOINTS_EXT, LINKS_EXT, NEW, and REP_JOINTS.
Referenced by main().
void SaveSamples 
( 
char * 
fname, 


boolean 
smooth, 


unsigned int 
nvs, 


unsigned int 
ns, 


double ** 
path 

) 
 
Saves a set of samples (possibly defining a solution path) to a file.
 Parameters

fname  The file where to store the samples. 
smooth  TRUE if the set of samples is the result of a smoothing process. 
nvs  The dimensionality of each sample. 
ns  The number of samples defining the path. 
path  The collection of samples defining the path. 
Definition at line 1318 of file samples.c.
References CreateFileName(), DeleteFileName(), SaveSamplesInt(), and SOL_EXT.
Referenced by main(), and MinimizeOnAtlas().
void SaveSamplesN 
( 
char * 
fname, 


boolean 
smooth, 


unsigned int 
n, 


unsigned int 
nvs, 


unsigned int 
ns, 


double ** 
path 

) 
 
Saves a set of samples (possibly defining a solution path) to a file. This function is used to save sets of paths.
 Parameters

fname  The file where to store the samples. 
smooth  TRUE if the set of samples is the result of a smoothing process. 
n  The number of this path. Used to define a different name for a sequence of paths to store. 
nvs  The dimensionality of each sample. 
ns  The number of samples defining the path. 
path  The collection of samples defining the path. 
Definition at line 1333 of file samples.c.
References CreateFileName(), DeleteFileName(), SaveSamplesInt(), and SOL_EXT.
Referenced by MinimizeOnAtlas().
boolean LoadSamples 
( 
Tfilename 
fname, 


unsigned int * 
nvs, 


unsigned int * 
ns, 


double *** 
path 

) 
 
Reads a set of samples from a file created using SaveSamples.
 Parameters

fname  Name of the file containing the samples. 
nvs  Number of entries for each sample. 
ns  Number of samples (output). 
path  The set of samples (output). 
 Returns
 TRUE if the sample set could be actually read.
Definition at line 1352 of file samples.c.
References Advance(), DeleteListOfBoxes(), EndOfList(), Error(), FALSE, First(), GetBoxCenter(), GetBoxNIntervals(), GetCurrent(), GetFileFullName(), InitIterator(), ListSize(), NEW, ReadListOfBoxes(), and TRUE.
Referenced by main().
void PlotSamples 
( 
Tparameters * 
p, 


Tplot3d * 
p3d, 


unsigned int 
xID, 


unsigned int 
yID, 


unsigned int 
zID, 


unsigned int 
ns, 


double ** 
path 

) 
 
Plots a 3D projection of a set of samples representing a path. The path is represented by a red line.
 Parameters

p  The set of parametres. 
p3d  The 3d plot where to add the plot. 
xID  First dimension for the projection (in the original system including system vars and dummies). 
yID  Second dimension for the projection (in the original system including system vars and dummies). 
zID  Third dimension for the projection (in the original system including system vars and dummies). 
ns  The number of samples. 
path  The set of samples. 
Definition at line 1399 of file samples.c.
References Close3dObject(), CT_CUT_X, CT_CUT_Y, CT_CUT_Z, DeleteColor(), GetParameter(), M_2PI, NEW, NewColor(), PlotVect3d(), and StartNew3dObject().
Referenced by main().
void DeleteSamples 
( 
unsigned int 
ns, 


double ** 
path 

) 
 

Follow us!