readworld.y

Go to the documentation of this file.

%{ 
#include "world.h"

#include "boolean.h"
#include "error.h"
#include "error_world.h"
#include "constants.h"
#include "defines.h"
#include "color.h"
#include "htransform.h"
#include "vector.h"

#include <stdlib.h> 
#include <stdio.h>
#include <string.h>
#include <math.h>

/*
* Definition of the function that initilizes a kinematic equations from a file.
* This is a method of the equations object.
*/

  /*Lex and Yacc variables*/
  extern FILE *ReadWorldin;

  /* first link/object in the file currently bein parsed */
  extern unsigned int firstLink; /* first link in the current object */
  extern unsigned int firstObject; /* first object in the current file */

  /*Lex and Yacc functions*/
  int ReadWorldlex(void); 
  void Switch2File(char *ln,unsigned int nl,unsigned int no,char *path,char *file);

  /*Our own variables*/
  extern unsigned int RWline; /* line number currently processed 
                                 (incremented by the LEX processor)*/
  
  /*Auxiliary variables used along readwordl*/
  unsigned int rw_lID,rw_lID1,rw_lID2; /*link identifier*/
  Tlink rw_link;
  Tjoint rw_joint;
  boolean rw_visible;

  /* Global pointer to allow the different parts of the parser to acces the 
     equations being initialized*/
  Tworld *rw_world;

  THTransform rw_transform;

  unsigned int rw_r;

  double **rw_point;
  unsigned int rw_i,rw_max;
  double *rw_X,*rw_Y,*rw_Z;
  double rw_angle;
  Tinterval rw_range,rw_range0;
  boolean rw_has_limits;
  double **rw_range_point;
  double **rw_in_patch_point;
  Tcolor rw_color_default,rw_color_body;
  Tfilename *rw_filename;
  Tpolyhedron rw_cbody;
  boolean rw_endEffector;

  Tconstants rw_constants;

  boolean rw_avoidLimits;
  double rw_avoidLimitsWeight;

  boolean rw_prevolute;
%}

%union 
{ 
  char *id; 
  char *string;
  int int_number;
  double real_number;
  double color[3];
} 

%start world

%token _CONSTANTS _ASSIGN _PI _COS _SIN _SQRT _LINKS _JOINTS _OBSTACLES _COLLISIONS _CHECK _NO _ALL _BODY _GRANULARITY _FIX  _ID _TX _TY _TZ _TXYZ _RX _RY _RZ _PRISMATIC _REVOLUTE _CREVOLUTE _SPHERICAL _UNIVERSAL _SPH_SPH _SPH_PRS_SPH _IN_PATCH _BOX _SPHERE _CYLINDER _LINE _SEGMENTS _LENGTH _RADIUS _SELFCOLLISIONS _RANGE _COLOR _RED _GREEN _BLUE _PURPLE _CYAN _YELLOW _WHITE _BLACK _GREY _DECORATION _HIDDEN _AVOID _LIMITS _INCLUDE

%token <id> _IDENTIFIER
%token <id> _EXT_IDENTIFIER
%token <int_number> _INTEGER 
%token <real_number> _REAL
%token <string> _STRING

%type <int_number> granularity
%type <int_number> shape_status
%type <real_number> expr
%type <color> color
%type <id> any_id 

%left '+' '-' 
%left '*' '/'
%left '^'
%right _MAX_PRECEDENCE 
%% 

world : world def_block
      |
      ;

def_block : constant_defs
          | link_defs 
          | joint_defs 
          | obstacle_defs 
          | collision_defs 
          | included_defs
          ;

constant_defs: _CONSTANTS constant_list
             ;

constant_list : constant_definition constant_list
              | 
              ;

constant_definition : _IDENTIFIER _ASSIGN expr
                      {
                        if (GetConstantWithName($1,&rw_constants)!=NO_UINT)
                          {
                            char s[200];
                            
                            sprintf(s,"Duplicated constant %s",$1);
                            ReadWorlderror(s);
                          }
                        
                        AddConstant($1,$3,&rw_constants);
                        free($1);
                      }
                    ;

any_id : _IDENTIFIER
         {
           $$=$1; /* do not free $1, we will free $$ */
         }
       | _EXT_IDENTIFIER
         {
           $$=$1; /* do not free $1, we will free $$ */
         }
       ;

expr : '+' expr    %prec _MAX_PRECEDENCE
       {
         $$=$2;
       }
     | '-' expr    %prec _MAX_PRECEDENCE
       {
         $$=-$2;
       }
     | expr '+' expr 
       {
         $$=$1+$3;
       }
     | expr '-' expr 
       {
         $$=$1-$3;
       }
     | expr '*' expr 
       {
         $$=$1*$3;
       }
     | expr '^' expr 
       {
         $$=pow($1,$3);
       }
     | expr '/' expr 
       {
         if ($3==0.0)
           ReadWorlderror("Division by zero");
         
         $$=$1/$3;
       }
     | '(' expr ')'
       {
         $$=$2;
       }       
     | _PI 
       {
         $$=M_PI;
       } 
     | _SIN '(' expr ')'
       {
         $$=sin($3);
       }
     | _COS '(' expr ')'
       {
         $$=cos($3);
       }
     | _SQRT '(' expr ')'
       {
         $$=sqrt($3);
       }
     | any_id
       {
         unsigned int nc;
         
         nc=GetConstantWithName($1,&rw_constants);
         
         if (nc!=NO_UINT)
           $$=GetConstantValue(nc,&rw_constants);
         else
           {
             char ms[200];
             sprintf(ms,"Undefined constant: %s",$1);
             ReadWorlderror(ms);
           }
           free($1);
       }
     | _INTEGER 
       {
         $$=(double)$1;
       } 
     | _REAL
       {
         $$=$1;
       }
     ; 

link_defs : _LINKS link_list
          ;
         
link_list : link_definition link_list
          | 
          ;

link_definition: _IDENTIFIER 
                 {
                   if (GetConstantWithName($1,&rw_constants)!=NO_UINT)
                     ReadWorlderror("Link with the name of a constant");

                   if (GetWorldLinkID($1,rw_world)!=NO_UINT)
                     ReadWorlderror("Duplicated link");

                   /*adds an empty link to the world*/
                   InitLink($1,&rw_link);
                   
                   free($1);
                 }
                 opt_link_definition
                 {
                   AddLink2World(&rw_link,rw_endEffector,rw_world);
                   DeleteLink(&rw_link);
                 }
                 ;


opt_link_definition : link_separator link_color link_shapes
                    |
                    ;

link_separator : ':'
                 {
                   rw_endEffector=FALSE;
                 }
               | '>'
                 {
                   rw_endEffector=TRUE;
                 }
               ;

link_shapes : shape 
              {
                AddBody2Link(&rw_cbody,&rw_link);
                DeletePolyhedron(&rw_cbody);
              }
              link_shapes
            | shape 
              {
                AddBody2Link(&rw_cbody,&rw_link);
                DeletePolyhedron(&rw_cbody);
              }
            ;

shape: _BODY _STRING body_color granularity shape_status 
       {
         Tfilename fb;

         CreateFileName(GetFilePath(rw_filename),$2,NULL,NULL,&fb);
         
         InitPolyhedronFromFile(&fb,&rw_color_body,
                                 $4,$5,&rw_cbody);

         DeleteFileName(&fb);
         free($2);
       }
     | _BOX
       {
         rw_i=0;
         rw_lID=NO_UINT;
       } 
       point pointp body_color shape_status
       {
         NewBox(rw_point[0][0],rw_point[0][1],rw_point[0][2],
                rw_point[1][0],rw_point[1][1],rw_point[1][2],
                &rw_color_body,$6,&rw_cbody);
       }
     | _SPHERE  expr  
       {
         rw_i=0;
         rw_lID=NO_UINT;
       } 
       point body_color granularity shape_status
       {
         NewSphere($2,rw_point[0],&rw_color_body,$6,$7,&rw_cbody);
       }
     | _CYLINDER  expr 
       {
         rw_i=0;
         rw_lID=NO_UINT;
       } 
       point pointp body_color granularity shape_status 
       {
         NewCylinder($2,rw_point[0],rw_point[1],&rw_color_body,2*$7,$8,&rw_cbody);
       }
     | _LINE 
       {
         rw_i=0;
         rw_lID=NO_UINT;
       } 
       point pointp body_color shape_status 
       {
         NewLine(rw_point[0],rw_point[1],&rw_color_body,$6,&rw_cbody);
       }
     | _SEGMENTS 
       {
         rw_i=0;
         rw_max=100;
         NEW(rw_X,rw_max,double);
         NEW(rw_Y,rw_max,double);
         NEW(rw_Z,rw_max,double);
         rw_lID=NO_UINT;
       } 
       point_list body_color 
       {
         NewSegments(rw_i,rw_X,rw_Y,rw_Z,&rw_color_body,&rw_cbody);
         free(rw_X);
         free(rw_Y);
         free(rw_Z);
       }
       ;

granularity : _GRANULARITY ':' _INTEGER
              {
                $$=$3;
              }
            |
              {
                $$=5;
              }

            ;

shape_status : _HIDDEN
               {
                 $$=HIDDEN_SHAPE;
               }
             | _DECORATION
               {
                 $$=DECOR_SHAPE;
               }
             |
               {
                 $$=NORMAL_SHAPE;
               }
             ;

color : _COLOR  '(' expr ',' expr ',' expr ')'
        {
          $$[0]=$3;
          $$[1]=$5;
          $$[2]=$7;
        }
      | _RED
        {
          $$[0]=1;
          $$[1]=0;
          $$[2]=0;
        }
      | _GREEN
        {
          $$[0]=0;
          $$[1]=1;
          $$[2]=0;
        }
      | _BLUE
        {
          $$[0]=0;
          $$[1]=0;
          $$[2]=1;
        }
      | _BLACK
        {
          $$[0]=0;
          $$[1]=0;
          $$[2]=0;
        }
      | _GREY
        {
          $$[0]=0.7;
          $$[1]=0.7;
          $$[2]=0.7;
        }
      | _WHITE
        {
          $$[0]=1;
          $$[1]=1;
          $$[2]=1;
        }
      | _YELLOW
        {
          $$[0]=1;
          $$[1]=1;
          $$[2]=0;
        }
      | _PURPLE
        {
          $$[0]=1;
          $$[1]=0;
          $$[2]=1;
        }
      | _CYAN
        {
          $$[0]=0;
          $$[1]=1;
          $$[2]=1;
        }
      | _INTEGER '*' color
        {
          unsigned int i;

          for(i=0;i<3;i++)
            $$[i]=(double)$1*$3[i];
        }
      | _REAL '*' color
        {
          unsigned int i;

          for(i=0;i<3;i++)
            $$[i]=$1*$3[i];
        }
      | color '+' color
        {
          unsigned int i;

          for(i=0;i<3;i++)
            $$[i]=$1[i]+$3[i];
        }
      ;

link_color: color
            {
              NewColor($1[0],$1[1],$1[2],&rw_color_default);
            }
          |
            {
              NewColor(DLC_R,DLC_G,DLC_B,&rw_color_default);
            }
          ;

body_color: color
            {
              NewColor($1[0],$1[1],$1[2],&rw_color_body);
            }
          |
            {
              CopyColor(&rw_color_body,&rw_color_default);
            }
          ;

joint_defs : _JOINTS joint_list
           ;

joint_list : joint_definition joint_list
           | 
           ;

joint_definition: fix_joint
                | prismatic_joint 
                | revolute_joint
                | crevolute_joint
                | spherical_joint
                | universal_joint
                | sph_sph_joint
                | sph_prs_sph_joint
                | in_patch_joint
                ;

fix_joint : _FIX  ':' any_id any_id
            {
              HTransformIdentity(&rw_transform);
            }
            transforms
            {
              unsigned int id1,id2;
                   
              id1=GetWorldLinkID($3,rw_world);  
              if (id1==NO_UINT)
                ReadWorlderror("Unkown link (1st id) when defining a fix joint");
                   
              id2=GetWorldLinkID($4,rw_world);          
              if (id2==NO_UINT)
                ReadWorlderror("Unkown link (2st id) when allowing a fix joint");

              NewFixJoint(GetWorldNJoints(rw_world),
                          id1,GetWorldLink(id1,rw_world),
                          id2,GetWorldLink(id2,rw_world),
                          &rw_transform,&rw_joint);

              AddJoint2World(&rw_joint,rw_world);

              DeleteJoint(&rw_joint);
              free($3);
              free($4);

              HTransformDelete(&rw_transform);

              rw_prevolute=FALSE;
            }
          ;

transforms: transform '*' transforms
          | transform 
          ;

transform : _ID 
            {
              /* Nothing need to be done */
            }
          |
            _TX '(' expr ')'
            {
              THTransform t;
              
              HTransformTx($3,&t);
              
              HTransformProduct(&rw_transform,&t,&rw_transform);
            }
          | _TY '(' expr ')'
            {
              THTransform t;
              
              HTransformTy($3,&t);
              
              HTransformProduct(&rw_transform,&t,&rw_transform);
            }
          | _TZ '(' expr ')'
            {
              THTransform t;
              
              HTransformTz($3,&t);
              
              HTransformProduct(&rw_transform,&t,&rw_transform);
            }
          | _TXYZ '(' expr ',' expr ',' expr')'
            {
              THTransform t;
              
              HTransformTxyz($3,$5,$7,&t);
              
              HTransformProduct(&rw_transform,&t,&rw_transform);
            }
          | _RX '(' expr ')'
            {
              THTransform t;
              
              HTransformRx($3,&t);
              
              HTransformProduct(&rw_transform,&t,&rw_transform);
            }
          | _RY '(' expr ')'
            {
              THTransform t;
              
              HTransformRy($3,&t);
              
              HTransformProduct(&rw_transform,&t,&rw_transform);
            }
          | _RZ '(' expr ')'
            {
              THTransform t;
              
              HTransformRz($3,&t);
              
              HTransformProduct(&rw_transform,&t,&rw_transform);
            }
          ;

prismatic_joint: _PRISMATIC ':'
                 any_id 
                  { 
                    rw_lID1=rw_lID=GetWorldLinkID($3,rw_world); 
                    if (rw_lID1==NO_UINT)
                      ReadWorlderror("Unkown link (1st id) when defining a prismatic joint");

                    rw_i=0;
                  }
                 point pointp
                 any_id
                 { 
                   rw_lID2=rw_lID=GetWorldLinkID($7,rw_world);
                   if (rw_lID2==NO_UINT)
                     ReadWorlderror("Unkown link (2st id) when allowing a prismatic joint");
                 } 
                 point pointp 
                 _RANGE range
                 avoid_limits
                 {
                   NewPrismaticJoint(GetWorldNJoints(rw_world),
                                     rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                     rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                     rw_point,&rw_range,
                                     rw_avoidLimits,rw_avoidLimitsWeight,
                                     &rw_joint);
                   
                   AddJoint2World(&rw_joint,rw_world);

                   DeleteJoint(&rw_joint);
                   free($3);
                   free($7);

                   rw_prevolute=FALSE;
                 }
                 ;

point : '(' expr ',' expr ',' expr ')'
        {
          rw_point[rw_i][0]=$2;
          rw_point[rw_i][1]=$4;
          rw_point[rw_i][2]=$6;
          rw_i++;
        }
      | _INTEGER
        {
          Tpolyhedron *b;
          
          if (rw_lID==NO_UINT)
            ReadWorlderror("Reference to body points can only be used inside joint definition");
          b=GetLinkBody(0,GetWorldLink(rw_lID,rw_world));
          GetPolyhedronDefiningPoint($1,rw_point[rw_i],b);
          rw_i++;
        }
      | _INTEGER ':' _INTEGER
        {
          Tpolyhedron *b;
          
          if (rw_lID==NO_UINT)
            ReadWorlderror("Reference to body points can only be used inside joint definition");
          b=GetLinkBody($1,GetWorldLink(rw_lID,rw_world));
          GetPolyhedronDefiningPoint($3,rw_point[rw_i],b);
          rw_i++;
        }
      ;

pointp : point
       | '+''(' expr ',' expr ',' expr ')'
         {
           if (rw_i==0)
             Error("Incremental vector in a wrong position??");
           
           rw_point[rw_i][0]=rw_point[rw_i-1][0]+$3;
           rw_point[rw_i][1]=rw_point[rw_i-1][1]+$5;
           rw_point[rw_i][2]=rw_point[rw_i-1][2]+$7;
           rw_i++;
         }
       ;

point_list : '(' coordinates3D ')'
           ;

coordinates3D : xyz ';' coordinates3D
              | xyz
              ;

xyz :  expr ',' expr ',' expr
       {
         if (rw_i==rw_max)
           {
             MEM_DUP(rw_X,rw_max,double);
             MEM_EXPAND(rw_Y,rw_max,double);
             MEM_EXPAND(rw_Z,rw_max,double);
           }
         rw_X[rw_i]=$1;
         rw_Y[rw_i]=$3;
         rw_Z[rw_i]=$5;
         rw_i++;
       }


range : '[' expr ',' expr ']'
        {
          NewInterval($2,$4,&rw_range);
          if (EmptyInterval(&rw_range))
            ReadWorlderror("Empty range");
        }
        ;

revolute_joint: _REVOLUTE ':'
                any_id 
                { 
                  rw_lID1=rw_lID=GetWorldLinkID($3,rw_world);
                  if (rw_lID1==NO_UINT)
                    ReadWorlderror("Unkown link (1st id) when defining a revolute joint");

                  rw_i=0; 
                } 
                point pointp
                any_id
                { 
                  rw_lID2=rw_lID=GetWorldLinkID($7,rw_world);
                  if (rw_lID2==NO_UINT)
                    ReadWorlderror("Unkown link (2st id) when allowing a revolute joint");
                } 
                point pointp
                opt_revolute_range
                {
                  NewRevoluteJoint(GetWorldNJoints(rw_world),rw_r,
                                   rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                   rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                   rw_point,
                                   rw_has_limits,&rw_range,rw_range_point,
                                   rw_avoidLimits,rw_avoidLimitsWeight,NULL,
                                   &rw_joint);

                  AddJoint2World(&rw_joint,rw_world);

                  DeleteJoint(&rw_joint);
                  free($3);
                  free($7);

                  rw_prevolute=TRUE;
                }
                ;

crevolute_joint: _CREVOLUTE ':'
                 any_id 
                 { 
                   rw_lID1=rw_lID=GetWorldLinkID($3,rw_world);
                   if (rw_lID1==NO_UINT)
                     ReadWorlderror("Unkown link (1st id) when defining a revolute joint");
                   
                   rw_i=0; 
                 } 
                 point pointp
                 any_id 
                 { 
                   rw_lID2=rw_lID=GetWorldLinkID($7,rw_world);
                   if (rw_lID2==NO_UINT)
                     ReadWorlderror("Unkown link (2st id) when allowing a revolute joint");
                 } 
                 point pointp
                 opt_revolute_range
                 {
                   unsigned int id,k;
                   Tjoint *jp;

                   id=GetWorldNJoints(rw_world);

                   if ((!rw_prevolute)||(id<1))
                     ReadWorlderror("Coupled revolute joints need a previous revolute joint");
                   
                   id=GetWorldNJoints(rw_world);
                   k=id;
                   /* take into account that we can have a chain of coupled joints: determine the
                      original non-coupled joints (only revolute joints can be coupled) */
                   do {
                     k--;
                     jp=GetWorldJoint(k,rw_world);
                   } while ((k>0)&&(GetJointType(jp)==REV_JOINT)&&(CoupledWith(jp)!=NO_UINT));
                   if ((GetJointType(jp)!=REV_JOINT)||(CoupledWith(jp)!=NO_UINT))
                     Error("Error defining a chain of coupled revolute joints");

                   NewRevoluteJoint(id,rw_r,
                                    rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                    rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                    rw_point,
                                    rw_has_limits,&rw_range,rw_range_point,
                                    rw_avoidLimits,rw_avoidLimitsWeight,jp,
                                    &rw_joint);
                   
                   AddJoint2World(&rw_joint,rw_world);
                   
                   DeleteJoint(&rw_joint);
                   free($3);
                   free($7);

                   rw_prevolute=TRUE;
                 }
                 ;

spherical_joint : _SPHERICAL ':'
                  any_id
                  {
                    rw_lID1=rw_lID=GetWorldLinkID($3,rw_world);
                    if (rw_lID1==NO_UINT)
                      ReadWorlderror("Unkown link (1st id) when defining a spherical joint");

                    rw_i=0; 
                  } 
                  point 
                  any_id 
                  { 
                    rw_lID2=rw_lID=GetWorldLinkID($6,rw_world);
                    if (rw_lID2==NO_UINT)
                      ReadWorlderror("Unkown link (2st id) when allowing a spherical joint");
                    
                    /*We skip  points[1] since the (incremental) range definition
                      assumes the displacement is given w.r.t. points[2] */
                    rw_i++;
                  } 
                  point
                  opt_spherical_range
                  {
                    NewSphericalJoint(GetWorldNJoints(rw_world),
                                      rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                      rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                      rw_point,
                                      rw_has_limits,rw_angle,rw_range_point,
                                      rw_avoidLimits,rw_avoidLimitsWeight,
                                      &rw_joint);
                    
                    AddJoint2World(&rw_joint,rw_world);

                    DeleteJoint(&rw_joint);
                    free($3);
                    free($6);
                    
                    rw_prevolute=FALSE;
                  }
                  ;

universal_joint : _UNIVERSAL ':'
                  any_id 
                  { 
                    rw_lID1=rw_lID=GetWorldLinkID($3,rw_world);
                    if (rw_lID1==NO_UINT)
                      ReadWorlderror("Unkown link (1st id) when defining a universal joint");

                    rw_i=0; 
                  } 
                  point pointp
                  any_id
                  { 
                    rw_lID2=rw_lID=GetWorldLinkID($7,rw_world);

                    if (rw_lID2==NO_UINT)
                      ReadWorlderror("Unkown link (2nd id) when defining a universal joint");
                  } 
                  point pointp
                  opt_universal_range
                  {
                    NewUniversalJoint(GetWorldNJoints(rw_world),rw_r,
                                      rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                      rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                      rw_point,
                                      rw_has_limits,&rw_range0,&rw_range,rw_range_point,
                                      rw_avoidLimits,rw_avoidLimitsWeight,
                                      &rw_joint);
                    
                    AddJoint2World(&rw_joint,rw_world);
                    
                    DeleteJoint(&rw_joint);
                    free($3);
                    free($7);

                    rw_prevolute=FALSE;
                  }
                ;

sph_sph_joint: _SPH_SPH ':'
               any_id
               { 
                 rw_lID1=rw_lID=GetWorldLinkID($3,rw_world);
                 if (rw_lID1==NO_UINT)
                   ReadWorlderror("Unkown link (1st id) when defining a spherical-spherical joint");

                 rw_i=0; 
               } 
               point 
               any_id
               { 
                 rw_lID2=rw_lID=GetWorldLinkID($6,rw_world);
                 if (rw_lID2==NO_UINT)
                   ReadWorlderror("Unkown link (2st id) when defining a spherical-spherical joint");
               } 
               point
               _LENGTH expr
               _RADIUS expr
               link_color
               {
                 NewSphSphJoint(GetWorldNJoints(rw_world),
                                rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                rw_point,$10,$12,&rw_color_default,
                                &rw_joint);

                 AddJoint2World(&rw_joint,rw_world);

                 DeleteJoint(&rw_joint);
                 free($3);
                 free($6);

                 rw_prevolute=FALSE;
               }
             ;

sph_prs_sph_joint: _SPH_PRS_SPH ':'
                   any_id 
                   { 
                     rw_lID1=rw_lID=GetWorldLinkID($3,rw_world);
                     if (rw_lID1==NO_UINT)
                       ReadWorlderror("Unkown link (1st id) when defining a spherical-spherical joint");
                     
                     rw_i=0; 
                   } 
                   point 
                   any_id
                   { 
                     rw_lID2=rw_lID=GetWorldLinkID($6,rw_world);
                     if (rw_lID2==NO_UINT)
                       ReadWorlderror("Unkown link (2st id) when defining a spherical-spherical joint");
                   }  
                   point
                   _RANGE range
                   _RADIUS expr
                   link_color
                   {
                     NewSphPrsSphJoint(GetWorldNJoints(rw_world),
                                       rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                       rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                       rw_point,&rw_range,$12,&rw_color_default,
                                       &rw_joint);
                     
                     AddJoint2World(&rw_joint,rw_world);

                     DeleteJoint(&rw_joint);
                     free($3);
                     free($6);

                     rw_prevolute=FALSE;
                   }
                 ;

in_patch_joint : _IN_PATCH ':'
                 any_id 
                 { 
                   rw_lID1=rw_lID=GetWorldLinkID($3,rw_world);
                   if (rw_lID1==NO_UINT)
                     ReadWorlderror("Unkown link (1st id) when defining a in_patch joint");
                   
                   rw_i=0; 
                 } 
                 point pointp
                 {
                   /*save the points in a different location*/
                   unsigned int i,j;

                   for(i=0;i<2;i++)
                     {
                       for(j=0;j<3;j++)
                         rw_in_patch_point[i][j]=rw_point[i][j];
                     }

                   rw_i=0; 
                 }
                 any_id
                 {
                   rw_lID2=rw_lID=GetWorldLinkID($8,rw_world);
                   if (rw_lID2==NO_UINT)
                     ReadWorlderror("Unkown link (2st id) when defining a in_patch joint");
                 }
                 point point point point
                 avoid_limits
                 {
                   NewInPatchJoint(GetWorldNJoints(rw_world),
                                   rw_lID1,GetWorldLink(rw_lID1,rw_world),
                                   rw_lID2,GetWorldLink(rw_lID2,rw_world),
                                   rw_in_patch_point,rw_point,
                                   rw_avoidLimits,rw_avoidLimitsWeight,
                                   &rw_joint);
                   
                   AddJoint2World(&rw_joint,rw_world);
                   
                   DeleteJoint(&rw_joint);
                   free($3);
                   free($8);

                   rw_prevolute=FALSE;
                 }
               ;

rpointp : '(' expr ',' expr ',' expr ')'
          {
            rw_range_point[rw_i][0]=$2;
            rw_range_point[rw_i][1]=$4;
            rw_range_point[rw_i][2]=$6;
            rw_i++;
          }
        | '+' '(' expr ',' expr ',' expr ')'
          {
            rw_range_point[rw_i][0]=rw_point[2*rw_i][0]+$3;
            rw_range_point[rw_i][1]=rw_point[2*rw_i][1]+$5;
            rw_range_point[rw_i][2]=rw_point[2*rw_i][2]+$7;
            rw_i++;
          }
        ;

opt_revolute_range: _RANGE
                    {
                      rw_i=0;
                      rw_has_limits=TRUE;
                    } 
                    range 
                    rpointp rpointp
                    avoid_limits
                  |
                    {
                      rw_has_limits=FALSE;
                    }
                  ;

opt_spherical_range : _RANGE 
                      {
                        rw_i=0;
                        rw_has_limits=TRUE;
                      }
                      '[' expr ']'
                      {
                        rw_angle=$4;
                      }
                      rpointp rpointp
                      avoid_limits
                    |
                      {
                        rw_has_limits=FALSE;
                      }
                    ;

opt_universal_range : _RANGE
                      {
                        rw_i=0;
                        rw_has_limits=TRUE;
                      } 
                      range 
                      {
                        CopyInterval(&rw_range0,&rw_range);
                      }
                      rpointp 
                      range rpointp
                      avoid_limits
                    |
                      {
                        rw_has_limits=FALSE;
                      }
                    ;

obstacle_defs : _OBSTACLES obstacle_list
              ;

obstacle_list : obstacle_definition obstacle_list
              | 
              ;

obstacle_definition: _IDENTIFIER ':' shape
                     { 
                       if (GetConstantWithName($1,&rw_constants)!=NO_UINT)
                         ReadWorlderror("Obstacle with the name of a constant");

                       if (GetWorldLinkID($1,rw_world)!=NO_UINT)
                         ReadWorlderror("Obstacle with the name of a link");

                       if (GetWorldObstacleID($1,rw_world)!=NO_UINT)
                         ReadWorlderror("Duplicated obstacle");

                       AddObstacle2World($1,&rw_cbody,rw_world);

                       DeletePolyhedron(&rw_cbody);

                       free($1);
                     }

collision_defs : _COLLISIONS collision_list
               ;

collision_list : collision_definition collision_list
               | 
               ;

collision_definition: _CHECK ':' _ALL 
                      {
                        CheckAllCollisions(firstLink,firstObject,rw_world);
                      }
                    | _NO _CHECK ':' _ALL
                      {
                        NoCheckAllCollisions(firstLink,firstObject,rw_world);
                      }
                    | _CHECK ':' _SELFCOLLISIONS
                      {
                        CheckSelfCollisions(firstLink,rw_world);
                      }
                    |  _NO _CHECK ':' _SELFCOLLISIONS
                      {
                        NoCheckSelfCollisions(firstLink,rw_world);
                      }
                    | _CHECK ':'  any_id ',' any_id
                      {
                        unsigned int id1,id2;
                        
                        id1=GetWorldLinkID($3,rw_world);
                        if (id1==NO_UINT)
                          ReadWorlderror("Unkown link (1st id) when activating a collision check");

                        id2=GetWorldLinkID($5,rw_world);
                        if (id2==NO_UINT)
                          {
                            id2=GetWorldObstacleID($5,rw_world);
                            if (id2==NO_UINT)
                              ReadWorlderror("Unkown link/obstacle (2st id) when activating a collision check");

                            CheckLinkObstacleCollision(id1,id2,rw_world);
                          }
                        else
                          CheckLinkLinkCollision(id1,id2,rw_world);

                        free($3);
                        free($5);
                      }
                    | _NO _CHECK ':' any_id ',' any_id
                      {
                        unsigned int id1,id2;
                        
                        id1=GetWorldLinkID($4,rw_world);
                        if (id1==NO_UINT)
                          ReadWorlderror("Unkown link (1st id) when disabling a collision check");

                        id2=GetWorldLinkID($6,rw_world);
                        if (id2==NO_UINT)
                          {
                            id2=GetWorldObstacleID($6,rw_world);
                            if (id2==NO_UINT)
                              ReadWorlderror("Unkown link/obstacle (2st id) when disabling a collision check");

                            NoCheckLinkObstacleCollision(id1,id2,rw_world);
                          }
                        else
                          NoCheckLinkLinkCollision(id1,id2,rw_world);

                        free($4);
                        free($6);
                      }
                    ; 

avoid_limits : _AVOID _LIMITS opt_weight
               {
                 rw_avoidLimits=TRUE;
           
               }
             |
               {
                 rw_avoidLimits=FALSE;
               }
             ;

opt_weight : expr
             {
               rw_avoidLimitsWeight=$1;
             }
           |
             {
               rw_avoidLimitsWeight=1.0;
             }
           ;


included_defs : _INCLUDE include_list
              ;

include_list : include_file include_list
             |
             ;

include_file : _IDENTIFIER ':' _STRING
               {
                 Switch2File($1,rw_world->nl,rw_world->no,GetFilePath(rw_filename),$3);
                 free($1);
                 free($3);
               }   
             ; 

%%
/*
 *
 */
void InitWorldFromFile(Tparameters *p,Tfilename *filename,Tworld *w)
{
  unsigned int i;

  NEW(rw_point,4,double*);
  for(i=0;i<4;i++)
    NEW(rw_point[i],3,double);

  NEW(rw_range_point,4,double*);
  for(i=0;i<4;i++)
    NEW(rw_range_point[i],3,double);

  NEW(rw_in_patch_point,2,double*);
  for(i=0;i<2;i++)
    NEW(rw_in_patch_point[i],3,double);

  rw_r=(unsigned int)(GetParameter(CT_REPRESENTATION,p));
  
  InitWorld(w); /*Generate an empty world*/

  InitConstants(&rw_constants); /*An empty set of constants*/
  
  ReadWorldin=fopen(GetFileFullName(filename),"r");
  if (!ReadWorldin) 
    {
      char ErrorText[500];

      sprintf(ErrorText,"File %s does not exists",GetFileFullName(filename));
      Error(ErrorText);
    }

  /* Indicates whether the previous joint was revolute */
  rw_prevolute=FALSE;

  /*Reset the lines numbering*/
  RWline=1;
  firstLink=0;
  firstObject=0;

  /*we initalize the global pointer to make the parameters accesibles to any one inside the YACC module*/
  rw_world=w;
  rw_filename=filename;

  /*and process the file*/
  ReadWorldparse();
  
  DeleteConstants(&rw_constants); 

  GenerateWorldEquations(p,w);

  for(i=0;i<4;i++)
    free(rw_point[i]);
  free(rw_point);

  for(i=0;i<4;i++)
    free(rw_range_point[i]);
  free(rw_range_point);

  for(i=0;i<2;i++)
    free(rw_in_patch_point[i]);
  free(rw_in_patch_point);

  fclose(ReadWorldin);
}