Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

renderpfnew.C

Go to the documentation of this file.
00001 //----------------------------------------------------------------------
00002 //               The Motion Strategy Library (MSL)
00003 //----------------------------------------------------------------------
00004 //
00005 // Copyright (c) 1998-2000 Iowa State University and Steve LaValle.  
00006 // All Rights Reserved.
00007 // 
00008 // Permission to use, copy, and distribute this software and its 
00009 // documentation is hereby granted free of charge, provided that 
00010 // (1) it is not a component of a commercial product, and 
00011 // (2) this notice appears in all copies of the software and
00012 //     related documentation. 
00013 // 
00014 // Iowa State University and the author make no representations
00015 // about the suitability or fitness of this software for any purpose.  
00016 // It is provided "as is" without express or implied warranty.
00017 //----------------------------------------------------------------------
00018 
00019 #include <math.h>
00020 #include <stdlib.h>
00021 
00022 #include "3Dtriangle.h"
00023 #include "geom.h"
00024 #include "model.h"
00025 #include "problem.h"
00026 #include "scene.h"
00027 #include "render.h"
00028 #include "renderpf.h"
00029 #include "defs.h"
00030 
00031 #include <LEDA/REDEFINE_NAMES.h>
00032 #undef set  // This leda class conflicts with Performer
00033 
00034 #define CONTROLPANEL_CENTER_X -0.22
00035 #define CONTROLPANEL_CENTER_Z -0.37
00036 #define CONTROLPANEL_WIDTH 0.2
00037 #define CONTROLPANEL_HEIGHT 0.1
00038 #define CONTROLPANEL_BAR_WIDTH 0.18
00039 #define CONTROLPANEL_BAR_HEIGHT 0.005
00040 #define CONTROLPAD_WIDTH 0.01
00041 #define CONTROLPAD_HEIGHT 0.02
00042 
00043 static float RGBRed[] =   {0xff,0x00,0xff,0xff,0x00};
00044 static float RGBGreen[] = {0x00,0xff,0xff,0x69,0x00};
00045 static float RGBBlue[] =  {0x00,0xff,0x00,0xb4,0xff};
00046 
00047 
00049 //
00050 // RenderPerformer Class
00051 //
00053 
00054 SharedData* RenderPerformer::Shared;
00055 
00056 RenderPerformer::RenderPerformer(): Render()
00057 {
00058 
00059 }
00060 
00061 
00062 RenderPerformer::RenderPerformer(string filepath=""): Render(filepath)
00063 {
00064 
00065 }
00066 
00067 
00068 RenderPerformer::RenderPerformer(Scene *s, 
00069                                  string filepath): Render(s,filepath)
00070 {
00071 
00072 }
00073 
00074 
00075 RenderPerformer::~RenderPerformer()
00076 {
00077 
00078 }
00079 
00080 
00081 void RenderPerformer::InitPerformer()
00082 {
00083   // Set view point orientation matrix
00084   pfMatrix camera_trans, camera_rot, camera_rot2;
00085   double theta = 0.0;
00086 
00087   camera_trans.makeIdent();
00088   camera_rot.makeIdent();
00089   theta = asin(sqrt(sqr(S->ViewingDirection[0])+
00090                     sqr(S->ViewingDirection[2])));
00091 
00092   if(fabs(theta) > 0.00001){
00093     camera_rot.makeRot(theta*180.0/PI,
00094                        S->ViewingDirection[2],
00095                        0.0,
00096                        (-1.0)*S->ViewingDirection[0]);
00097   }
00098   camera_rot2.postRot(camera_rot,
00099                       S->ViewingOrientation*180.0/PI,
00100                       S->ViewingDirection[0],
00101                       S->ViewingDirection[1],
00102                       S->ViewingDirection[2]);
00103   
00104   camera_trans.postTrans(camera_rot2,
00105                          S->ViewingPosition[0], 
00106                          S->ViewingPosition[1],
00107                          S->ViewingPosition[2]);
00108 
00109   // Initialize Performer
00110   pfInit();
00111     
00112   // Use default multiprocessing mode based on number of processors.
00113   pfMultiprocess( PFMP_DEFAULT );
00114 
00115   // allocate shared before fork()'ing parallel processes  
00116   Shared = (SharedData*)pfMalloc(sizeof(SharedData), pfGetSharedArena());
00117  
00118   // Set Performer Search Path
00119   pfFilePath(FilePath);
00120 
00121   // Load all loader DSO's before pfConfig() forks 
00122   LoadConverterDSO();
00123 
00124   // initiate multi-processing mode set in pfMultiprocess call 
00125   // FORKs for Performer processes,  CULL and DRAW, etc. happen here.
00126   pfConfig();                   
00127   
00128   // Initiate performer utility library
00129   pfuInit();
00130 
00131   // Create Mouse and InputEvents
00132   Shared->Mouse = new pfuMouse;
00133   Shared->InputEvents = new pfuEventStream;
00134     
00135   // Create a new pfScene
00136   pfScene *PerfScene = new pfScene;
00137 
00138   // Load Environment and attach it to scene
00139   Shared->ShowCase = new pfSwitch;
00140   Shared->ControlPanel = new pfSCS(camera_rot2);
00141   Shared->ControlPad = new pfDCS;
00142   Shared->WorldDCS = new pfDCS;
00143   Shared->WorkEnv = new pfSwitch;
00144   Shared->Env = new pfGroup;
00145   Shared->BodiesDCS = new pfDCS;
00146   Shared->BoundBox = new pfGeode;
00147   LoadEnvironment(Shared->Env);
00148   LoadBoundBox(Shared->BoundBox);
00149   LoadBodies(Shared->BodiesDCS);
00150   LoadControlPanel(Shared->ControlPanel, Shared->ControlPad);
00151   Shared->ControlPanel->addChild(Shared->ControlPad);
00152   Shared->WorkEnv->addChild(Shared->Env);
00153   Shared->WorkEnv->addChild(Shared->BoundBox);
00154   Shared->WorldDCS->addChild(Shared->WorkEnv);
00155   Shared->WorldDCS->addChild(Shared->BodiesDCS);
00156   Shared->ShowCase->addChild(Shared->WorldDCS);
00157   Shared->ShowCase->addChild(Shared->ControlPanel);  
00158   PerfScene->addChild(Shared->ShowCase);
00159 
00160   // Create a pfLightSource in "south-west" and attach it to scene
00161   pfLightSource *sun = new pfLightSource;
00162   sun->setPos(-0.3f, -0.3f, 1.0f, 0.0f);
00163   PerfScene->addChild(sun);
00164  
00165   // Configure and open GL window
00166   pfPipe *Pipe = pfGetPipe(0);
00167   Shared->PW = new pfPipeWindow(Pipe);
00168   Shared->PW->setWinType(PFPWIN_TYPE_X);
00169   Shared->PW->setName("Motion Strategy Library: IRIS Performer Window");
00170   Shared->PW->setOriginSize(0,0,500,500);
00171   Shared->PW->open();
00172   
00173   // Create and configure a pfChannel.
00174   pfChannel *chan = new pfChannel(Pipe);
00175   chan->setScene(PerfScene);
00176   chan->setFOV(45.0f, 45.0f);
00177   chan->setNearFar(1.0f, 1000.0f);
00178   chan->setTravFunc(PFTRAV_DRAW, DrawChannel);
00179   chan->setViewport(0.0, 1.0, 0, 1.0);
00180   chan->setShare(PFCHAN_FOV | PFCHAN_NEARFAR
00181                  | PFCHAN_EARTHSKY 
00182                  | PFCHAN_STRESS 
00183                  | PFCHAN_LOD | PFCHAN_SWAPBUFFERS
00184                  | PFCHAN_SWAPBUFFERS_HW);
00185   Shared->Chan = chan;
00186 
00187   // Set Chan view point   
00188   //Shared->Chan->setViewMat(camera_trans);
00189 
00190   // EarthSky
00191   pfEarthSky *esky = new pfEarthSky();
00192   esky->setMode(PFES_BUFFER_CLEAR, PFES_SKY_GRND);
00193   esky->setAttr(PFES_GRND_HT, -1000.0f);
00194   esky->setColor(PFES_GRND_FAR, 0.3f, 0.1f, 0.0f, 1.0f);
00195   esky->setColor(PFES_GRND_NEAR, 0.5f, 0.3f, 0.1f, 1.0f);
00196   chan->setESky(esky);
00197 
00198   // Initilize the slave channel
00199   /*pfChannel *chaneye = new pfChannel(Pipe);
00200   pfScene *eyescene = new pfScene;
00201   pfMatrix m;
00202   m.makeIdent();
00203   pfSCS *eyeworld = new pfSCS(m); 
00204   
00205   eyeworld->addChild(Shared->EnvDCS);
00206   eyeworld->addChild(Shared->BodiesDCS);
00207   eyescene->addChild(eyeworld);
00208 
00209   chaneye->setScene(eyescene);
00210   chan->attach(chaneye);
00211   chaneye->setViewport(0.0, 1.0/4.0, 3.0/4.0, 1.0);
00212   Shared->ChanEye = chaneye;
00213   PW->removeChan(ChanEye); // remove chanslaver as default
00214   */
00215 
00216   // Initilize input device
00217   pfuInitInput(Shared->PW,PFUINPUT_X);
00218 
00219   // Get frame rate and set it to FrameTime
00220   FrameTime = (double) (1.0/pfGetFrameRate());
00221   //cout << "FrameTime: " << FrameTime << endl;
00222 }
00223 
00224 
00225 void RenderPerformer::LoadConverterDSO()
00226 {
00227   string object;
00228   forall(object, EnvList) {
00229     if( !strcmp(object,"Obst"))
00230       pfdInitConverter(object);
00231   }
00232 
00233   forall(object, BodyList) {
00234     if( !strcmp(object,"Robot"))
00235       pfdInitConverter(object);
00236   }
00237 }
00238 
00239 
00240 string RenderPerformer::GetExtensionSuffix(string s){
00241   string suffix;
00242   int i, num=0;
00243 
00244   for(i=0; i<s.length(); i++){
00245     if(s[i] == '.')
00246       num = i;
00247   }
00248 
00249   if(num <= s.length() - 1){
00250     for(i=num; i<s.length(); i++){
00251       suffix[i-num] = s[i];
00252     }
00253     //siffix[s.length()] = '\n';
00254     //cout << s << " Extension got: " << suffix << " Length:" << suffix.length() << endl;
00255     return suffix;
00256   }
00257 
00258   return suffix;
00259 }
00260 
00261 
00262 
00263 void RenderPerformer::LoadEnvironment(pfGroup *env){
00264   string object;
00265   pfNode *root;
00266 
00267   forall(object, EnvList){
00268     // Append to Performer search path
00269     if (GetExtensionSuffix(object).length() == 0) {
00270       root = LoadNativeModel(1.0, 0.0, 1.0, object);
00271     }
00272     else{
00273       pfFilePath(FilePath);
00274       
00275       cout << "Loading " << object << "\n";
00276       root = pfdLoadFile(object);
00277       if (root == NULL)
00278         {
00279           pfExit();
00280           exit(-1);
00281         }
00282     }
00283     env->addChild(root);    
00284   }
00285 
00286   // Draw bounding box
00287 
00288 
00289 
00290 
00291 }
00292 
00293 
00294 void RenderPerformer::LoadBodies(pfGroup *bodies){
00295   string object;
00296   pfNode *root;
00297   int i;
00298 
00299   i = 0;
00300   forall(object, BodyList) {
00301     if (GetExtensionSuffix(object).length() == 0) {
00302       root = LoadNativeModel(RGBRed[i%4], RGBGreen[i%4], RGBBlue[i%4], 
00303                             object);
00304     }
00305     else {
00306       // Append to Performer search path
00307       pfFilePath(FilePath);
00308       
00309       cout << "Loading " << object << "\n";
00310       root = pfdLoadFile(object);
00311       if (root == NULL)
00312         {
00313           pfExit();
00314           exit(-1);
00315         }
00316     }
00317     pfDCS *piece = new pfDCS;
00318     piece->addChild(root);
00319     bodies->addChild(piece);    
00320 
00321     i++;
00322   }
00323 }
00324 
00325 
00326 
00327 pfNode* RenderPerformer::LoadNativeModel(double r, double g, double b,  
00328                                          string file)
00329 {
00330   pfGeode *root = new pfGeode;
00331   pfGeoSet *gset = new pfGeoSet;
00332   list<Triangle> trlist;
00333   list<polygon> plist;
00334   int i, num;
00335   Triangle t;
00336   double d1[3],d2[3], n[3];
00337 
00338   file_istream fin(FilePath + file);
00339   if (S->GeomDim == 2) {
00340     fin >> plist;
00341     trlist = PolygonsToTriangles(plist,5.0); // Defined in 3Dtriangle.C
00342   }
00343   else
00344     fin >> trlist;
00345   fin.close();
00346 
00347   num = trlist.length();
00348   //cout << "number of triangles: " << num << endl; 
00349   pfVec3 *tri = new pfVec3[3*num];
00350   pfVec3 *norm = new pfVec3[num];
00351   pfVec4 *color = new pfVec4[1];
00352 
00353   for (i=0; i<num; i++) {
00354     t = trlist.contents(trlist.get_item(i));
00355     tri[3*i].set((float)t.p1.xcoord(), (float)t.p1.ycoord(), (float)t.p1.zcoord());
00356     tri[3*i+1].set((float)t.p2.xcoord(), (float)t.p2.ycoord(), (float)t.p2.zcoord());
00357     tri[3*i+2].set((float)t.p3.xcoord(), (float)t.p3.ycoord(), (float)t.p3.zcoord());
00358 
00359     d1[0] = t.p2.xcoord() - t.p1.xcoord();
00360     d1[1] = t.p2.ycoord() - t.p1.ycoord();
00361     d1[2] = t.p2.zcoord() - t.p1.zcoord();
00362     d2[0] = t.p3.xcoord() - t.p1.xcoord();
00363     d2[1] = t.p3.ycoord() - t.p1.ycoord();
00364     d2[2] = t.p3.zcoord() - t.p1.zcoord();
00365     NormCrossProduct(d1, d2, n);
00366     norm[i].set(n[0], n[1], n[2]);
00367   }  
00368   color[0].set( r, g, b, 0.0f );
00369 
00370   gset->setAttr(PFGS_COORD3, PFGS_PER_VERTEX, tri, NULL);
00371   gset->setAttr(PFGS_NORMAL3, PFGS_PER_PRIM, norm, NULL);
00372   gset->setAttr(PFGS_COLOR4, PFGS_OVERALL, color, NULL);
00373   gset->setPrimType(PFGS_TRIS);
00374   gset->setNumPrims(num);
00375 
00376   //Specify Geometry State
00377   pfGeoState *gstate = new pfGeoState;
00378   gstate->setMode(PFSTATE_ENLIGHTING, PF_ON);
00379   gstate->setMode(PFSTATE_CULLFACE, PF_OFF);
00380   //gstate->setMode(PFSTATE_SHADEMODEL, PFSM_GOURAUD);
00381 
00382   // Using material
00383   pfMaterial *mat = new pfMaterial;
00384   mat->setColor(PFMTL_AMBIENT, 0.0f, 0.0f, 0.0f);
00385   mat->setColor(PFMTL_SPECULAR, 0.5f, 0.5f, 0.5f);
00386   mat->setShininess(10.0f);
00387   mat->setColorMode(PFMTL_BOTH, PFMTL_CMODE_AD);
00388   //mat->apply();
00389 
00390   gstate->setAttr(PFSTATE_FRONTMTL, mat);
00391   gset->setGState(gstate);
00392 
00393   //gset->setDrawMode(PFGS_FLATSHADE, PF_ON);
00394 
00395   root->addGSet(gset);
00396   
00397   return root;
00398 }
00399 
00400 
00401 void RenderPerformer::LoadBoundBox(pfGeode *bound){
00402   pfGeoSet *gset = new pfGeoSet;
00403   pfVec3 *ver = new pfVec3[24];
00404   pfVec3 *norm = new pfVec3[12];
00405   pfVec4 *color = new pfVec4[1];
00406 
00407   ver[0].set(S->LowerWorld[0], S->LowerWorld[1], S->LowerWorld[2]);
00408   ver[1].set(S->UpperWorld[0], S->LowerWorld[1], S->LowerWorld[2]);
00409   norm[0].set(0.0,1.0,0.0);
00410 
00411   ver[2].set(S->LowerWorld[0], S->LowerWorld[1], S->LowerWorld[2]);
00412   ver[3].set(S->LowerWorld[0], S->UpperWorld[1], S->LowerWorld[2]);
00413   norm[1].set(0.0,1.0,0.0);
00414 
00415   ver[4].set(S->LowerWorld[0], S->LowerWorld[1], S->LowerWorld[2]);
00416   ver[5].set(S->LowerWorld[0], S->LowerWorld[1], S->UpperWorld[2]);
00417   norm[2].set(0.0,1.0,0.0);
00418 
00419   ver[6].set(S->UpperWorld[0], S->LowerWorld[1], S->LowerWorld[2]);
00420   ver[7].set(S->UpperWorld[0], S->UpperWorld[1], S->LowerWorld[2]);
00421   norm[3].set(0.0,1.0,0.0);
00422 
00423   ver[8].set(S->UpperWorld[0], S->LowerWorld[1], S->LowerWorld[2]);
00424   ver[9].set(S->UpperWorld[0], S->LowerWorld[1], S->UpperWorld[2]);
00425   norm[4].set(0.0,1.0,0.0);
00426 
00427   ver[10].set(S->LowerWorld[0], S->UpperWorld[1], S->LowerWorld[2]);
00428   ver[11].set(S->UpperWorld[0], S->UpperWorld[1], S->LowerWorld[2]);
00429   norm[5].set(0.0,1.0,0.0);
00430 
00431   ver[12].set(S->LowerWorld[0], S->UpperWorld[1], S->LowerWorld[2]);
00432   ver[13].set(S->LowerWorld[0], S->UpperWorld[1], S->UpperWorld[2]);
00433   norm[6].set(0.0,1.0,0.0);
00434 
00435   ver[14].set(S->LowerWorld[0], S->LowerWorld[1], S->UpperWorld[2]);
00436   ver[15].set(S->UpperWorld[0], S->LowerWorld[1], S->UpperWorld[2]);
00437   norm[7].set(0.0,1.0,0.0);
00438 
00439   ver[16].set(S->LowerWorld[0], S->LowerWorld[1], S->UpperWorld[2]);
00440   ver[17].set(S->LowerWorld[0], S->UpperWorld[1], S->UpperWorld[2]);
00441   norm[8].set(0.0,1.0,0.0);
00442 
00443   ver[18].set(S->UpperWorld[0], S->UpperWorld[1], S->UpperWorld[2]);
00444   ver[19].set(S->LowerWorld[0], S->UpperWorld[1], S->UpperWorld[2]);
00445   norm[9].set(0.0,1.0,0.0);
00446 
00447   ver[20].set(S->UpperWorld[0], S->UpperWorld[1], S->UpperWorld[2]);
00448   ver[21].set(S->UpperWorld[0], S->LowerWorld[1], S->UpperWorld[2]);
00449   norm[10].set(0.0,1.0,0.0);
00450 
00451   ver[22].set(S->UpperWorld[0], S->UpperWorld[1], S->UpperWorld[2]);
00452   ver[23].set(S->UpperWorld[0], S->UpperWorld[1], S->LowerWorld[2]);
00453   norm[11].set(0.0,1.0,0.0);
00454 
00455   color[0].set( 1, 0, 0, 0.0f );
00456 
00457   gset->setAttr(PFGS_COORD3, PFGS_PER_VERTEX, ver, NULL);
00458   gset->setAttr(PFGS_NORMAL3, PFGS_PER_PRIM, norm, NULL);
00459   gset->setAttr(PFGS_COLOR4, PFGS_OVERALL, color, NULL);
00460   gset->setPrimType(PFGS_LINES);
00461   gset->setNumPrims(12);
00462 
00463   //Specify Geometry State
00464   pfGeoState *gstate = new pfGeoState;
00465   gstate->setMode(PFSTATE_ENLIGHTING, PF_ON);
00466   gstate->setMode(PFSTATE_CULLFACE, PF_OFF);
00467   //gstate->setMode(PFSTATE_SHADEMODEL, PFSM_GOURAUD);
00468 
00469   // Using material
00470   pfMaterial *mat = new pfMaterial;
00471   mat->setColor(PFMTL_AMBIENT, 0.0f, 0.0f, 0.0f);
00472   mat->setColor(PFMTL_SPECULAR, 0.5f, 0.5f, 0.5f);
00473   mat->setShininess(10.0f);
00474   mat->setColorMode(PFMTL_BOTH, PFMTL_CMODE_AD);
00475   gstate->setAttr(PFSTATE_FRONTMTL, mat);
00476  
00477   gset->setGState(gstate);
00478 
00479   bound->addGSet(gset);
00480 }
00481 
00482 
00483 
00484 
00485 void RenderPerformer::LoadControlPanel(pfGroup *con, pfDCS *pad){
00486   double y_coord;
00487   pfGeode *geopad = new pfGeode;
00488   pfGeode *geo = new pfGeode;
00489   pfGeoSet *gset_frame = new pfGeoSet;
00490   pfGeoSet *gset_pad = new pfGeoSet;
00491 
00492   y_coord = (-1.0)*S->ViewingPosition.length() + 1.0;
00493   //y_coord = 0.0;
00494 
00495   //Specify Geometry State
00496   pfGeoState *gstate = new pfGeoState;
00497   gstate->setMode(PFSTATE_TRANSPARENCY, PFTR_ON);
00498   gstate->setMode(PFSTATE_ENLIGHTING, PF_ON);
00499   gstate->setMode(PFSTATE_CULLFACE, PFCF_OFF);
00500 
00501   // Using material
00502   pfMaterial *mat = new pfMaterial;
00503   mat->setColor(PFMTL_AMBIENT, 0.0f, 0.0f, 0.0f);
00504   mat->setColor(PFMTL_SPECULAR, 0.5f, 0.5f, 0.5f);
00505   mat->setShininess(10.0f);
00506   mat->setColorMode(PFMTL_BOTH, PFMTL_CMODE_AD);
00507   gstate->setAttr(PFSTATE_FRONTMTL, mat);
00508 
00509   pfVec3 *tri = new pfVec3[4];
00510   pfVec3 *norm = new pfVec3[1];
00511   pfVec4 *color = new pfVec4[1];
00512 
00513   tri[0].set((-1.0)*CONTROLPANEL_BAR_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00514              (-1.0)*CONTROLPANEL_BAR_HEIGHT + CONTROLPANEL_CENTER_Z);
00515   tri[1].set((-1.0)*CONTROLPANEL_BAR_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00516              CONTROLPANEL_BAR_HEIGHT + CONTROLPANEL_CENTER_Z);
00517   tri[2].set(CONTROLPANEL_BAR_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00518              CONTROLPANEL_BAR_HEIGHT + CONTROLPANEL_CENTER_Z);
00519   tri[3].set(CONTROLPANEL_BAR_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00520              (-1.0)*CONTROLPANEL_BAR_HEIGHT + CONTROLPANEL_CENTER_Z);
00521   norm[0].set(0.0, -1.0, 0.0);
00522   color[0].set(1.0, 0.5, 0.0, 0.5f );
00523 
00524   gset_frame->setAttr(PFGS_COORD3, PFGS_PER_VERTEX, tri, NULL);
00525   gset_frame->setAttr(PFGS_NORMAL3, PFGS_PER_PRIM, norm, NULL);
00526   gset_frame->setAttr(PFGS_COLOR4, PFGS_PER_PRIM, color, NULL);
00527   gset_frame->setPrimType(PFGS_QUADS);
00528   gset_frame->setNumPrims(1);
00529   gset_frame->setGState(gstate);
00530   geo->addGSet(gset_frame);
00531 
00532   con->addChild(geo);
00533 
00534   // Draw Control Pad
00535   pfVec3 *tri1 = new pfVec3[4];
00536   pfVec3 *norm1 = new pfVec3[1];
00537   pfVec4 *color1 = new pfVec4[1];
00538 
00539   tri1[0].set((-1.0)*CONTROLPAD_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00540               (-1.0)*CONTROLPAD_HEIGHT + CONTROLPANEL_CENTER_Z);
00541   tri1[1].set((-1.0)*CONTROLPAD_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00542               CONTROLPAD_HEIGHT + CONTROLPANEL_CENTER_Z);
00543   tri1[2].set(CONTROLPAD_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00544               CONTROLPAD_HEIGHT + CONTROLPANEL_CENTER_Z);
00545   tri1[3].set(CONTROLPAD_WIDTH + CONTROLPANEL_CENTER_X, y_coord,
00546               (-1.0)*CONTROLPAD_HEIGHT + CONTROLPANEL_CENTER_Z);
00547   norm1[0].set(0.0, -1.0, 0.0);
00548   color1[0].set(1.0, 0.0, 0.0, 0.5f );
00549 
00550   gset_pad->setAttr(PFGS_COORD3, PFGS_PER_VERTEX, tri1, NULL);
00551   gset_pad->setAttr(PFGS_NORMAL3, PFGS_PER_PRIM, norm1, NULL);
00552   gset_pad->setAttr(PFGS_COLOR4, PFGS_PER_PRIM, color1, NULL);
00553   gset_pad->setPrimType(PFGS_QUADS);
00554   gset_pad->setNumPrims(1);
00555   gset_pad->setGState(gstate);
00556   geopad->addGSet(gset_pad);
00557   
00558   pad->addChild(geopad);
00559 }
00560 
00561 
00562 
00563 
00564 void RenderPerformer::Normalize(double v[3]){
00565   GLfloat d = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
00566 
00567   v[0] /=d;
00568   v[1] /=d;
00569   v[2] /=d;
00570 }
00571 
00572 void RenderPerformer::NormCrossProduct(double v1[3], double v2[3], double out[3]){
00573   out[0] = v1[1]*v2[2] - v1[2]*v2[1];
00574   out[1] = v1[2]*v2[0] - v1[0]*v2[2];
00575   out[2] = v1[0]*v2[1] - v1[1]*v2[0];
00576 
00577   Normalize(out);
00578 }
00579 
00580 void RenderPerformer::GetCurrentMousePos(double &x, double &y){
00581   pfuGetMouse(Shared->Mouse);
00582   pfuMapMouseToChan(Shared->Mouse, Shared->Chan); // no matter mouse in chan or not return a value
00583   x = (double)Shared->Mouse->xchan;
00584   y = (double)Shared->Mouse->ychan;
00585 
00586   switch(Shared->Mouse->click){
00587   case 1: // click right
00588     Shared->HoldRightMouse = true;
00589     break;
00590   case 2: // click  middle
00591     Shared->HoldMiddleMouse = true;
00592     break;
00593   case 4: // click left
00594     Shared->HoldLeftMouse = true;
00595     break;
00596   default: // none
00597     break;
00598   }
00599   
00600   switch(Shared->Mouse->release){
00601   case 5: // release right and left
00602     Shared->HoldRightMouse = false;
00603     Shared->HoldLeftMouse = false;
00604     break;
00605   case 7: // release middle;
00606     Shared->HoldMiddleMouse = false;
00607     break;
00608   default: // none
00609     break;
00610   }     
00611 }
00612 
00613 /*
00614 // Make a homogeneous transformation matrix, roll first, then pitch, finnally head
00615 void RenderPerformer::MakeHomoMatrix(pfMatrix &m, double x, double y, double z,
00616                                      double r, double p, double h){
00617 
00618 
00619 }
00620 */
00621 
00622 void RenderPerformer::AnimateBodies(){
00623   int i,j;
00624   vector conf(6);
00625   vector c;
00626   list_item item;
00627   pfMatrix orientation, rot, delta_rot;
00628   int num;
00629 
00630   num = FrameList.length();
00631   if( num >= 1){
00632     item = FrameList.get_item(Shared->AnimatingStep);
00633     c = FrameList.contents(item);
00634     for(i=0; i < S->NumBodies; i++){
00635       // for each robot, first read its own configuration from FrameList
00636       for(j = 0; j < 6; j++)
00637         conf[j] = c[6*i+j];
00638       
00639       // update coordinate system
00640       orientation.makeIdent();
00641       rot.makeIdent();
00642       // rotate x
00643       delta_rot.makeRot(conf[3]*180.0/PI, 1.0, 0.0, 0.0);
00644       rot.postMult(delta_rot);
00645       // rotate y
00646       delta_rot.makeRot(conf[4]*180.0/PI, 0.0, 1.0, 0.0);
00647       rot.postMult(delta_rot);
00648       // rotate Z
00649       delta_rot.makeRot(conf[5]*180.0/PI, 0.0, 0.0, 1.0);
00650       rot.postMult(delta_rot);
00651       
00652       // Translate
00653       orientation.postTrans(rot, conf[0], conf[1], conf[2]);
00654       
00655       pfDCS *child_robot = pfGetChild(Shared->BodiesDCS, i);
00656       child_robot->setMat(orientation);
00657     }
00658     
00659     // update AnimatingStep
00660     if(Shared->AnimatingStep == num-1)
00661       Shared->AnimatingStep = 0;
00662     else{
00663       Shared->AnimatingStep += Shared->AnimatingSpeed;
00664       if(Shared->AnimatingStep >= num-1)
00665         Shared->AnimatingStep = num-1;
00666     }
00667   }
00668 }
00669 
00670 
00671 void RenderPerformer::Init(){
00672   Render::Init();
00673 
00674   InitPerformer();
00675 
00676   Shared->HoldRightMouse = false;
00677   Shared->HoldLeftMouse = false;
00678   Shared->HoldMiddleMouse = false;
00679   //Shared->ReleaseRightMouse = false;
00680   //Shared->ReleaseLeftMouse = false;
00681   //Shared->ReleaseMiddleMouse = false;
00682 
00683   Shared->AnimatingStep = 0;
00684   Shared->AnimatingSpeed = 1;
00685 
00686   Shared->TranX = 0.0;
00687   Shared->TranY = 0.0;
00688   Shared->TranZ = 0.0;
00689   Shared->RotX = 0.0;
00690   Shared->RotY = 0.0;
00691   Shared->RotZ = 0.0;
00692 
00693   //Set boundbox
00694   Shared->ShowBoundBox = false;
00695   
00696   // Set control pannel initial pos
00697   Shared->ControlPanelOn = true;
00698   Shared->FocusOnControlPad = false;
00699   Shared->PadX = (-1.0)*CONTROLPANEL_BAR_WIDTH;
00700   Shared->ControlPad->setTrans(Shared->PadX, 0.0, 0.0);
00701 
00702   // Make Eye initial matrix
00703   pfMatrix delta_rot;
00704   Shared->EyeMat.makeIdent();
00705   // rotate along Z 90 degrees
00706   delta_rot.makeRot(-90.0, 0.0, 0.0, 1.0);
00707   Shared->EyeMat.postMult(delta_rot);
00708   // Translate to designed position
00709   delta_rot.makeTrans(10, 10, 0);
00710   Shared->EyeMat.postMult(delta_rot);
00711 
00712   // Get courrent mouse position
00713   GetCurrentMousePos(Shared->MouseXOld, Shared->MouseYOld);
00714 }
00715 
00716   
00717 
00718 
00719 void RenderPerformer::Terminate(){
00720   // Terminate parallel processes and exit
00721   pfuExitInput();
00722   pfExit();
00723 }
00724 
00725 
00726 
00727 void RenderPerformer::HandleKeyInput(){
00728   int inkey;
00729 
00730   pfuGetEvents(Shared->InputEvents);
00731 
00732   if (Shared->InputEvents->numKeys > 0) {
00733     inkey = Shared->InputEvents->keyQ[0];
00734     pfuResetEventStream(Shared->InputEvents);
00735     //cout << inkey << endl;
00736 
00737     switch(inkey){
00738     case 99 : // "c"
00739       Shared->ControlPanelOn = !Shared->ControlPanelOn;  // Toggle
00740       break;
00741     case 98 : // "b"
00742       Shared->ShowBoundBox = !Shared->ShowBoundBox;  // Toggle
00743       break;
00744     }
00745   }
00746 }
00747 
00748 
00749 
00750 void RenderPerformer::GetControlPadSize(double &padwidth_l, double &padwidth_r, 
00751                                         double &padheight_b, double &padheight_top){
00752 
00753   padwidth_l = (CONTROLPANEL_CENTER_X + Shared->PadX - CONTROLPAD_WIDTH)/tan(22.5*PI/180.0);
00754   padwidth_r = (CONTROLPANEL_CENTER_X + Shared->PadX + CONTROLPAD_WIDTH)/tan(22.5*PI/180.0);
00755   padheight_b = (CONTROLPANEL_CENTER_Z - CONTROLPAD_HEIGHT)/tan(22.5*PI/180.0);
00756   padheight_top = (CONTROLPANEL_CENTER_Z + CONTROLPAD_HEIGHT)/tan(22.5*PI/180.0);
00757   //cout <<  padwidth_l << " " <<  padwidth_r  << "  " 
00758   //     << padheight_b << " " << padheight_top << endl;
00759 
00760 }
00761 
00762 
00763 
00764 void RenderPerformer::HandleMouseEvents(){
00765   double mousex_new, mousey_new;
00766   pfMatrix transform, dtransform;
00767   double padw_l, padw_r, padh_b, padh_top;
00768 
00769   // Get control pad size
00770   GetControlPadSize(padw_l, padw_r, padh_b, padh_top);
00771 
00772   // Get current mouse position
00773   GetCurrentMousePos(mousex_new, mousey_new);
00774 
00775   // If mouse is inside the pad and ControlPanelOn is true
00776   // deal with control pannel
00777   if( Shared->MouseXOld >= padw_l
00778       && Shared->MouseXOld <= padw_r
00779       && Shared->MouseYOld >= padh_b
00780       && Shared->MouseYOld <= padh_top
00781       && Shared->HoldLeftMouse
00782       && Shared->ControlPanelOn ){
00783     Shared->FocusOnControlPad = true;    
00784   }
00785   if(!Shared->HoldLeftMouse)
00786     Shared->FocusOnControlPad = false;; 
00787 
00788   if(Shared->FocusOnControlPad){
00789     Shared->PadX = Shared->PadX + 0.3*(mousex_new - Shared->MouseXOld);
00790     if(Shared->PadX >= CONTROLPANEL_BAR_WIDTH)
00791       Shared->PadX = CONTROLPANEL_BAR_WIDTH;
00792     else
00793       if(Shared->PadX <= (-1.0)*CONTROLPANEL_BAR_WIDTH)
00794         Shared->PadX = (-1.0)*CONTROLPANEL_BAR_WIDTH;
00795     Shared->AnimatingSpeed = 1 + (int)(100*(Shared->PadX + CONTROLPANEL_BAR_WIDTH));  
00796     Shared->ControlPad->setTrans(Shared->PadX, 0.0, 0.0);    
00797   }
00798 
00799   // else set global orientation
00800   if( !Shared->FocusOnControlPad && Shared->HoldRightMouse ){
00801     if( fabs(mousey_new - Shared->MouseYOld) > 0.001 ){
00802       Shared->TranY = Shared->TranY + 60*(mousey_new - Shared->MouseYOld);
00803     }
00804   }
00805   
00806   if( !Shared->FocusOnControlPad && Shared->HoldLeftMouse ){
00807     if(fabs(Shared->MouseXOld - mousex_new) > 0.00001)
00808       Shared->RotY = Shared->RotY - 40*(Shared->MouseXOld - mousex_new);
00809     if(fabs(Shared->MouseYOld - mousey_new) > 0.00001)
00810       Shared->RotX = Shared->RotX + 40*(Shared->MouseYOld - mousey_new);
00811   }
00812 
00813   if( !Shared->FocusOnControlPad && Shared->HoldMiddleMouse ){
00814     if(fabs(Shared->MouseXOld - mousex_new) > 0.00001)
00815       Shared->TranX = Shared->TranX + 60*(mousex_new - Shared->MouseXOld);
00816     if(fabs(Shared->MouseYOld - mousey_new) > 0.00001)
00817       Shared->TranZ = Shared->TranZ + 60*(mousey_new - Shared->MouseYOld);
00818   }
00819 
00820   // set up global orientation matrix
00821   transform.makeIdent();
00822   dtransform.makeRot(Shared->RotX, 1.0, 0.0, 0.0);
00823   transform.postMult(dtransform);
00824   dtransform.makeRot(Shared->RotY, 0.0, 1.0, 0.0);
00825   transform.postMult(dtransform);
00826   dtransform.makeRot(Shared->RotZ, 0.0, 0.0, 1.0);
00827   transform.postMult(dtransform);
00828   dtransform.makeTrans(Shared->TranX, Shared->TranY, Shared->TranZ);
00829   transform.postMult(dtransform);
00830   Shared->WorldDCS->setMat(transform);
00831   
00832   //Update old mouse position
00833   Shared->MouseXOld = mousex_new;
00834   Shared->MouseYOld = mousey_new;
00835 }
00836 
00837 
00838 void RenderPerformer::HandleEvents(){
00839   // Go to sleep until next frame time.
00840   pfSync();             
00841   
00842   //Collect key input events
00843   HandleKeyInput();
00844  
00845   // handle Mouse events
00846   HandleMouseEvents();
00847 
00848   // setup body animating
00849   if(Animated){
00850     AnimateBodies();
00851   }
00852 
00853   // Always display world
00854   Shared->WorkEnv->setVal(Shared->WorkEnv->searchChild(Shared->Env));
00855   Shared->ShowCase->setVal(Shared->ShowCase->searchChild(Shared->WorldDCS));
00856 
00857   // Show Bound Box
00858   if(Shared->ShowBoundBox){
00859     Shared->WorkEnv->setVal(PFSWITCH_ON);
00860   }
00861 
00862   // Show Control Panel
00863   if(Shared->ControlPanelOn){
00864     Shared->ShowCase->setVal(PFSWITCH_ON);
00865   }
00866 
00867   // Initiate cull/draw for this frame.
00868   pfFrame();
00869 }
00870 
00871 
00872 
00873 // Draw process callback
00874 void RenderPerformer::DrawChannel(pfChannel *chan, void *){
00875   chan->clear();
00876   pfDraw ();
00877 }
00878 
00879 
00880 
00881 #include <LEDA/UNDEFINE_NAMES.h>
00882 
00883 
00884 
00885 
00886 
00887 
00888 
00889 
00890 
00891 
00892 
00893 
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.