Institut de Robòtica i Informàtica Industrial
KRD Group

The CuikSuite Project

plot3d.c

Go to the documentation of this file.
00001 #include "plot3d.h"
00002 #include "error.h"
00003 
00004 #include <stdlib.h>
00005 #include <string.h>
00006 #include <math.h>
00007 
00019 /* Documented but not included in the doxygen documentation because it
00020    screws up the IRI fomat. */
00030 #define CYLINDER_STRING "{INST geom { CBEZ223 2 1 0 1 1 0 0 1 0 2 1 1 1 1 1 0 1 1 2 0 1 1 0 1 0 0 1 0 0 1 1 0 0 1 1 0 0 1 1 0 0 1 1 } transforms{ = TLIST 0.5 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 0.5 0 0 0 0 0 1 0 0 -1 0 0 0 0 0 1 0.5 0 0 0 0 -1 0 0 0 0 -1 0 0 0 0 1 0.5 0 0 0 0 0 -1 0 0 1 0 0 0 0 0 1}}"
00031 
00034 void InitPlot3d(char *name,boolean axes,int argc, char **arg,Tplot3d *p)
00035 {
00036   char command[200];
00037   unsigned int i;
00038 
00039   if (name!=NULL)
00040     {
00041       p->f=fopen(name,"w");
00042 
00043       if (!p->f)
00044         Error("Can not create output file in InitPlot3d");
00045     }
00046   else
00047     p->f=stdout;
00048 
00049   //start the ooglfile
00050   if (argc>0)
00051     {
00052       fprintf(p->f,"#\n# Geomview plot generated with the following command line:\n#      ");
00053       for(i=0;i<(unsigned int)argc;i++)
00054         fprintf(p->f,"%s ",arg[i]);
00055       fprintf(p->f,"\n#\n");
00056     }
00057 
00058   fprintf(p->f,"(progn ");
00059   fprintf(p->f,"(normalization World none)\n");  
00060   fprintf(p->f,"(bbox-draw World no)\n");
00061 
00062   /*do not show the main pannel of geomview*/
00063   /*fprintf(p->f,"(ui-panel geomview off)\n");*/
00064 
00065   /*show the tools pannel of geomview*/
00066   /*fprintf(p->f,"(ui-panel tools on)\n");*/
00067 
00068   fprintf(p->f,"(backcolor World 1.0 1.0 1.0)\n"); /*background color de fons*/
00069 
00070   fprintf(p->f,"(dice World 15.0)\n"); /*granularity of curbed objects*/
00071   fprintf(p->f," )\n");
00072 
00073 
00074   if (name==NULL)
00075     {
00076       NEW(p->fileName,7,char);
00077       strcpy(p->fileName,"stdout");
00078     }
00079   else
00080     {
00081       i=strlen(name)+1;
00082       NEW(p->fileName,i,char);
00083       strcpy(p->fileName,name);
00084       i=0;
00085       while (p->fileName[i]!=0)
00086         {
00087           if ((p->fileName[i]=='/')||(p->fileName[i]=='.'))
00088             p->fileName[i]='_';
00089           i++;
00090         }
00091     }
00092 
00093   if (axes)
00094     {
00095       //axes (we assign them identifier 0)
00096       fprintf(p->f,"(geometry sol_%s_0 ",p->fileName);
00097       if (p->f!=stdout) fclose(p->f);
00098 
00099       if (p->f==stdout)
00100         sprintf(command,"cat %s/share/axes.list",_CUIK_SUITE_MAIN_DIR);
00101       else
00102         sprintf(command,"cat %s/share/axes.list >> %s",_CUIK_SUITE_MAIN_DIR,name);
00103       
00104       system(command);
00105 
00106       if (p->f!=stdout) p->f=fopen(name,"a");
00107       fprintf(p->f," )\n");
00108     }
00109 
00110   p->nobj=0; /* Last object added to the scene */
00111 
00112   NewColor(DCP3D_R,DCP3D_G,DCP3D_G,&(p->color));
00113   
00114   p->inObject=FALSE;
00115 }
00116 
00117 void Start3dBlock(Tplot3d *p)
00118 {
00119   fprintf(p->f,"(progn \n");
00120 }
00121 
00122 void Close3dBlock(Tplot3d *p)
00123 {
00124   fprintf(p->f,")\n");
00125   fflush(p->f);
00126 }
00127 
00128 void  Delete3dObject(unsigned int nobj,Tplot3d *p)
00129 {
00130   fprintf(p->f,"(delete sol_%s_%u )\n",p->fileName,nobj);
00131 }
00132 
00133 unsigned int StartNew3dObject(Tcolor *c,Tplot3d *p)
00134 {
00135   Close3dObject(p);
00136 
00137   p->inObject=TRUE;
00138   CopyColor(&(p->color),c);
00139 
00140   p->nobj++; /* We increas the object identifier (empty objects also use an identifier although they are never created) */
00141 
00142   fprintf(p->f,"(geometry sol_%s_%u {LIST \n",p->fileName,p->nobj); 
00143 
00144   return(p->nobj); /*Return the identifier that will be assigned to the new object*/
00145 }
00146 
00147 void Close3dObject(Tplot3d *p)
00148 {
00149   if (p->inObject)
00150     {
00151       /*close the previous appearance and object, if any*/
00152       fprintf(p->f,"})\n"); 
00153       fprintf(p->f,"(merge-ap sol_%s_%u {shading flat})\n",p->fileName,p->nobj);
00154       
00155       p->inObject=FALSE;
00156       SetColor3dObject(p->nobj,&(p->color),p);
00157     }
00158 }
00159 
00160 void SetColor3dObject(unsigned int nobj,Tcolor *c,Tplot3d *p)
00161 {
00162   if (!p->inObject)
00163     {
00164       fprintf(p->f,"(merge-ap sol_%s_%u {material {kd 0.75 ",p->fileName,nobj);
00165       fprintf(p->f," ambient ");PrintColor(p->f,c);
00166       fprintf(p->f," *diffuse ");PrintColor(p->f,c);
00167       fprintf(p->f," edgecolor ");PrintColor(p->f,c);
00168       fprintf(p->f,"}})\n");
00169     }
00170 }
00171 
00172 void Delay3dObject(double t,Tplot3d *p)
00173 {
00174   if (t!=0.0)
00175     fprintf(p->f,"(sleep-for %f)\n",t);
00176 }
00177 
00178 void Move3dObject(unsigned int nobj,THTransform *t,Tplot3d *p)
00179 { 
00180   fprintf(p->f,"(xform-set sol_%s_%u {",p->fileName,nobj);
00181   HTransformPrintT(p->f,t);
00182   fprintf(p->f,"})\n");
00183 }
00184 
00185 void PlotBox3d(double min_x,double max_x,
00186                double min_y,double max_y,
00187                double min_z,double max_z,
00188                Tplot3d *p)
00189 {
00190   if (!p->inObject)
00191     Error("Adding geometry to a non-existing object");
00192 
00193   fprintf(p->f,"{OFF 8 6 1\n");
00194  
00195   /*vertices*/
00196   fprintf(p->f,"%f %f %f\n",min_x,min_y,min_z);
00197   fprintf(p->f,"%f %f %f\n",min_x,max_y,min_z);
00198   fprintf(p->f,"%f %f %f\n",max_x,max_y,min_z);
00199   fprintf(p->f,"%f %f %f\n",max_x,min_y,min_z);
00200 
00201   fprintf(p->f,"%f %f %f\n",min_x,min_y,max_z);
00202   fprintf(p->f,"%f %f %f\n",min_x,max_y,max_z);
00203   fprintf(p->f,"%f %f %f\n",max_x,max_y,max_z);
00204   fprintf(p->f,"%f %f %f\n",max_x,min_y,max_z);
00205 
00206   /*faces*/
00207   fprintf(p->f,"4 3 2 1 0\n");
00208   fprintf(p->f,"4 4 5 6 7\n");
00209   fprintf(p->f,"4 0 1 5 4\n"); 
00210   fprintf(p->f,"4 2 3 7 6\n"); 
00211   fprintf(p->f,"4 3 0 4 7\n");
00212   fprintf(p->f,"4 1 2 6 5}\n"); 
00213 
00214   fflush(p->f);
00215 }
00216 
00217 void PlotTriangle3d(double x1,double y1,double z1,
00218                     double x2,double y2,double z2,
00219                     double x3,double y3,double z3,
00220                     Tplot3d *p)
00221 {
00222    if (!p->inObject)
00223     Error("Adding geometry to a non-existing object");
00224 
00225   /*vertices*/
00226   fprintf(p->f,"{OFF 3 1 1\n");
00227   fprintf(p->f,"%f %f %f\n",x1,y1,z1);
00228   fprintf(p->f,"%f %f %f\n",x2,y2,z2);
00229   fprintf(p->f,"%f %f %f\n",x3,y3,z3);
00230   
00231   /*faces*/
00232   fprintf(p->f,"3 0 1 2}\n"); 
00233 
00234   fflush(p->f);
00235 }
00236 
00237 void Plot3dObject(unsigned int nv,unsigned int nf,unsigned int ne,
00238                   double **v,
00239                   unsigned int *nvf,
00240                   unsigned int **fv,Tplot3d *p)
00241 {
00242   unsigned int i,j;
00243 
00244   if (!p->inObject)
00245     Error("Adding geometry to a non-existing object");
00246 
00247   fprintf(p->f,"OFF\n");
00248   fprintf(p->f,"%u %u %u\n",nv,nf,ne);
00249   fprintf(p->f,"\n");
00250   for(i=0;i<nv;i++)
00251     fprintf(p->f,"%f %f %f\n",v[i][0],v[i][1],v[i][2]);
00252 
00253   fprintf(p->f,"\n");
00254   for(i=0;i<nf;i++)
00255     {
00256       fprintf(p->f,"%u   ",nvf[i]);
00257       for(j=0;j<nvf[i];j++)
00258         fprintf(p->f,"%u ",fv[i][j]);
00259 
00260       fprintf(p->f,"\n");
00261     }
00262   fprintf(p->f,"\n");
00263 }
00264 
00265 void PlotSphere(double r,double x,double y,double z,Tplot3d *p)
00266 {
00267   if (!p->inObject)
00268     Error("Adding geometry to a non-existing object");
00269  
00270   fprintf(p->f,"{SPHERE %f %f %f %f}\n",r,x,y,z);
00271 }
00272 
00273 void PlotCylinder(double r,double *p1,double *p2,Tplot3d *p)
00274 {
00275   THTransform f;
00276 
00277   if (!p->inObject)
00278     Error("Adding geometry to a non-existing object");
00279 
00280   HTransformX2Vect(r,r,p1,p2,&f);
00281 
00282   fprintf(p->f,"{INST\n");
00283   fprintf(p->f,"  geom       %s \n",CYLINDER_STRING);
00284   fprintf(p->f,"  transform ");
00285   HTransformPrintT(p->f,&f);
00286   fprintf(p->f,"}\n");
00287   
00288   HTransformDelete(&f);
00289 }
00290 
00291 void PlotVect3d(unsigned int n,
00292                 double *x,double *y,double *z,
00293                 Tplot3d *p)
00294 {
00295   unsigned int i;
00296 
00297   if (!p->inObject)
00298     Error("Adding geometry to a non-existing object");
00299  
00300   fprintf(p->f,"{VECT 1 %u 1 \n   %u \n   1\n",n,n);
00301 
00302   for(i=0;i<n;i++)
00303     fprintf(p->f,"   %f %f %f\n",x[i],y[i],z[i]);
00304 
00305   PrintColor(p->f,&(p->color));
00306   fprintf(p->f," 0}\n"); /*alpha component is 0*/
00307 
00308   fflush(p->f);
00309   
00310 }
00311 
00312 void Take3dSnapshot(char *name,Tplot3d *p)
00313 {
00314   /*fprintf(p->f,"(snapshot Camera \"| convert - %s.gif\")\n",name);*/
00315   fprintf(p->f,"(snapshot Camera \"%s.ppm\")\n",name);
00316 }
00317 
00318 void ClosePlot3d(boolean quit,double average_x,double average_y,double average_z,Tplot3d *p)
00319 {
00320   unsigned int i;
00321 
00322   Close3dObject(p);
00323 
00324   /*translate the objects*/
00325   if ((average_x!=0.0)&&(average_y!=0.0)&&(average_z!=0.0))
00326     {
00327       /* Object 0 (reserved for the global axes, if any) are not plotted */
00328       for(i=1;i<=p->nobj;i++)
00329         fprintf(p->f,"(xform-set sol_%s_%u {1 0 0 0 0 1 0 0 0 0 1 0 %f %f %f 1})\n",p->fileName,i,average_x,average_y,average_z);
00330     }
00331 
00332   if (quit)
00333     fprintf(p->f,"(quit)\n");
00334 
00335   if (p->f!=stdout) fclose(p->f);
00336   
00337   free(p->fileName);
00338 }