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