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 "render.h" 00023 #include "defs.h" 00024 00025 // Include the VCR buttons 00026 #include "pixmaps/ff.xpm" 00027 #include "pixmaps/pause.xpm" 00028 #include "pixmaps/play.xpm" 00029 #include "pixmaps/rw.xpm" 00030 #include "pixmaps/stop.xpm" 00031 #include "pixmaps/prev.xpm" 00032 #include "pixmaps/next.xpm" 00033 00034 // Include the msl icon 00035 #include "pixmaps/msl.xpm" 00036 00037 00039 // 00040 // Render Class 00041 // 00043 00044 Render::Render() { 00045 Render(""); 00046 } 00047 00048 00049 Render::Render(string filepath) 00050 { 00051 FilePath = filepath; 00052 } 00053 00054 00055 Render::Render(Scene *s, string filepath) 00056 { 00057 SetScene(s); 00058 00059 FilePath = filepath; 00060 ControlFreak = false; 00061 } 00062 00063 00064 // This sets up the file names for the environment and movable bodies 00065 void Render::Init() 00066 { 00067 int i; 00068 std::ifstream fin; 00069 00070 Reset(); 00071 00072 FrameTime = 0.1; // This is default; each renderer should determine this 00073 AnimationStartPause = 0.0; // should be in seconds 00074 AnimationEndPause = 0.0; 00075 RenderCtlWindowOn = false; 00076 00077 AttachedCameraOn = false; 00078 BoundingBoxOn = false; 00079 MultipleViewsOn = false; 00080 ShowPathOn = false; 00081 00082 // Colors for objects (add more if you like, but increase RENDERCOLORS) 00083 //RGBRed[0] = .467; RGBGreen[0] = 0.53; RGBBlue[0] = 0.6; // Light Slate Gray 00084 RGBRed[0] = 1.0; RGBGreen[0] = 0.0; RGBBlue[0] = 1.0; // Magenta 00085 RGBRed[1] = 0.8; RGBGreen[1] = 0.0; RGBBlue[1] = 0.0; // Red 00086 RGBRed[2] = 0.0; RGBGreen[2] = 0.7; RGBBlue[2] = 0.0; // Green 00087 RGBRed[3] = 0.7; RGBGreen[3] = 0.7; RGBBlue[3] = 0.0; // Yellow 00088 RGBRed[4] = 0.0; RGBGreen[4] = 0.7; RGBBlue[4] = 0.7; // Lightblue 00089 RGBRed[5] = 1.0; RGBGreen[5] = 0.0; RGBBlue[5] = 1.0; // Magenta 00090 RGBRed[6] = 0.0; RGBGreen[6] = 0.0; RGBBlue[6] = 0.7; // Blue 00091 RGBRed[7] = 1.0; RGBGreen[7] = 0.65; RGBBlue[7] = 0.0; // Orange 00092 RGBRed[8] = 1.0; RGBGreen[8] = 0.078; RGBBlue[8] = 0.576; // DeepPink 00093 RGBRed[9] = 1.0; RGBGreen[9] = 0.51; RGBBlue[9] = 0.278; // Sienna1 00094 00095 fin.open((FilePath+"EnvList").c_str()); 00096 if (fin) { 00097 fin >> EnvList; 00098 fin.close(); } 00099 else { 00100 fin.open((FilePath+"Obst").c_str()); 00101 if (fin) 00102 EnvList.push_back("Obst"); 00103 fin.close(); 00104 } 00105 00106 char* s = new char[50]; 00107 00108 // Load bodies 00109 fin.open((FilePath+"BodyList").c_str()); 00110 if (fin) 00111 fin >> BodyList; 00112 else { 00113 fin.close(); 00114 fin.open((FilePath+"Robot").c_str()); 00115 if (fin) 00116 BodyList.push_back("Robot"); 00117 else { // Multiple robots 00118 for (i = 0; i < S->NumBodies; i++) { 00119 fin.close(); 00120 sprintf(s,"%sRobot%d",FilePath.c_str(),i); 00121 fin.open(s); 00122 if (fin) { 00123 sprintf(s,"Robot%d",i); 00124 BodyList.push_back(string(s)); // Use the robot from World 00125 } 00126 } 00127 } 00128 } 00129 fin.close(); 00130 00131 } 00132 00133 00134 00135 void Render::SetScene(Scene *s) 00136 { 00137 S = s; // S is the Scene for Render 00138 } 00139 00140 00141 void Render::MakeAnimationFrames(const list<MSLVector> &xlist, double deltat) 00142 { 00143 double t; 00144 int i; 00145 00146 StateList = xlist; 00147 TimeList.clear(); 00148 00149 t = 0.0; 00150 for (i = 0; i < (int) xlist.size(); i++) { 00151 TimeList.push_back(t); 00152 t += deltat; 00153 } 00154 00155 SetFrameList(); 00156 } 00157 00158 00159 void Render::MakeAnimationFrames(const list<MSLVector> &xlist, 00160 const list<double> &timelist) 00161 { 00162 StateList = xlist; 00163 TimeList = timelist; 00164 SetFrameList(); 00165 } 00166 00167 00168 void Render::SetFrameList() { 00169 double ltime; // Lower time 00170 double utime; // Upper time 00171 double crtime; // Current time 00172 double lasttime; 00173 double lambda; 00174 MSLVector sc; // scene configuration 00175 MSLVector lx,ux; // lower and upper states 00176 int i; 00177 list<MSLVector>::iterator stli,tstli; 00178 list<double>::iterator tli,li; 00179 00180 //cout << "Running SetFrameList\n"; 00181 //cout << "StateList: " << StateList << "\n"; 00182 //cout << "TimeList: " << TimeList << "\n"; 00183 00184 FrameList.clear(); 00185 NumFrames = 0; 00186 00187 if (TimeList.size() < 2) { 00188 if (TimeList.size() == 1) { 00189 FrameList.push_back(S->StateToSceneConfiguration(StateList.front())); 00190 NumFrames = 1; 00191 } 00192 return; 00193 } 00194 00195 // Make the starting pause 00196 for (i = 0; i < (int) (AnimationStartPause/FrameTime); i++) { 00197 FrameList.push_back(S->StateToSceneConfiguration(StateList.front())); 00198 NumFrames++; 00199 } 00200 00201 li = TimeList.begin(); li++; 00202 stli = StateList.begin(); stli++; 00203 utime = *li; // Time index of second element 00204 ux = *stli; 00205 ltime = TimeList.front(); 00206 lx = StateList.front(); 00207 crtime = ltime; 00208 lasttime = TimeList.back(); 00209 tli = li; tli++; 00210 while (crtime <= lasttime) { 00211 while ((utime < crtime)&& // Make sure the crtime within upper and lower 00212 (tli != TimeList.end())) { 00213 li++; 00214 stli++; 00215 utime = *li; 00216 ux = *stli; 00217 tli = li; tli++; 00218 } 00219 tli = li; tli--; 00220 ltime = *tli; 00221 tstli = stli; tstli--; 00222 lx = *tstli; 00223 while (ltime > crtime) { // This can happen sometimes (I think)! 00224 tli--; 00225 tstli--; 00226 ltime = *tli; 00227 lx = *tstli; 00228 } 00229 00230 lambda = (crtime - ltime)/(utime - ltime); 00231 //cout << "lambda: " << lambda << "\n"; 00232 00233 // Interoplate based on states lx and ux 00234 sc = S->InterpolatedSceneConfiguration(lx,ux,lambda); 00235 FrameList.push_back(sc); 00236 NumFrames++; 00237 crtime += FrameTime; 00238 } 00239 00240 // Add in the last frame, if necessary 00241 if (FrameList.back() != (S->StateToSceneConfiguration(StateList.back()))) 00242 FrameList.push_back(S->StateToSceneConfiguration(StateList.back())); 00243 00244 // Make the ending pause 00245 for (i = 0; i < (int) (AnimationEndPause/FrameTime); i++) { 00246 FrameList.push_back(S->StateToSceneConfiguration(StateList.back())); 00247 NumFrames++; 00248 } 00249 00250 //list<MSLVector>::iterator x; 00251 //cout << "StateList:\n"; 00252 //forall(x,StateList) 00253 // cout << *x << "\n"; 00254 //cout << "TimeList: " << TimeList << "\n"; 00255 //cout << "\n\nFrameList:\n"; 00256 //forall(x,FrameList) 00257 // cout << *x << "\n"; 00258 //cout << "NumFrames: " << NumFrames << "\n"; 00259 } 00260 00261 00262 00263 void Render::SetCurrentAnimationFrame() { 00264 MSLVector c(S->SceneConfigurationDim); 00265 int num,skip,i; 00266 00267 // How long has the frame been stuck? 00268 FrameStuckTime = used_time() - LastFrameTime; 00269 00270 skip = (int) (AnimationTimeScale * FrameStuckTime / FrameTime); 00271 00272 if (skip > 0) { 00273 AnimationFrameIndex += skip; 00274 used_time(LastFrameTime); 00275 } 00276 00277 num = FrameList.size(); 00278 if (AnimationFrameIndex > num - 1) { 00279 AnimationFrameIndex = 0; 00280 used_time(LastFrameTime); 00281 } 00282 00283 if (AnimationFrameIndex < 0) { 00284 AnimationFrameIndex = 0; 00285 used_time(LastFrameTime); 00286 } 00287 00288 if (num > 0) { 00289 list<MSLVector>::iterator fi; 00290 fi = FrameList.begin(); 00291 for (i = 0; i < AnimationFrameIndex - 1; i++) 00292 fi++; 00293 c = *fi; 00294 } 00295 00296 CurrentAnimationFrame = c; 00297 } 00298 00299 00300 00301 void Render::ButtonHandle(int b) 00302 { 00303 int i; 00304 list<MSLVector>::iterator fi; 00305 //cout << "Button " << b << "\n"; 00306 00307 switch (b) { 00308 00309 case 70: // Play 00310 if (!AnimationActive) { 00311 used_time(LastFrameTime); 00312 AnimationActive = true; 00313 } 00314 break; 00315 case 71: // Stop 00316 AnimationActive = false; 00317 AnimationFrameIndex = 0; 00318 LastFrameTime = 0.0;; 00319 used_time(LastFrameTime); 00320 break; 00321 case 72: // Pause 00322 if (AnimationActive) { 00323 AnimationActive = false; 00324 } 00325 else { 00326 AnimationActive = true; 00327 used_time(LastFrameTime); 00328 } 00329 cout << "Frame: " << AnimationFrameIndex << " Time stamp: " 00330 << AnimationFrameIndex*FrameTime << "s\n"; 00331 break; 00332 case 73: // Deccelerate animation 00333 AnimationTimeScale /= 1.41421356237; // Two presses doubles the speed 00334 break; 00335 case 74: // Accelerate animation 00336 AnimationTimeScale *= 1.41421356237; // Two presses doubles the speed 00337 break; 00338 case 75: 00339 Reset(); 00340 break; 00341 case 76: // Prev frame 00342 AnimationActive = false; 00343 if (AnimationFrameIndex > 0) 00344 AnimationFrameIndex--; 00345 else 00346 AnimationFrameIndex = FrameList.size() - 1; 00347 fi = FrameList.begin(); 00348 for (i = 0; i < AnimationFrameIndex - 1; i++) 00349 fi++; 00350 CurrentAnimationFrame = *fi; 00351 ShowCurrentAnimationFrame(); 00352 cout << "Frame: " << AnimationFrameIndex << " Time stamp: " 00353 << AnimationFrameIndex*FrameTime << "s\n"; 00354 break; 00355 case 77: // Next frame 00356 AnimationActive = false; 00357 if (AnimationFrameIndex < (int) FrameList.size() - 1) 00358 AnimationFrameIndex++; 00359 else 00360 AnimationFrameIndex = 0; 00361 fi = FrameList.begin(); 00362 for (i = 0; i < AnimationFrameIndex - 1; i++) 00363 fi++; 00364 CurrentAnimationFrame = *fi; 00365 ShowCurrentAnimationFrame(); 00366 cout << "Frame: " << AnimationFrameIndex << " Time stamp: " 00367 << AnimationFrameIndex*FrameTime << "s\n"; 00368 break; 00369 case 80: cout << "Toggle Show Path\n"; 00370 ShowPathOn = !ShowPathOn; 00371 break; 00372 case 81: cout << "Toggle Attached Camera\n"; 00373 AttachedCameraOn = !AttachedCameraOn; 00374 break; 00375 case 82: cout << "Toggle Multiple Views\n"; 00376 MultipleViewsOn = !MultipleViewsOn; 00377 break; 00378 case 83: cout << "Toggle Bounding Box\n"; 00379 BoundingBoxOn = !BoundingBoxOn; 00380 break; 00381 default: cout << "Option " << b << " not implemented\n"; 00382 break; 00383 } 00384 } 00385 00386 00387 void Render::MainLoop(Gui *g) { 00388 00389 g->Finished = false; 00390 while (!g->Finished) { 00391 g->HandleEvents(); 00392 HandleEvents(); 00393 } 00394 00395 } 00396 00397 00398 00399 void Render::Reset() { 00400 AnimationActive = false; 00401 AnimationFrameIndex = 0; 00402 if (FrameList.size() > 0) 00403 CurrentAnimationFrame = FrameList.front(); 00404 else 00405 CurrentAnimationFrame = MSLVector(S->SceneConfigurationDim); 00406 AnimationTimeScale = 1.0; 00407 LastFrameTime = 0.0; 00408 used_time(LastFrameTime); 00409 // Leave these next four alone because the GUI is not connected to them 00410 //AttachedCameraOn = false; 00411 //BoundingBoxOn = false; 00412 //MultipleViewsOn = false; 00413 //ShowPathOn = false; 00414 AmbientLight = 0.2; 00415 } 00416 00417 00418 /* 00419 void Render::ToggleRenderCtlWindow() 00420 { 00421 char *play,*stop,*pause,*rw,*ff,*prev,*next; 00422 RenderCtlWindowOn = !RenderCtlWindowOn; 00423 if (RenderCtlWindowOn) { 00424 RenderCtlWindow = new window(200,155,"MSL: Render Control Window"); 00425 RenderCtlWindow->set_icon_pixrect(RenderCtlWindow->create_pixrect(msl_xpm)); 00426 RenderCtlWindow->double_item("AnimationTimeScale",AnimationTimeScale); 00427 RenderCtlWindow->double_item("AmbientLight",AmbientLight); 00428 RenderCtlWindow->bool_item("BoundingBox",BoundingBoxOn); 00429 RenderCtlWindow->bool_item("MultipleViews",MultipleViewsOn); 00430 RenderCtlWindow->bool_item("AttachedCamera",AttachedCameraOn); 00431 RenderCtlWindow->bool_item("ShowPath",ShowPathOn); 00432 00433 // Make the animation buttons 00434 play = RenderCtlWindow->create_pixrect(play_xpm); 00435 stop = RenderCtlWindow->create_pixrect(stop_xpm); 00436 pause = RenderCtlWindow->create_pixrect(pause_xpm); 00437 rw = RenderCtlWindow->create_pixrect(rw_xpm); 00438 ff = RenderCtlWindow->create_pixrect(ff_xpm); 00439 prev = RenderCtlWindow->create_pixrect(prev_xpm); 00440 next = RenderCtlWindow->create_pixrect(next_xpm); 00441 RenderCtlWindow->buttons_per_line(7); 00442 RenderCtlWindow->button(stop,stop,"",71); 00443 RenderCtlWindow->button(prev,prev,"",76); 00444 RenderCtlWindow->button(pause,pause,"",72); 00445 RenderCtlWindow->button(next,next,"",77); 00446 RenderCtlWindow->button(rw,rw,"",73); 00447 RenderCtlWindow->button(play,play,"",70); 00448 RenderCtlWindow->button(ff,ff,"",74); 00449 00450 RenderCtlWindow->button("Reset",75); 00451 00452 RenderCtlWindow->display(); 00453 } 00454 else { 00455 RenderCtlWindow->close(); 00456 } 00457 } 00458 */ 00459 00460 00461 00462 00463