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 }