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

SIG_SimulationVisualisation.cpp

00001 #include "SIGEL_Visualisation/SIG_SimulationVisualisation.h"
00002 #include "SIGEL_Tools/SIG_TypeConverter.h"
00003 
00004 #include <cmath>
00005 
00006 using namespace SIGEL_Tools;
00007 
00008 namespace SIGEL_Visualisation
00009 {
00010    SIG_SimulationVisualisation::SIG_SimulationVisualisation(SIGEL_Robot::SIG_Robot const &robot,
00011                                                             SIGEL_Environment::SIG_Environment const &environment,
00012                                                             SIGEL_Simulation::SIG_SimulationParameters const &simulationParameter,
00013                                                             SIGEL_Program::SIG_Program const &program)
00014      : SIG_Visualisation(),
00015        environmentRenderer(environment),
00016        robotRenderer(robot),
00017        robot(robot),
00018        environment(environment),
00019        simulationParameter(simulationParameter),
00020        program(program),
00021        frameCounter( 0 )
00022    {
00023      robotPathPointAddingRate = static_cast< int >( 0.5 / simulationParameter.getStepSize() );
00024 
00025      if (robotPathPointAddingRate == 0)
00026        robotPathPointAddingRate = 1;
00027 
00028      int noOfLinks = robot.getLinkIter().count();
00029      renderRecorder = new SIG_RenderRecorder(noOfLinks);
00030 
00031      simulation = new SIGEL_Simulation::SIG_Simulation(robot,
00032                                                        environment,
00033                                                        program,
00034                                                        simulationParameter,
00035                                                        *renderRecorder);
00036 
00037      floatingTexts.resize( robot.getNrOfPoints() );
00038      for (int i=0; i<robot.getNrOfPoints(); i++)
00039        floatingTexts.insert( i, robotRenderer.floatingTexts[ i ] );
00040 
00041      updateRobotLinks();
00042      updateRobotPoints();
00043    };
00044 
00045    SIG_SimulationVisualisation::~SIG_SimulationVisualisation()
00046    {
00047      delete simulation;
00048      delete renderRecorder;
00049    };
00050 
00051   void SIG_SimulationVisualisation::setPointsVisible( bool visible )
00052   {
00053     robotRenderer.setPointsVisible( visible );
00054   };
00055 
00056   void SIG_SimulationVisualisation::setPlaneColor( QColor newColor )
00057   {
00058     double red = double( newColor.red() ) / 255;
00059     double green = double( newColor.green() ) / 255;
00060     double blue = double( newColor.blue() ) / 255;
00061 
00062     environmentRenderer.setPlaneColor( red,
00063                                        green,
00064                                        blue );
00065   };
00066 
00067   void SIG_SimulationVisualisation::setShowPlane( bool newShowPlane )
00068   {
00069     environmentRenderer.setShowPlane( newShowPlane );
00070   };
00071 
00072   void SIG_SimulationVisualisation::setShowGrid( bool newShowGrid )
00073   {
00074     environmentRenderer.setShowGrid( newShowGrid );
00075   };
00076 
00077   void SIG_SimulationVisualisation::setShowRobotPath( bool newShowRobotPath )
00078   {
00079     environmentRenderer.setShowRobotPath( newShowRobotPath );
00080   };
00081 
00082    void SIG_SimulationVisualisation::visualize()
00083    {
00084      SIG_Visualisation::visualize();
00085 
00086      environmentRenderer.setLookPoint( viewSettings.lookPoint );
00087      environmentRenderer.setRenderMode( viewSettings.renderMode );
00088      environmentRenderer.render();
00089      robotRenderer.render();
00090    };
00091 
00092    void SIG_SimulationVisualisation::makeTimeSteps(int noOfTimeSteps)
00093      throw (SIGEL_Simulation::SIG_SimulationCannotSolveException)
00094    {
00095      for (int i=1; i <= noOfTimeSteps; i++)
00096        {
00097          simulation->makeTimeSteps( 1 );
00098 
00099          if ( frameCounter == 0 )
00100            {
00101              DL_vector realRobotPosition;
00102 
00103              DL_vector robotsRealOrigin = robot.initialLocation;
00104              robotsRealOrigin.timesis( -1 );
00105 
00106              this->getRobotRotation().times( &robotsRealOrigin,
00107                                              &realRobotPosition );
00108 
00109              DL_vector robotPosition = this->getRobotPosition();
00110 
00111              realRobotPosition.plusis( &robotPosition );
00112 
00113              environmentRenderer.addRobotPathPoint( realRobotPosition );
00114            };
00115 
00116          frameCounter++;
00117          frameCounter %= robotPathPointAddingRate;
00118        };
00119 
00120      updateRobotLinks();
00121      updateRobotPoints();
00122    };
00123 
00124   void SIG_SimulationVisualisation::updateRobotLinks()
00125   {
00126     int noOfLinks = robot.getLinkIter().count();
00127     for (int i=0; i<noOfLinks; i++)
00128       {
00129         robotRenderer.sceneObjects[i]->setPosition( renderRecorder->robotLinks[i]->position );
00130         robotRenderer.sceneObjects[i]->setRotation( renderRecorder->robotLinks[i]->rotation );
00131       };
00132   };
00133 
00134   void SIG_SimulationVisualisation::updateRobotPoints()
00135   {
00136     int noOfPoints = robotRenderer.sceneObjects.size() - robot.getLinkIter().count();
00137 
00138     for (int i=robot.getLinkIter().count(); i<robotRenderer.sceneObjects.size(); i++)
00139       {
00140         int linkNumber = robotRenderer.sceneObjects[i]->getNumber();
00141 
00142         robotRenderer.sceneObjects[i]->setPosition( renderRecorder->robotLinks[ linkNumber ]->position );
00143         robotRenderer.sceneObjects[i]->setRotation( renderRecorder->robotLinks[ linkNumber ]->rotation );
00144       };
00145   };
00146 
00147   QTime SIG_SimulationVisualisation::getSimulationTime() const
00148   {
00149     return renderRecorder->simulationTime;
00150   };
00151 
00152   DL_vector SIG_SimulationVisualisation::getRobotPosition() const
00153   {
00154     int rootLinkNumber = robot.getRootLink()->getNumber();
00155 
00156     return renderRecorder->robotLinks[ rootLinkNumber ]->position;
00157   };
00158 
00159   DL_matrix SIG_SimulationVisualisation::getRobotRotation() const
00160   {
00161     int rootLinkNumber = robot.getRootLink()->getNumber();
00162 
00163     return renderRecorder->robotLinks[ rootLinkNumber ]->rotation;
00164   };
00165 
00166   bool SIG_SimulationVisualisation::exportToPovray( QString includeFilename,
00167                                                     QString fileName )
00168   {
00169     QFile file( fileName );
00170 
00171     if (!file.open( IO_WriteOnly ))
00172       return false;
00173 
00174     QTextStream stream( &file );
00175 
00176     DL_vector finalEyePoint;
00177     if (viewSettings.relativeEyePoint)
00178       {
00179         finalEyePoint.assign( &viewSettings.lookPoint );
00180         finalEyePoint.plusis( &viewSettings.eyePoint );
00181       }
00182     else
00183       {
00184         finalEyePoint.assign( &viewSettings.eyePoint );
00185       };
00186 
00187     NEWMAT::ColumnVector finalEyePointColumnVector = SIG_TypeConverter::toColumnVector( finalEyePoint );
00188 
00189     NEWMAT::ColumnVector lookPointColumnVector = SIG_TypeConverter::toColumnVector( viewSettings.lookPoint );
00190 
00191     stream << "#include "
00192            << "\""
00193            << includeFilename
00194            << "\"\n"
00195            << "\n"
00196            << environmentRenderer.exportToPovray()
00197            << robotRenderer.exportToPovray()
00198            << "object {\n"
00199            << "  headLight\n"
00200            << "  translate "
00201            << SIG_Renderer::vectorToPovray( SIG_TypeConverter::sigelToPovray() * finalEyePointColumnVector )
00202            << "\n"
00203            << "}\n"
00204            << "\n"
00205            << "camera {\n"
00206            << "  cameraSettings\n"
00207            << "  location "
00208            << SIG_Renderer::vectorToPovray( SIG_TypeConverter::sigelToPovray() * finalEyePointColumnVector )
00209            << "\n"
00210            << "  look_at "
00211            << SIG_Renderer::vectorToPovray( SIG_TypeConverter::sigelToPovray() * lookPointColumnVector )
00212            << "\n"
00213            << "}\n"
00214            << "\n"
00215            << "global_settings { ambient_light rgb "
00216            << SIG_Renderer::vectorToPovray( ambientSceneColor )
00217            << " }\n";
00218 
00219     file.close();
00220 
00221     return true;
00222   };
00223 
00224   bool SIG_SimulationVisualisation::createPovrayIncludeFile( QString fileName,
00225                                                              double aspectRatio )
00226   {
00227     QFile file( fileName );
00228 
00229     if (!file.open( IO_WriteOnly ))
00230       return false;
00231 
00232     QTextStream stream( &file );
00233 
00234     static double const pi = 4 * std::atan( 1 );
00235 
00236     static double const xAngle = 100;
00237 
00238     static double const xAngleRad = (xAngle / 360) * 2 * pi;
00239 
00240     static double const directionLength = 0.5 * 1 / std::tan( xAngleRad / 2 );
00241 
00242     stream << environmentRenderer.createPovrayDeclarations()
00243            << "\n"
00244            << robotRenderer.createPovrayDeclarations()
00245            << "#declare cameraSettings = camera {\n"
00246            << "                            right " << aspectRatio << "*x\n"
00247            << "                            up y\n"
00248            << "                            direction <0,0,"
00249            << directionLength
00250            << ">\n"
00251            << "                          }\n"
00252            << "\n"
00253            << "#declare headLight = light_source {\n"
00254            << "                       <0,0,0>,\n"
00255            << "                       rgb 1\n"
00256            << "                       fade_distance 3\n"
00257            << "                       fade_power 1\n"
00258            << "                     }\n"
00259            << "\n"
00260            << "background { rgb <0.258824,0.258824,0.435294> }\n"
00261            << "\n"
00262            << "fog {\n"
00263            << "  fog_type 1\n"
00264            << "  distance 80\n"
00265            << "  rgb <0.258824,0.258824,0.435294>\n"
00266            << "}\n"
00267            << "\n";
00268 
00269     file.close();
00270 
00271     return true;
00272   };
00273 }

Generated at Mon Sep 3 01:32:32 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000