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; 

  /*Lex and Yacc functions*/
  int ReadWorldlex(void); 

  /*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;

  double **rw_point;
  unsigned int rw_i;
  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;
  Tcpolyhedron rw_cbody;

  Tconstants rw_constants;

  boolean rw_avoidLimits;
  double rw_avoidLimitsWeight;
%}

%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 _BODY _GRANULARITY _FIX _ID _TX _TY _TZ _TXYZ _RX _RY _RZ _PRISMATIC _REVOLUTE _SPHERICAL _UNIVERSAL _SPH_SPH _IN_PATCH _SPHERE _CYLINDER _LENGTH _RADIUS _SELFCOLLISIONS _RANGE _COLOR _RED _GREEN _BLUE _PURPLE _CYAN _YELLOW _WHITE _BLACK _HIDDEN _AVOID _LIMITS

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

%type <int_number> granularity
%type <real_number> expr
%type <color> color

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

world: constant_defs
       link_defs 
       joint_defs 
      

constant_defs: _CONSTANTS constant_list
             |
             ;

constant_list : constant_definition constant_list
              | constant_definition
              ;

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);
                      }
                    ;

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);
       }
     | _IDENTIFIER
       {
         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
          ;

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_world);
                   DeleteLink(&rw_link);
                 }
                 ;


opt_link_definition : ':' link_color link_shapes
                    |
                    ;

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

shape: _BODY _STRING body_color granularity
       {
         Tfilename fb;

         CreateFileName(GetFilePath(rw_filename),$2,NULL,NULL,&fb);
         
         InitCPolyhedronFromFile(GetFileFullName(&fb),&rw_color_body,
                                 $4,&rw_cbody);

         DeleteFileName(&fb);
         free($2);
       }
     | _SPHERE  expr '(' expr ',' expr ',' expr ')' 
       body_color granularity
       {
         double c[3];
         
         c[0]=$4;
         c[1]=$6;
         c[2]=$8;

         NewSphere($2,c,&rw_color_body,$11,&rw_cbody);
       }
     | _CYLINDER  expr '(' expr ',' expr ',' expr ')' '(' expr ',' expr ',' expr ')' 
       body_color granularity
       {
         double p1[3],p2[3];
         
         p1[0]=$4;
         p1[1]=$6;
         p1[2]=$8;
         
         p2[0]=$11;
         p2[1]=$13;
         p2[2]=$15;
         
         NewCylinder($2,p1,p2,&rw_color_body,$18,&rw_cbody);
       }
       ;

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

            ;

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;
        }
      | _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
           ;

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

fix_joint : _FIX  ':' _IDENTIFIER  _IDENTIFIER 
            {
              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);
            }
          ;

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 ':'
                 _IDENTIFIER 
                  { 
                    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
                 _IDENTIFIER
                 { 
                   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);
                 }
                 ;

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
        {
          Tcpolyhedron *b;
          
          b=GetLinkBody(0,GetWorldLink(rw_lID,rw_world));
          GetCPolyhedronDefiningPoint($1,rw_point[rw_i],b);
          rw_i++;
        }
      | _INTEGER ':' _INTEGER
        {
          Tcpolyhedron *b;
          
          b=GetLinkBody($1,GetWorldLink(rw_lID,rw_world));
          GetCPolyhedronDefiningPoint($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++;
         }
       ;

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

revolute_joint: _REVOLUTE ':'
                _IDENTIFIER 
                { 
                  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
                _IDENTIFIER 
                { 
                  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_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,
                                   &rw_joint);

                  AddJoint2World(&rw_joint,rw_world);

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

spherical_joint : _SPHERICAL ':'
                  _IDENTIFIER 
                  {
                    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 
                  _IDENTIFIER 
                  { 
                    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);
                }
                ;

universal_joint : _UNIVERSAL ':'
                  _IDENTIFIER 
                  { 
                    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
                  _IDENTIFIER 
                  { 
                    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_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);
                  }
                ;

sph_sph_joint: _SPH_SPH ':'
               _IDENTIFIER 
               { 
                 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 
               _IDENTIFIER 
               { 
                 rw_lID2=rw_lID=GetWorldLinkID($6,rw_world);
                 if (rw_lID2==NO_UINT)
                   ReadWorlderror("Unkown link (2st id) when allowing 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);
               }
             ;

in_patch_joint : _IN_PATCH ':'
                 _IDENTIFIER  
                 { 
                   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; 
                 }
                 _IDENTIFIER 
                 {
                   rw_lID2=rw_lID=GetWorldLinkID($8,rw_world);
                   if (rw_lID2==NO_UINT)
                     ReadWorlderror("Unkown link (2st id) when allowing a spherical-spherical 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);
                 }
               ;

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;
                      }
                    ;

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

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


%%
/*
 *
 */
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);
  
  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);
    }

  /*Reset the lines numbering*/
  RWline=1;

  /*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); 

  w->stage=WORLD_WITH_GEOMETRY;

  GenerateWorldEquationSystems(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);
}