world.h

Go to the documentation of this file.

#ifndef WORLDH
#define WORLDH

#include "vector.h"
#include "environment.h"
#include "mechanism.h"
#include "cuiksystem.h"
#include "equations.h"
#include "plot3d.h"
#include "box.h"
#include "csmapping.h"
#include "parameters.h"
#include "filename.h"
#include "statistics.h"
#include "list.h"
#include "jacobian.h"
#include "cd.h"

#include <stdlib.h>


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

#define INITIAL_FRAME_DELAY 5.0

#define FRAME_RATE (1.0/10) 

#define FINAL_FRAME_DELAY 500.0
/*****************************************************************************/
/*****************************************************************************/

#define WORLD_IN_DEFINITION 0

#define WORLD_DEFINED 1

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

typedef struct {
  Tjoint *joint; 
  double sign; 
} TBranchStep;

typedef struct {
  Tvector steps; 
} Tbranch;

typedef struct {
  unsigned int stage; 
  Tenvironment e; 
  Tmechanism m;   
  unsigned int nl; 
  unsigned int nb; 
  unsigned int nj; 
  unsigned int no; 
  unsigned int np; 
  unsigned int endEffector; 
  double maxCoord; 
  boolean **checkCollisionsLL;
  boolean **checkCollisionsLO;
  TCuikSystem kinCS; 
  unsigned int nvars;  
  unsigned int nsvars; 
  boolean *systemVars; 
  unsigned int *sortedLinks; 
  unsigned int *jointTo;     
  Tbranch *branch2Link; 
  Tequations refEqs;    
  TJacobian refEqsJ;    
  boolean *openLink;    
  unsigned int nwcd;  
  TworldCD *wcd;      
  unsigned int ndof;       
  unsigned int *joint2dof; 
  unsigned int *dof2joint; 
  unsigned int *dof2range; 
} Tworld;


void InitWorld(Tworld *w);

void InitWorldFromFile(Tparameters *p,Tfilename *f,
                       Tworld *w);

unsigned int AddLink2World(Tlink *l,boolean object,Tworld *w);


unsigned int AddJoint2World(Tjoint *j,Tworld *w);

void AddObstacle2World(char *name,Tpolyhedron *o,Tworld *w);

unsigned int GetWorldMobility(Tworld *w);

boolean IsMechanismInWorldAllSpheres(Tworld *w);

unsigned int GetWorldLinkID(char *linkName,Tworld *w);

unsigned int GetWorldObstacleID(char *obsName,Tworld *w);

Tlink *GetWorldLink(unsigned int linkID,Tworld *w);


Tjoint *GetWorldJoint(unsigned int jointID,Tworld *w);

unsigned int GetWorldNLinks(Tworld *w);

unsigned int GetWorldNJoints(Tworld *w);

unsigned int GetWorldNObstacles(Tworld *w);

unsigned int GetWorldNConvexBodiesInLinks(Tworld *w);

unsigned int GetWorldNConvexBodies(Tworld *w);


unsigned int GetWorldNDOF(Tworld *w);


void GetWorldRangeDOF(unsigned int ndof,Tinterval *r,Tworld *w);


void GetWorldJointLabel(unsigned int ndof,char **string,Tworld *w);

void GetWorldDOFLabel(unsigned int ndof,char **string,Tworld *w);


boolean IsWorldPolynomial(Tworld *w);

void CheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w);

void NoCheckAllCollisions(unsigned int fl,unsigned int fo,Tworld *w);

boolean AnyCollision(Tworld *w);

void PrintCollisions(FILE *f,Tworld *w);

void CheckSelfCollisions(unsigned int fl,Tworld *w);

void NoCheckSelfCollisions(unsigned int fl,Tworld *w);

void CheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w);

void NoCheckLinkLinkCollision(unsigned int a,unsigned int b,Tworld *w);

void CheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w);

void NoCheckLinkObstacleCollision(unsigned int a,unsigned int b,Tworld *w);

void InitWorldCD(Tparameters *pr,unsigned int mt,Tworld *w);

boolean WorldCanCollide(Tworld *w);

void GetLinkTransformsFromSolutionPoint(Tparameters *p,boolean simp,double *sol,
                                        THTransform **tl,Tworld *w);

unsigned int GetSolutionPointFromLinkTransforms(Tparameters *p,THTransform *tl,
                                                double **sol,Tworld *w);

void DeleteLinkTransforms(THTransform *tl,Tworld *w);

boolean WorldContinuousCD(Tworld *w);

boolean MoveAndCheckCDFromTransforms(boolean all,unsigned int tID,
                                     THTransform *tl,THTransform *tlPrev,Tworld *w);

boolean MoveAndCheckCD(Tparameters *p,boolean all,unsigned int tID,
                       double *sol,double *solPrev,Tworld *w);


boolean NewtonInWorld(Tparameters *p,double *v,Tbox *b_sol,Tworld *w);

unsigned int GetWorldSystemVars(boolean **sv,Tworld *w);

unsigned int GetWorldVarTopology(unsigned int vID,Tworld *w);

unsigned int GetWorldSimpVariableMask(Tparameters *p,boolean **sv,Tworld *w);


void GetWorldVarNames(char **vn,Tworld *w);

void GenerateWorldEquations(Tparameters *p,Tworld *w);

void GenerateWorldSingularityEquations(Tparameters *p,char *ln,TCuikSystem *cs,
                                       Tworld *w);

void GenerateWorldTWSEquations(Tparameters *p,char *ln,TCuikSystem *cs,
                               Tworld *w);

void GetWorldJacobian(TJacobian *J,Tworld *w);

void GetWorldSimpJacobian(Tparameters *p,TJacobian *J,Tworld *w);

unsigned int GetWorldSimpTopology(Tparameters *p,
                                  unsigned int **t,Tworld *w);

void GetWorldKinJacobian(Tparameters *p,unsigned int *nr,unsigned int *nc,
                         Tequation ***J,Tworld *w);
unsigned int WorldSimpCuikNewton(Tparameters *p,double *pt,Tworld *w);

void EvaluateWorldJacobian(double *p,double ***J,Tworld *w);

void WorldEvaluateEquations(double *pt,double *r,Tworld *w);

boolean WorldInequalitiesHold(double *pt,Tworld *w);

boolean WorldSimpInequalitiesHold(Tparameters *p,double *pt,Tworld *w);

double WorldErrorInSimpInequalities(Tparameters *p,double *pt,Tworld *w);

void WorldEvaluateSimpEquations(Tparameters *p,double *pt,double *r,
                                Tworld *w);

void WorldEvaluateSubSetSimpEquations(Tparameters *p,boolean *se,double *pt,double *r,
                                      Tworld *w);

double WorldErrorInEquations(double *pt,Tworld *w);

double WorldErrorInSimpEquations(Tparameters *p,double *pt,Tworld *w);

double WorldErrorFromDOFs(Tparameters *p,double *dof,Tworld *w);

double EvaluateWorldCost(Tparameters *p,boolean simp,double *pt,void *w);

unsigned int GetWorldNumVariables(Tworld *w);

unsigned int GetWorldNumSystemVariables(Tworld *w);

unsigned int RegenerateWorldOriginalPoint(Tparameters *p,double *s,
                                          double **o,Tworld *w);

unsigned int WorldGenerateSimplifiedPoint(Tparameters *p,double *o,
                                          double **s,Tworld *w);

void GetWorldInitialBox(Tbox *b,Tworld *w);

void GetWorldSimpInitialBox(Tparameters *p,Tbox *b,Tworld *w);


unsigned int MaxWorldReduction(Tparameters *p,Tbox *b,
                               double *reduction,Tworld *w);

void PlotWorld(Tparameters *pr,Tplot3d *pt,double axesLength,Tworld *w);

unsigned int RegenerateWorldSolutionPoint(Tparameters *pr,double *p,
                                          double **v,Tworld *w);

void WorldPrintAtoms(Tparameters *pr,FILE *f,double *pt,Tworld *w);

void WorldStoreRigidGroups(Tparameters *pr,FILE *f,double *pt,Tworld *w);

void WorldAtomJacobian(Tparameters *pr,double *sol,unsigned int *nr,unsigned int *nc,double ***J,Tworld *w);

void MoveWorld(Tparameters *pr,Tplot3d *pt,Tbox *b,Tworld *w);

void PrintWorldAxes(Tparameters *pr,FILE *f,Tbox *b,Tworld *w);

void PrintWorldCollisionInfo(FILE *f,char *fname,Tworld *w);

void MoveWorldDOF(Tparameters *pr,Tplot3d *pt,double *dof,Tworld *w);

unsigned int WorldDOF2Sol(Tparameters *p,double *dof,double **sol,Tbox *b,Tworld *w);

void WorldSample2DOF(Tparameters *p,double *sample,double *dof,Tworld *w);

void AnimateWorld(Tparameters *pr,char *pname,double axesLength,
                  double frameDelay,Tlist *p,Tworld *w);

void PrintWorldCS(Tparameters *p,Tfilename *fname,Tworld *w);

void PrintWorld(char *fname,int argc,char **arg,Tworld *w);

void DeleteWorld(Tworld *w);

#endif