cp.c
Go to the documentation of this file.
1 #include "cp.h"
2 
3 #include "boolean.h"
4 #include "interval.h"
5 #include "monomial.h"
6 #include "equation.h"
7 #include "variable.h"
8 #include "defines.h"
9 #include "box_list.h"
10 #include "random.h"
11 #include "jacobian.h"
12 
13 #include <math.h>
14 
15 
56 void GetSCpSystem(boolean silhouette,Tparameters *p,
57  double **planes,Tcp *cp,TCuikSystem *cs,
58  boolean **originalSystemVars,TCuikSystem *csR);
59 
77  unsigned int n,unsigned int dim,unsigned int m,double **v);
78 
79 
80 /*********************************************************************/
81 /*********************************************************************/
82 /*********************************************************************/
83 
84 void GetSCpSystem(boolean silhouette,Tparameters *p,double **planes,
85  Tcp *cp,TCuikSystem *cs,boolean **originalSystemVars,TCuikSystem *csR)
86 {
87  unsigned int maxLevel,i,nvars,neqs,nvarsR;
88 
89  maxLevel=(unsigned int)GetParameter(CT_N_DOF,p)-1;
90 
91  if ((cp->level==maxLevel)&&(!silhouette))
92  Error("In the last recursion level only silhouette are computed");
93 
94  /*Copy the original system*/
95  CopyCuikSystem(csR,cs);
96 
97  /*Get the equations/variables of the original system*/
98  nvars=GetCSNumVariables(cs);
99  neqs=GetCSNumEquations(cs);
100 
101  /*Set the already fixed ranges (ranges up to level)*/
102  for(i=0;i<cp->level;i++)
104 
105  if ((!silhouette)&&(cp->level==maxLevel))
106  Error("Computing critical points on a terminal silhouette");
107 
108  /* For 1 degree of freedom remaining we do not need the Jacobian */
109  if (cp->level<maxLevel)
110  {
111  unsigned int j,npp,nl,*nvl;
112  char vname[100];
113  Tequation eq,eq1;
114  Tmonomial f;
115  TJacobian J; /*The Jacobian Matrix*/
116  #if (BOUNDED_LAMBDAS)
117  Tinterval one_one,zero_one;
118  #else
119  Tinterval allR,one;
120  #endif
121 
122  /*Determine the number of projection planes to use*/
123  npp=(silhouette?2:1); /*number of projection planes to use*/
124  nl=neqs+npp; /*rows of the Jacobian matrix*/
125 
126  /*Add the lambda_i variables*/
127 
128  #if (BOUNDED_LAMBDAS)
129  NewInterval(-1.0,1.0,&one_one);
130  NewInterval( 0.0,1.0,&zero_one);
131  #else
132  NewInterval(-INF,INF,&allR);
133  NewInterval(1.0,1.0,&one);
134  #endif
135 
136  NEW(nvl,nl,unsigned int);
137  for(i=0;i<nl;i++)
138  {
139  Tvariable v;
140  /*lambda_i*/
141  sprintf(vname,"_lambda_%u",i+1);
142 
143  NewVariable(SYSTEM_VAR,vname,&v);
144  #if (BOUNDED_LAMBDAS)
145  SetVariableInterval((i==0?&zero_one:&one_one),&v);
146  #else
147  SetVariableInterval((i<nl-1?&allR:&one),&v);
148  #endif
149 
150  nvl[i]=AddVariable2CS(&v,csR);
151 
152  DeleteVariable(&v);
153  }
154 
155  #if (BOUNDED_LAMBDAS)
156  /*Add the equation \sum_i lambda_i^2 =1*/
157  GenerateGeneralNormEquation(nl,nvl,1.0,&eq);
158  AddEquation2CS(p,&eq,csR);
159  DeleteEquation(&eq);
160  #endif
161 
162  /*Generate the Jacobian matrix (a matrix of equations (possibly ct))*/
163  GetCSJacobian(&J,cs);
164 
165  /*Generate the equations from the extended Jacobian matrix (for the non-fixed
166  variables up to now)*/
167  InitMonomial(&f);
168  for(j=cp->level;j<nvars;j++)
169  {
170  /* eq=\sum_i lambda_i*J(i,j)=\sum_i lambda_i \sum_{k (monomials in J(i,j))} f_{i,j,k}
171  The monomials in J(i,j) include the ct monomial that is stored as
172  the value of equation J(i,j) */
173 
174  InitEquation(&eq);
175  SetEquationCmp(EQU,&eq);
177 
178  for(i=0;i<neqs;i++)
179  {
180  CopyEquation(&eq1,GetJacobianEquation(i,j,&J));
181  VarScaleEquation(nvl[i],&eq1);
182  AccumulateEquations(&eq1,1.0,&eq);
183  DeleteEquation(&eq1);
184  }
185 
186  for(i=0;i<npp;i++)
187  {
188  InitMonomial(&f);
189  AddVariable2Monomial(NFUN,nvl[neqs+i],1,&f);
190  AddCt2Monomial(planes[i][j],&f);
191  AddMonomial(&f,&eq);
192  DeleteMonomial(&f);
193  }
194  AddEquation2CS(p,&eq,csR);
195  DeleteEquation(&eq);
196  }
197 
198  /*Remove the stored data*/
199  DeleteJacobian(&J);
200  free(nvl);
201  }
202 
203  /* The variables possibly added to csR are marked as non
204  original variables.*/
205  nvarsR=GetCSSystemVars(originalSystemVars,csR);
206  for(i=nvars;i<nvarsR;i++)
207  (*originalSystemVars)[i]=FALSE;
208 }
209 
210 /*
211  * Generates 'n' orthonormals planes in a space of dimension 'dim' that
212  * we use for projection. 'm' is level at which these planes are fixed ->
213  * values up to m are zero
214  */
216  unsigned int n,unsigned int dim,unsigned int m,double **v)
217 {
218 #if (RANDOM_PLANES)
219  unsigned int i,j;
220 
221  /*generate 'n' random planes*/
222  for(i=0;i<n;i++)
223  {
224  for(j=0;j<m;j++)
225  v[i][j]=0;
226  for(j=m;j<dim;j++)
227  v[i][j]=randomDouble();
228  }
229 
230  if (n>0)
231  {
232  double norm;
233 
234  /*The first plane is normalized*/
235  norm=0;
236  for(j=m;j<dim;j++)
237  norm+=(v[0][j]*v[0][j]);
238  norm=sqrt(norm);
239  for(j=m;j<dim;j++)
240  v[0][j]/=norm;
241 
242  if (n>1)
243  {
244  double p; /*norm of the projection of v1 on v0*/
245 
246  /*Second vector is made orthonormal w.r.t. vector 1*/
247  /* v1=v1-proj(v1,v0) and then normalized. */
248 
249  /* compute the norm of the projection of 'v1' on 'v0' */
250  p=0;
251  for(j=m;j<dim;j++)
252  p+=(v[1][j]*v[0][j]);
253  /*Substract from v1 the component on v0*/
254  for(j=m;j<dim;j++)
255  v[1][j]-=p*v[0][j];
256  /*Normalize the resulting v1 (othonormal w.r.t. v0*/
257  norm=0;
258  for(j=m;j<dim;j++)
259  norm+=(v[1][j]*v[1][j]);
260  norm=sqrt(norm);
261  for(j=m;j<dim;j++)
262  v[1][j]/=norm;
263  }
264  }
265 #else
266  unsigned int i,j;
267 
268  for(i=0;i<n;i++)
269  {
270  for(j=0;j<m;j++)
271  v[i][j]=0;
272  for(j=m;j<dim;j++)
273  v[i][j]=(i==(j-m));
274  }
275 #endif
276 }
277 
278 void NewCP(unsigned int l,Tbox *b,Tcp *cp)
279 {
280  unsigned int i;
281  double center;
282  Tinterval *it;
283 
284  cp->level=l;
285  CopyBox(&(cp->fixedRanges),b);
286 
287  /*The first 'l' variables of the box must have punctual range*/
288  /*Critical points are defined form solution boxes that are tiny but
289  not punctual. We transform them to punctual taking the center of
290  the solutin intervals. */
291  for(i=0;i<l;i++)
292  {
293  it=GetBoxInterval(i,b);
294  center=IntervalCenter(it);
295  NewInterval(center,center,it);
296  }
297 }
298 
299 unsigned int GetCPlevel(Tcp *cp)
300 {
301  return(cp->level);
302 }
303 
305 {
306  return(&(cp->fixedRanges));
307 }
308 
309 void DealWithCP(FILE *f_out,unsigned int n,Tparameters *p,TCuikSystem *cs,
310  Tlist *cps,Tcp *cp)
311 {
312  TCuikSystem csR; /* The cuik system reduced according to the
313  information stored in the 'cp*/
314  Titerator it;
315  Tcp *ncp; /*new critical points recursively generated from here*/
316  unsigned int l,maxLevel,nVars;
317  boolean *originalSystemVars;
318  Tlist solutions,points;
319  char heading[20];
320  double *planes[2];
321  unsigned int nsols;
322 
323  maxLevel=(unsigned int)GetParameter(CT_N_DOF,p)-1;
324 
325  /* Ensure we will get all the possible solutions */
326  nsols=(unsigned int)GetParameter(CT_N_SOLUTIONS,p);
328 
329  nVars=GetCSNumVariables(cs);
330 
331  NEW(planes[0],nVars,double);
332  NEW(planes[1],nVars,double);
333 
334  GenerateProjectionPlanes(p,2,nVars,cp->level,planes);
335 
336  GetSCpSystem(TRUE,p,planes,cp,cs,&originalSystemVars,&csR); /*TRUE -> Silhouette*/
337 
338  fprintf(f_out,"SILHOUTTE LEVEL: %u/%u\n",cp->level,maxLevel);
339  PrintCuikSystemWithSimplification(p,f_out,&csR);
340  //exit(0);
341 
342  InitListOfBoxes(&solutions);
343  SolveCuikSystem(p,FALSE,NULL,NULL,f_out,&solutions,&csR);
344 
345  sprintf(heading,"SIL %u",n);
346  PrintListOfBoxes(f_out,originalSystemVars,heading,&solutions);
347  DeleteListOfBoxes(&solutions);
348 
349  free(originalSystemVars);
350  DeleteCuikSystem(&csR);
351 
352  if (cp->level<maxLevel) /*If the silhouette was not the terminal one*/
353  {
354  GetSCpSystem(FALSE,p,planes,cp,cs,&originalSystemVars,&csR);/*FALSE -> Critical points*/
355 
356  InitListOfBoxes(&solutions);
357 
358  SolveCuikSystem(p,FALSE,NULL,NULL,f_out,&solutions,&csR);
359 
360  ListOfBoxesCluster(originalSystemVars,&points,&solutions);
361 
362  InitIterator(&it,&points);
363  First(&it);
364  l=cp->level+1;
365  while(!EndOfList(&it))
366  {
367  NEW(ncp,1,Tcp);
368  NewCP(l,(Tbox *)GetCurrent(&it),ncp);
369  AddLastElement((void *)&ncp,cps);
370 
371  Advance(&it);
372  }
373 
374  sprintf(heading,"CP %u",n);
375  PrintListOfBoxes(f_out,originalSystemVars,heading,&points);
376 
377  free(originalSystemVars);
378  DeleteCuikSystem(&csR);
379  DeleteListOfBoxes(&points);
380  DeleteListOfBoxes(&solutions);
381  }
382  free(planes[0]);
383  free(planes[1]);
385 }
386 
387 
388 void DeleteCP(Tcp *cp)
389 {
390  DeleteBox(&(cp->fixedRanges));
391 }
Definition of the boolean type.
void First(Titerator *i)
Moves an iterator to the first position of its associated list.
Definition: list.c:356
void PrintCuikSystemWithSimplification(Tparameters *p, FILE *f, TCuikSystem *cs)
Prints the simplified cuiksystem.
Definition: cuiksystem.c:5047
Definition of the Tequation type and the associated functions.
Tinterval * GetBoxInterval(unsigned int n, Tbox *b)
Returns a pointer to one of the intervals defining the box.
Definition: box.c:270
void SetCSVariableRange(unsigned int n, Tinterval *r, TCuikSystem *cs)
Gets the range of a variable from a cuiksystem.
Definition: cuiksystem.c:2574
#define SYSTEM_EQ
One of the possible type of equations.
Definition: equation.h:146
#define FALSE
FALSE.
Definition: boolean.h:30
void SetEquationType(unsigned int type, Tequation *eq)
Changes the type of the equation (SYSTEM_EQ, CARTESIAN_EQ, DUMMY_EQ, DERIVED_EQ). ...
Definition: equation.c:1013
unsigned int GetCSNumEquations(TCuikSystem *cs)
Gets the number of equations already in the cuiksystem.
Definition: cuiksystem.c:2664
#define NEW(_var, _n, _type)
Allocates memory space.
Definition: defines.h:385
void DealWithCP(FILE *f_out, unsigned int n, Tparameters *p, TCuikSystem *cs, Tlist *cps, Tcp *cp)
Processes a critical point.
Definition: cp.c:309
void PrintListOfBoxes(FILE *f, boolean *used, char *heading, Tlist *l)
Prints a list of boxes.
Definition: box_list.c:265
void DeleteEquation(Tequation *eq)
Destructor.
Definition: equation.c:1748
Definition of the TJacobian type and the associated functions.
#define SYSTEM_VAR
One of the possible type of variables.
Definition: variable.h:24
unsigned int AddVariable2CS(Tvariable *v, TCuikSystem *cs)
Adds a variable to the system.
Definition: cuiksystem.c:2511
void CopyEquation(Tequation *eq_dst, Tequation *eq_orig)
Copy constructor.
Definition: equation.c:216
Tbox * GetCPFixedRanges(Tcp *cp)
Gets the collection of fixed ranges for a critical point.
Definition: cp.c:304
void DeleteJacobian(TJacobian *j)
Destructor.
Definition: jacobian.c:249
void ListOfBoxesCluster(boolean *used, Tlist *l_out, Tlist *l_in)
Clusters a list of boxes.
Definition: box_list.c:138
#define EQU
In a Tequation, the equation relational operator is equal.
Definition: equation.h:201
void DeleteCuikSystem(TCuikSystem *cs)
Destructor.
Definition: cuiksystem.c:5113
unsigned int GetCSSystemVars(boolean **sv, TCuikSystem *cs)
Identifies the system variables.
Definition: cuiksystem.c:2614
Tequation * GetJacobianEquation(unsigned int r, unsigned int c, TJacobian *j)
Returns one element of the Jacobian.
Definition: jacobian.c:57
void SetVariableInterval(Tinterval *i, Tvariable *v)
Sets the new range for the variable.
Definition: variable.c:70
#define TRUE
TRUE.
Definition: boolean.h:21
void InitEquation(Tequation *eq)
Constructor.
Definition: equation.c:86
void Error(const char *s)
General error function.
Definition: error.c:80
#define NFUN
No trigonometric function for the variable.
Definition: variable_set.h:36
void GenerateProjectionPlanes(Tparameters *p, unsigned int n, unsigned int dim, unsigned int m, double **v)
Generates n orthonormals planes.
Definition: cp.c:215
void DeleteCP(Tcp *cp)
Destructor.
Definition: cp.c:388
Collection of methods to work on Tlist of boxes.
void AddMonomial(Tmonomial *f, Tequation *eq)
Adds a new monomial to the equation.
Definition: equation.c:1356
void CopyCuikSystem(TCuikSystem *cs_dst, TCuikSystem *cs_src)
Copy constructor.
Definition: cuiksystem.c:2204
double randomDouble()
Returns a random double in the [0,1] interval.
Definition: random.c:33
void SolveCuikSystem(Tparameters *p, boolean restart, char *fstate, Tbox *searchSpace, FILE *f_out, Tlist *sol, TCuikSystem *cs)
Determines the solution set for a cuiksystem.
Definition: cuiksystem.c:3926
void DeleteListOfBoxes(Tlist *l)
Destructor.
Definition: box_list.c:353
void AddLastElement(void *Info, Tlist *list)
Adds an element at the tail of the list.
Definition: list.c:206
void SetEquationCmp(unsigned int cmp, Tequation *eq)
Changes the relational operator (LEQ, GEQ, EQU) of the equation.
Definition: equation.c:1018
void GenerateGeneralNormEquation(unsigned int nv, unsigned int *v, double n, Tequation *eq)
Construtor. Generates an equation that is the norm of a vector.
Definition: equation.c:1443
boolean EndOfList(Titerator *i)
Checks if an iterator is pointing at the end of the list.
Definition: list.c:445
void VarScaleEquation(unsigned int v, Tequation *eq)
Scales an equation with a variable factor.
Definition: equation.c:669
An equation.
Definition: equation.h:236
void DeleteVariable(Tvariable *v)
Destructor.
Definition: variable.c:95
unsigned int level
Definition: cp.h:58
Definitions of constants and macros used in several parts of the cuik library.
A generic list.
Definition: list.h:46
void InitIterator(Titerator *i, Tlist *list)
Constructor.
Definition: list.c:284
void GetCSJacobian(TJacobian *J, TCuikSystem *cs)
Defines the Jacobian of a CuikSystem.
Definition: cuiksystem.c:2669
void NewVariable(unsigned int type, char *name, Tvariable *v)
Constructor.
Definition: variable.c:21
void CopyBox(void *b_out, void *b_in)
Box copy operator.
Definition: box.c:160
void NewCP(unsigned int l, Tbox *b, Tcp *cp)
Constructor.
Definition: cp.c:278
void AddEquation2CS(Tparameters *p, Tequation *eq, TCuikSystem *cs)
Adds an equation to the system.
Definition: cuiksystem.c:2481
A scaled product of powers of variables.
Definition: monomial.h:32
A table of parameters.
Critical point structure.
Definition: cp.h:57
#define CT_N_SOLUTIONS
Number of solution boxes to deliver.
Definition: parameters.h:304
A box.
Definition: box.h:83
Data associated with each variable in the problem.
Definition: variable.h:84
void InitListOfBoxes(Tlist *l)
Constructor.
Definition: box_list.c:24
A cuiksystem, i.e., a set of variables and equations defining a position analysis problem...
Definition: cuiksystem.h:181
void * GetCurrent(Titerator *i)
Gets the element pointed by the iterator.
Definition: list.c:299
Definition of the Tvariable type and the associated functions.
Definition of the Tcp type and the associated functions.
void AddVariable2Monomial(unsigned int fn, unsigned int varid, unsigned int p, Tmonomial *f)
Adds a power variable to the monomial.
Definition: monomial.c:171
double IntervalCenter(Tinterval *i)
Gets the interval center.
Definition: interval.c:129
void GetSCpSystem(boolean silhouette, Tparameters *p, double **planes, Tcp *cp, TCuikSystem *cs, boolean **originalSystemVars, TCuikSystem *csR)
Generates a cuiksystem whose solution is a silhouette or a set of critical points.
Definition: cp.c:84
void DeleteBox(void *b)
Destructor.
Definition: box.c:1259
The Jacobian of a set of equations.
Definition: jacobian.h:23
double GetParameter(unsigned int n, Tparameters *p)
Gets the value for a particular parameter.
Definition: parameters.c:93
void NewInterval(double lower, double upper, Tinterval *i)
Constructor.
Definition: interval.c:47
#define INF
Infinite.
Definition: defines.h:70
void AddCt2Monomial(double k, Tmonomial *f)
Scales a monomial.
Definition: monomial.c:158
List iterator.
Definition: list.h:61
#define CT_N_DOF
Dimensionality of the solution space for the mechanism at hand.
Definition: parameters.h:318
Definition of basic randomization functions.
Definition of the Tmonomial type and the associated functions.
unsigned int GetCPlevel(Tcp *cp)
Gets the creation level of a critical point.
Definition: cp.c:299
Defines a interval.
Definition: interval.h:33
Tbox fixedRanges
Definition: cp.h:62
void ChangeParameter(unsigned int n, double v, Tparameters *p)
Sets the value for a particular parameter.
Definition: parameters.c:164
void InitMonomial(Tmonomial *f)
Constructor.
Definition: monomial.c:17
unsigned int GetCSNumVariables(TCuikSystem *cs)
Gets the number of variables already in the cuiksystem.
Definition: cuiksystem.c:2544
void AccumulateEquations(Tequation *eqn, double ct, Tequation *eq)
Adds a scaled equation to another equation.
Definition: equation.c:366
boolean Advance(Titerator *i)
Moves an iterator to the next position of its associated list.
Definition: list.c:373
void DeleteMonomial(Tmonomial *f)
Destructor.
Definition: monomial.c:289
Definition of the Tinterval type and the associated functions.