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 // Standard includes 00020 #include <math.h> 00021 #include <stdlib.h> 00022 #include <assert.h> 00023 00024 // OpenInventor includes 00025 #include <Xm/Xm.h> 00026 #include <Inventor/Xt/SoXt.h> 00027 #include <Inventor/SoDB.h> 00028 #include <Inventor/Xt/viewers/SoXtExaminerViewer.h> 00029 #include <Inventor/nodes/SoTransform.h> 00030 #include <Inventor/nodes/SoSeparator.h> 00031 #include <Inventor/nodes/SoCamera.h> 00032 #include <Inventor/nodes/SoIndexedFaceSet.h> 00033 #include <Inventor/nodes/SoMaterial.h> 00034 #include <Inventor/nodes/SoLineSet.h> 00035 #include <Inventor/nodes/SoSwitch.h> 00036 #include <Inventor/sensors/SoTimerSensor.h> 00037 #include <Inventor/fields/SoMFInt32.h> 00038 00039 // MSL includes 00040 #include "renderiv.h" 00041 #include "defs.h" 00042 00043 00044 // default viewer dimensions 00045 const short DEF_VIEWER_WIDTH = 500; 00046 const short DEF_VIEWER_HEIGHT = 400; 00047 00048 //--------------------------------------------------------------------- 00049 // RenderIv 00050 // 00051 // Method: Constructors 00052 // 00053 //--------------------------------------------------------------------- 00054 RenderIv::RenderIv(): Render() 00055 { 00056 ControlFreak = true; 00057 _ivRoot = NULL; 00058 _ivData = NULL; 00059 } 00060 00061 00062 RenderIv::RenderIv(string filepath=""): Render(filepath) 00063 { 00064 ControlFreak = true; 00065 _ivRoot = NULL; 00066 _ivData = NULL; 00067 } 00068 00069 00070 RenderIv::RenderIv(Scene *s, string filepath): Render(s,filepath) 00071 { 00072 ControlFreak = true; 00073 _ivRoot = NULL; 00074 _ivData = NULL; 00075 } 00076 00077 00078 //--------------------------------------------------------------------- 00079 // RenderIv 00080 // 00081 // Method: Destructor 00082 // 00083 //--------------------------------------------------------------------- 00084 RenderIv::~RenderIv() 00085 { 00086 if (_ivRoot) 00087 _ivRoot->unref(); 00088 // inventor garbage collection should take care of deleting 00089 // this and all of the other scene graph nodes 00090 } 00091 00092 00093 //--------------------------------------------------------------------- 00094 // RenderIv 00095 // 00096 // Method: Reset 00097 // 00098 //--------------------------------------------------------------------- 00099 void RenderIv::Reset() 00100 { 00101 // Use reset from base class 00102 Render::Reset(); 00103 00104 // clear all data 00105 if (_ivData) 00106 _ivData->removeAllChildren(); 00107 } 00108 00109 00110 00111 //--------------------------------------------------------------------- 00112 // RenderIv 00113 // 00114 // Method: Init 00115 // 00116 //--------------------------------------------------------------------- 00117 void RenderIv::Init() 00118 { 00119 Render::Init(); 00120 00121 // initialize Xt and Inventor 00122 const char* MAIN_WINDOW_TITLE = "MSL Library Iowa State University"; 00123 Widget mainWindow = SoXt::init(MAIN_WINDOW_TITLE); 00124 if (!mainWindow) { 00125 exit(1); 00126 } 00127 00128 //create the root 00129 _ivRoot = new SoSeparator; 00130 _ivRoot->ref(); 00131 00132 // create the main perspective viewer 00133 _viewer = new SoXtExaminerViewer(mainWindow); 00134 _viewer->setTitle(MAIN_WINDOW_TITLE); 00135 _viewer->setSceneGraph(_ivRoot); 00136 _viewer->setSize(SbVec2s(DEF_VIEWER_WIDTH, DEF_VIEWER_HEIGHT)); 00137 _viewer->show(); 00138 00139 // Sest default view for the camera 00140 float defaultCam[3] = { 0.0, 3.0, 5.0 }; 00141 if (S->GeomDim == 2) 00142 defaultCam[1] = -defaultCam[1]; 00143 SoCamera *camera = _viewer->getCamera(); 00144 camera->position.setValue(defaultCam[0], defaultCam[1], defaultCam[2]); 00145 camera->pointAt(SbVec3f(0.0, 0.0, 0.0)); 00146 camera->focalDistance.setValue(camera->position.getValue().length()); 00147 _viewer->saveHomePosition(); 00148 00149 // set the background color 00150 SbColor ivColor(0,0,0); 00151 _viewer->setBackgroundColor(ivColor); 00152 00153 // set up the timer callback 00154 SoTimerSensor *callbackSensor = new SoTimerSensor(_TimerCB, this); 00155 callbackSensor->setInterval(1/2000.0); // as fast as possible 00156 callbackSensor->schedule(); 00157 00158 // initialize the data 00159 _ivData = new SoSeparator; 00160 _ivRoot->addChild(_ivData); 00161 if (!_InitData()) cerr << "renderiv: Error initializing data" << endl; 00162 _viewer->viewAll(); 00163 00164 // show the window 00165 SoXt::show(mainWindow); 00166 } 00167 00168 00169 //--------------------------------------------------------------------- 00170 // RenderIv 00171 // Method: MainLoop 00172 // 00173 //--------------------------------------------------------------------- 00174 void RenderIv::MainLoop(Gui *g) 00175 { 00176 g->Finished = false; 00177 _pGui = g; 00178 00179 // enter the inventor loop 00180 SoXt::mainLoop(); 00181 } 00182 00183 00185 // 00186 // PROTECTED METHODS 00187 // 00188 00189 00190 //--------------------------------------------------------------------- 00191 // RenderIv 00192 // Method: _TimerCB 00193 // 00194 // Purpose: Callback for the timer sensor 00195 // 00196 //--------------------------------------------------------------------- 00197 void RenderIv::_TimerCB(void *userData, SoSensor *) 00198 { 00199 RenderIv *riv = (RenderIv *)userData; 00200 // execute idle function 00201 riv->_IdleFunction(); 00202 } 00203 00204 00205 00206 //--------------------------------------------------------------------- 00207 // RenderIv 00208 // Method: _TimerCB 00209 // 00210 // Purpose: Callback for the timer sensor 00211 // 00212 //--------------------------------------------------------------------- 00213 inline void RenderIv::_IdleFunction() 00214 { 00215 // Handle the window events for the Gui 00216 _pGui->HandleEvents(); 00217 00218 // Allow exiting by pressing Exit button in Gui 00219 if (_pGui->Finished) exit(-1); 00220 00221 // update display toggles 00222 if (_bDisplayBounds != BoundingBoxOn) { 00223 _bDisplayBounds = BoundingBoxOn; 00224 _SetSwitch(_ivBoundsSwitch, _bDisplayBounds); 00225 } 00226 if (_bDisplayPath != ShowPathOn) { 00227 _bDisplayPath = ShowPathOn; 00228 _SetSwitch(_ivPathSwitch, _bDisplayPath); 00229 } 00230 if (_bDisplayPath && NumFrames != _pathFrames) 00231 _UpdatePathDisplay(); 00232 00233 // update body state if animation is on 00234 if (AnimationActive) { 00235 SetCurrentAnimationFrame(); 00236 _UpdateBodies(CurrentAnimationFrame); 00237 } 00238 } 00239 00240 00241 //--------------------------------------------------------------------- 00242 // RenderIv 00243 // Method: _ReadIvFile 00244 // 00245 // Purpose: Read Inventor scene from a file 00246 // 00247 //--------------------------------------------------------------------- 00248 SoSeparator* RenderIv::_ReadIvFile(const char *filename) 00249 { 00250 // Open the input file 00251 SoInput sceneInput; 00252 if (!sceneInput.openFile(filename)) { 00253 cerr << "Cannot open file: " << filename << endl; 00254 return NULL; 00255 } 00256 00257 // Read the whole file into the database 00258 SoSeparator *graph = SoDB::readAll(&sceneInput); 00259 if (graph == NULL) { 00260 cerr << "Problem reading file: " << filename << endl; 00261 return NULL; 00262 } 00263 00264 sceneInput.closeFile(); 00265 return graph; 00266 } 00267 00268 00269 //--------------------------------------------------------------------- 00270 // RenderIv 00271 // Method: _InitObject 00272 // 00273 // Purpose: Load a single object 00274 // 00275 //--------------------------------------------------------------------- 00276 SoSeparator* RenderIv::_InitObject(const string &fname) 00277 { 00278 SoSeparator* pObject = NULL; 00279 cout << " loading file: " << fname << endl; 00280 00281 // check for native inventor file 00282 if (fname.substr(fname.length()-3,3) == ".iv") 00283 pObject = _ReadIvFile(fname.c_str()); 00284 else { 00285 // otherwise load raw geometry 00286 list<MSLTriangle> trlist; 00287 std::ifstream fin(fname.c_str()); 00288 00289 if (S->GeomDim == 2) { 00290 list<MSLPolygon> plist; 00291 fin >> plist; 00292 trlist = PolygonsToTriangles(plist, 2.0); // Defined in 3Dtriangle.C 00293 } 00294 else 00295 fin >> trlist; 00296 pObject = _InitTriangleGeom(trlist); 00297 } 00298 00299 return pObject; 00300 } 00301 00302 00303 //--------------------------------------------------------------------- 00304 // RenderIv 00305 // Method: _InitBoundsDisplay 00306 // 00307 // Purpose: initialize workspace boundary visualization 00308 // 00309 //--------------------------------------------------------------------- 00310 bool RenderIv::_InitBoundsDisplay() 00311 { 00312 // build configuration bounds visualization nodes 00313 SoVertexProperty* pVertexProp = new SoVertexProperty; 00314 SoLineSet *lines = new SoLineSet; 00315 lines->vertexProperty = pVertexProp; 00316 _ivBoundsSwitch = new SoSwitch(1); 00317 _ivBoundsSwitch->addChild(lines); 00318 _ivRoot->addChild(_ivBoundsSwitch); 00319 _bDisplayBounds = false; 00320 // display the bounds if flag is set 00321 _SetSwitch(_ivBoundsSwitch, _bDisplayBounds); 00322 00323 // set the color 00324 const float color[3] = { 0.5, 0.5, 0.5}; 00325 uint32_t red = uint32_t(color[0] * 255) << 24; 00326 uint32_t green = uint32_t(color[1] * 255) << 16; 00327 uint32_t blue = uint32_t(color[2] * 255) << 8; 00328 uint32_t alpha = 0x000000FF; 00329 uint32_t packedColor = red | green | blue | alpha; 00330 pVertexProp->orderedRGBA.setValue(packedColor); 00331 00332 // default bounding box 00333 float L[3] = { -10, -10, -10 }; 00334 float U[3] = { 10, 10, 10 }; 00335 00336 // get the workspace bounds 00337 if (S->LowerWorld[0] != S->UpperWorld[0]) { 00338 L[0]= S->LowerWorld[0]; L[1]= S->LowerWorld[1]; L[2]= S->LowerWorld[2]; 00339 U[0]= S->UpperWorld[0]; U[1]= S->UpperWorld[1]; U[2]= S->UpperWorld[2]; 00340 } 00341 cerr << "Workspace boundary: ( " << L[0] << ", " << L[1] << ", " << L[2] 00342 << " ) - ( " << U[0] << ", " << U[1] << ", " << U[2] << " )" << endl; 00343 00344 // set the bounding box lines 00345 const int numPoints = 16; 00346 float points[numPoints][3] = { 00347 {L[0],L[1],L[2]}, {L[0],L[1],U[2]}, {U[0],L[1],U[2]}, {U[0],L[1],L[2]}, 00348 {L[0],L[1],L[2]}, {L[0],U[1],L[2]}, {L[0],U[1],U[2]}, {U[0],U[1],U[2]}, 00349 {U[0],U[1],L[2]}, {L[0],U[1],L[2]}, {U[0],U[1],L[2]}, {U[0],L[1],L[2]}, 00350 {U[0],L[1],U[2]}, {U[0],U[1],U[2]}, {L[0],U[1],U[2]}, {L[0],L[1],U[2]} 00351 }; 00352 pVertexProp->vertex.setNum(numPoints); 00353 pVertexProp->vertex.setValues(0, numPoints, points); 00354 00355 return true; 00356 } 00357 00358 00359 //--------------------------------------------------------------------- 00360 // RenderIv 00361 // Method: _InitPathDisplay 00362 // 00363 // Purpose: initialize path visualization 00364 // 00365 //--------------------------------------------------------------------- 00366 bool RenderIv::_InitPathDisplay() 00367 { 00368 // build configuration bounds visualization nodes 00369 _pPathVertexProp = new SoVertexProperty; 00370 SoLineSet *lines = new SoLineSet; 00371 lines->vertexProperty = _pPathVertexProp; 00372 _ivPathSwitch = new SoSwitch(1); 00373 _ivPathSwitch->addChild(lines); 00374 _ivRoot->addChild(_ivPathSwitch); 00375 _bDisplayPath = false; 00376 _pathFrames = 0; 00377 // display the bounds if flag is set 00378 _SetSwitch(_ivPathSwitch, _bDisplayPath); 00379 00380 // set the color 00381 const float color[3] = { 1.0, 1.0, 0.2}; 00382 uint32_t red = uint32_t(color[0] * 255) << 24; 00383 uint32_t green = uint32_t(color[1] * 255) << 16; 00384 uint32_t blue = uint32_t(color[2] * 255) << 8; 00385 uint32_t alpha = 0x000000FF; 00386 uint32_t packedColor = red | green | blue | alpha; 00387 _pPathVertexProp->orderedRGBA.setValue(packedColor); 00388 00389 // default bounding box 00390 float L[3] = { -3, -3, -1 }; 00391 float U[3] = { 2, 3, 1 }; 00392 00393 // reset to empty path 00394 const int numPoints = 16; 00395 float points[numPoints][3] = { 00396 {L[0],L[1],L[2]}, {L[0],L[1],U[2]}, {U[0],L[1],U[2]}, {U[0],L[1],L[2]}, 00397 {L[0],L[1],L[2]}, {L[0],U[1],L[2]}, {L[0],U[1],U[2]}, {U[0],U[1],U[2]}, 00398 {U[0],U[1],L[2]}, {L[0],U[1],L[2]}, {U[0],U[1],L[2]}, {U[0],L[1],L[2]}, 00399 {U[0],L[1],U[2]}, {U[0],U[1],U[2]}, {L[0],U[1],U[2]}, {L[0],L[1],U[2]} 00400 }; 00401 _pPathVertexProp->vertex.setNum(numPoints); 00402 _pPathVertexProp->vertex.setValues(0, numPoints, points); 00403 00404 return true; 00405 } 00406 00407 00408 //--------------------------------------------------------------------- 00409 // RenderIv 00410 // 00411 // Method: _InitTriangleGeom 00412 // 00413 // Purpose: initialize object geometry from triangles 00414 // 00415 //--------------------------------------------------------------------- 00416 SoSeparator* RenderIv::_InitTriangleGeom(list<MSLTriangle> &triangles) 00417 { 00418 int numTriangles = triangles.size(); 00419 if (numTriangles <= 0) { 00420 cerr << "renderiv: WARNING - empty triangle list." << endl; 00421 return NULL; 00422 } 00423 00424 // build iv hierarchy 00425 SoSeparator* pSep = new SoSeparator; 00426 SoMaterial* pMat = new SoMaterial; 00427 SoVertexProperty* pVertexProp = new SoVertexProperty; 00428 SoIndexedFaceSet* pFaceSet = new SoIndexedFaceSet; 00429 pSep->addChild(pMat); 00430 pSep->addChild(pFaceSet); 00431 00432 // Change the colors for different bodies 00433 static int objnum = 0; 00434 objnum++; 00435 SbColor c(RGBRed[(objnum) % RENDERCOLORS], 00436 RGBGreen[(objnum) % RENDERCOLORS], 00437 RGBBlue[(objnum) % RENDERCOLORS]); 00438 pMat->diffuseColor.setValue(c[0], c[1], c[2]); 00439 00440 // set the vertex data 00441 pFaceSet->vertexProperty = pVertexProp; 00442 const int numVerts = numTriangles * 3; 00443 typedef float float3[3]; 00444 float3* points = new float3[numVerts]; 00445 int v = 0; 00446 list<MSLTriangle>::iterator t; 00447 forall(t, triangles) { 00448 points[v][0] = t->p1.xcoord(); 00449 points[v][1] = t->p1.ycoord(); 00450 points[v][2] = t->p1.zcoord(); 00451 v++; 00452 points[v][0] = t->p2.xcoord(); 00453 points[v][1] = t->p2.ycoord(); 00454 points[v][2] = t->p2.zcoord(); 00455 v++; 00456 points[v][0] = t->p3.xcoord(); 00457 points[v][1] = t->p3.ycoord(); 00458 points[v][2] = t->p3.zcoord(); 00459 v++; 00460 } 00461 assert(v == numVerts); 00462 pVertexProp->vertex.setNum(numVerts); 00463 pVertexProp->vertex.setValues(0, numVerts, points); 00464 00465 // set the triangle vertex indices 00466 const int numIndices = numTriangles * 4; 00467 int32_t *vIndex = new int32_t[numIndices]; 00468 int i = 0; 00469 v = 0; 00470 for (int t = 0; t < numTriangles; t++) { 00471 vIndex[i++] = v++; vIndex[i++] = v++; vIndex[i++] = v++; 00472 vIndex[i++] = -1; 00473 } 00474 assert (v == numVerts); 00475 assert (i == numIndices); 00476 00477 pFaceSet->coordIndex.setValues(0, numIndices, vIndex); 00478 pFaceSet->coordIndex.setNum(numIndices); 00479 delete [] vIndex; 00480 00481 return pSep; 00482 } 00483 00484 00485 //--------------------------------------------------------------------- 00486 // RenderIv 00487 // Method: _InitData 00488 // 00489 // Purpose: Load all obstacles and body geometry 00490 // 00491 //--------------------------------------------------------------------- 00492 bool RenderIv::_InitData() 00493 { 00494 // set control variables 00495 AnimationActive = false; 00496 00497 // EnvList was initialized by Init in Render base class 00498 list<string>::iterator fname; 00499 forall(fname, EnvList) { 00500 SoSeparator* pObject = _InitObject(FilePath + *fname); 00501 if (!pObject) 00502 cerr << "renderiv: ERROR initializing obstacle: " << *fname << endl; 00503 else 00504 _ivRoot->addChild(pObject); 00505 } 00506 00507 // Bodies 00508 forall(fname, BodyList) { 00509 SoSeparator* pObject = _InitObject(FilePath + *fname); 00510 if (!pObject) 00511 cerr << "renderiv: ERROR initializing body: " << *fname << endl; 00512 else { 00513 // create a movable transformation 00514 SoSeparator* objRoot = new SoSeparator; 00515 SoTransform* objTrans = new SoTransform; 00516 objRoot->addChild(objTrans); 00517 objRoot->addChild(pObject); 00518 _ivRoot->addChild(objRoot); 00519 _bodyTrans.push_back(objTrans); 00520 } 00521 } 00522 00523 // init the display items 00524 if (!_InitBoundsDisplay()) 00525 cerr << "renderiv: ERROR initializing workspace boundary" << endl; 00526 if (!_InitPathDisplay()) 00527 cerr << "renderiv: ERROR initializing path display" << endl; 00528 00529 00530 // update the current positions 00531 _UpdateBodies(CurrentAnimationFrame); 00532 00533 return true; 00534 } 00535 00536 00537 //--------------------------------------------------------------------- 00538 // RenderIv 00539 // Method: _SetSwitch 00540 // 00541 // Purpose: set the value of a rendering switch 00542 // 00543 //--------------------------------------------------------------------- 00544 inline void RenderIv::_SetSwitch(SoSwitch *pSwitch, bool bFlag) 00545 { 00546 pSwitch->whichChild.setValue(bFlag ? SO_SWITCH_ALL : SO_SWITCH_NONE); 00547 } 00548 00549 00550 //--------------------------------------------------------------------- 00551 // RenderIv 00552 // 00553 // Method: _UpdatePathDisplay 00554 // 00555 // Purpose: update the path visualization 00556 // 00557 //--------------------------------------------------------------------- 00558 void RenderIv::_UpdatePathDisplay() 00559 { 00560 _pathFrames = NumFrames; 00561 00562 // check for empty path 00563 if (NumFrames < 2) { 00564 _pPathVertexProp->vertex.setNum(0); 00565 return; 00566 } 00567 00568 MSLVector next, prev = FrameList.front(); 00569 int BodyNum = prev.dim() / 6; 00570 00571 // point allocation 00572 const int numPoints = BodyNum * ((NumFrames - 1) * 2); 00573 typedef float float3[3]; 00574 float3* points = new float3[numPoints]; 00575 00576 // get all frame data 00577 int bInd; 00578 int p = 0; 00579 00580 list<MSLVector>::iterator frp; 00581 frp = FrameList.begin(); 00582 for (int i = 1; i < NumFrames; i++) { 00583 for (int j = 0; j < BodyNum; j++) { 00584 bInd = 6 * j; 00585 frp++; next = *frp; 00586 // set the endpoints 00587 points[p][0] = prev[bInd]; 00588 points[p][1] = prev[bInd+1]; 00589 points[p][2] = prev[bInd+2]; 00590 p++; 00591 points[p][0] = next[bInd]; 00592 points[p][1] = next[bInd+1]; 00593 points[p][2] = next[bInd+2]; 00594 p++; 00595 prev = next; 00596 } 00597 } 00598 assert(p == numPoints); 00599 00600 // set the points 00601 _pPathVertexProp->vertex.setNum(numPoints); 00602 _pPathVertexProp->vertex.setValues(0, numPoints, points); 00603 delete [] points; 00604 } 00605 00606 00607 //--------------------------------------------------------------------- 00608 // RenderIv 00609 // 00610 // Method: _SetBodyTransform 00611 // 00612 // Purpose: set a 3D transformation 00613 // 00614 //--------------------------------------------------------------------- 00615 inline void RenderIv::_SetTransform(SoTransform* pTrans, 00616 double tx, double ty, double tz, 00617 double rx, double ry, double rz) 00618 { 00619 // set the rotation matrix from RPY euler angles 00620 float ca = cos(rx); float sa = sin(rx); 00621 float cb = cos(ry); float sb = sin(ry); 00622 float cc = cos(rz); float sc = sin(rz); 00623 00624 float R11 = cb * cc; 00625 float R12 = sa * sb * cc - ca * sc; 00626 float R13 = ca * sb * cc + sa * sc; 00627 float R21 = cb * sc; 00628 float R22 = sa * sb * sc + ca * cc; 00629 float R23 = ca * sb * sc - sa * cc; 00630 float R31 = -sb; 00631 float R32 = sa * cb; 00632 float R33 = ca * cb; 00633 00634 // build row-major matrix 00635 SbMatrix matrix(R11, R21, R31, 0.0, 00636 R12, R22, R32, 0.0, 00637 R13, R23, R33, 0.0, 00638 tx, ty, tz, 1.0); 00639 pTrans->setMatrix(matrix); 00640 } 00641 00642 00643 //--------------------------------------------------------------------- 00644 // RenderIv 00645 // 00646 // Method: _UpdateBodies 00647 // 00648 // Purpose: update the body configuration 00649 // 00650 //--------------------------------------------------------------------- 00651 void RenderIv::_UpdateBodies(const MSLVector &qConfig) 00652 { 00653 int index = 0; 00654 list<SoTransform*>::iterator pTrans; 00655 forall (pTrans, _bodyTrans) { 00656 _SetTransform(*pTrans, qConfig[index], qConfig[index+1], qConfig[index+2], 00657 qConfig[index+3], qConfig[index+4], qConfig[index+5]); 00658 index += 6; 00659 } 00660 assert(index == qConfig.dim()); 00661 } 00662 00663