diff options
Diffstat (limited to 'src/uscxml/plugins/invoker/graphics')
-rw-r--r-- | src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.cpp | 300 | ||||
-rw-r--r-- | src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.h | 30 |
2 files changed, 325 insertions, 5 deletions
diff --git a/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.cpp b/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.cpp index 49c9ccb..726cd08 100644 --- a/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.cpp +++ b/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.cpp @@ -1,5 +1,12 @@ #include "OSGConverter.h" #include <glog/logging.h> +#include "uscxml/config.h" + +#include <osg/MatrixTransform> +#include <osgDB/ReadFile> +#include <osgDB/WriteFile> +#include <osgDB/Registry> +#include <osgGA/TrackballManipulator> #ifdef BUILD_AS_PLUGINS #include <Pluma/Connector.hpp> @@ -15,10 +22,15 @@ bool connect(pluma::Host& host) { } #endif -OSGConverter::OSGConverter() { +OSGConverter::OSGConverter() : _isRunning(false) { } OSGConverter::~OSGConverter() { + _isRunning = false; + std::set<tthread::thread*>::iterator threadIter = _threads.begin(); + while(threadIter != _threads.end()) { + (*threadIter)->join(); + } }; boost::shared_ptr<IOProcessorImpl> OSGConverter::create(Interpreter* interpreter) { @@ -33,16 +45,294 @@ Data OSGConverter::getDataModelVariables() { } void OSGConverter::send(const SendRequest& req) { - std::cout << req << std::endl; - Event event; - event.name = "error"; - returnEvent(event); + + /** + * we have to resolve all datamodel dependent strings first as + * we cannot access the datamodel from within another thread without locking + */ + + // make a copy + SendRequest actualReq(req); + + if (actualReq.params.find("source") == actualReq.params.end()) { + // no explicit source + if (actualReq.params.find("sourceexpr") != actualReq.params.end() && _interpreter->getDataModel()) { + actualReq.params.insert(std::make_pair("source", _interpreter->getDataModel().evalAsString(actualReq.params.find("sourceexpr")->second))); + } else { + LOG(ERROR) << "SendRequests for osginvoker missing source or sourceExpr and datamodel"; + return; + } + } + + if (actualReq.params.find("dest") == actualReq.params.end()) { + // no explicit destination + if (actualReq.params.find("destexpr") != actualReq.params.end() && _interpreter->getDataModel()) { + actualReq.params.insert(std::make_pair("dest", _interpreter->getDataModel().evalAsString(actualReq.params.find("destexpr")->second))); + } else { + LOG(ERROR) << "SendRequests for osginvoker missing dest or destExpr and datamodel"; + return; + } + } + + if (actualReq.params.find("format") == actualReq.params.end()) { + // no explicit format + if (actualReq.params.find("formatexpr") != actualReq.params.end() && _interpreter->getDataModel()) { + actualReq.params.insert(std::make_pair("format", _interpreter->getDataModel().evalAsString(actualReq.params.find("formatexpr")->second))); + } else { + std::string format; + size_t lastDot; + std::string dest = req.params.find("dest")->second; + if ((lastDot = dest.find_last_of(".")) != std::string::npos) { + lastDot++; + format = dest.substr(lastDot, dest.length() - lastDot); + } + if (format.length() == 0 || format.find_last_of(PATH_SEPERATOR) != std::string::npos) { + // empty format or pathseperator in format + format = "png"; + } + actualReq.params.insert(std::make_pair("format", format)); + } + } + + if (actualReq.params.find("height") == actualReq.params.end()) { + // no explicit height + if (actualReq.params.find("heightexpr") != actualReq.params.end() && _interpreter->getDataModel()) { + actualReq.params.insert(std::make_pair("height", _interpreter->getDataModel().evalAsString(actualReq.params.find("heightexpr")->second))); + } + } + + if (actualReq.params.find("width") == actualReq.params.end()) { + // no explicit width + if (actualReq.params.find("widthexpr") != actualReq.params.end() && _interpreter->getDataModel()) { + actualReq.params.insert(std::make_pair("width", _interpreter->getDataModel().evalAsString(actualReq.params.find("widthexpr")->second))); + } + } + + _workQueue.push(actualReq); } void OSGConverter::cancel(const std::string sendId) { } void OSGConverter::invoke(const InvokeRequest& req) { + int nrThreads = 1; + if (req.params.find("threads") != req.params.end() && isNumeric(req.params.find("threads")->second.c_str(), 10)) { + nrThreads = strTo<int>(req.params.find("threads")->second); + } + + _isRunning = true; + for (int i = 0; i < nrThreads; i++) { + _threads.insert(new tthread::thread(OSGConverter::run, this)); + } +} + +void OSGConverter::run(void* instance) { + OSGConverter* INSTANCE = (OSGConverter*)instance; + while(true) { + SendRequest req = INSTANCE->_workQueue.pop(); + if (INSTANCE->_isRunning) { + INSTANCE->process(req); + } else { + return; + } + } +} + +void OSGConverter::process(const SendRequest& req) { + + int width = (req.params.find("width") != req.params.end() ? strTo<int>(req.params.find("width")->second) : 640); + int height = (req.params.find("height") != req.params.end() ? strTo<int>(req.params.find("height")->second) : 480); + + assert(req.params.find("source") != req.params.end()); + assert(req.params.find("dest") != req.params.end()); + assert(req.params.find("format") != req.params.end()); + + std::string source = req.params.find("source")->second; + std::string dest = req.params.find("dest")->second; + std::string format = req.params.find("format")->second; + + osg::ref_ptr<osg::Node> sceneGraph = setupGraph(source); + + osgDB::ReaderWriter::WriteResult result; + if (osgDB::Registry::instance()->getReaderWriterForExtension(format) != NULL) { + // write as another 3D file + result = osgDB::Registry::instance()->writeNode(*sceneGraph, dest, osgDB::Registry::instance()->getOptions()); + } + + if (result.error()) { + // make a screenshot + osgViewer::ScreenCaptureHandler::CaptureOperation* cOp = new NameRespectingWriteToFile(dest, + format, + osgViewer::ScreenCaptureHandler::WriteToFile::OVERWRITE + ); + + osgViewer::ScreenCaptureHandler* captureHandler = new osgViewer::ScreenCaptureHandler(cOp, -1); + + osgViewer::Viewer viewer; + viewer.setSceneData(sceneGraph); + viewer.setCameraManipulator(new osgGA::TrackballManipulator()); + viewer.addEventHandler(captureHandler); + captureHandler->startCapture(); + + osg::DisplaySettings* ds = osg::DisplaySettings::instance().get(); + osg::ref_ptr<osg::GraphicsContext::Traits> traits = new osg::GraphicsContext::Traits(ds); + traits->width = width; + traits->height = height; + traits->pbuffer = true; + osg::ref_ptr<osg::GraphicsContext> gc = osg::GraphicsContext::createGraphicsContext(traits.get()); + GLenum pbuffer = gc->getTraits()->doubleBuffer ? GL_BACK : GL_FRONT; + + viewer.getCamera()->setGraphicsContext(gc.get()); + viewer.getCamera()->setViewport(new osg::Viewport(0,0,traits->width,traits->height)); + viewer.getCamera()->setDrawBuffer(pbuffer); + viewer.getCamera()->setReadBuffer(pbuffer); + + // set background color + viewer.getCamera()->setClearColor(osg::Vec4f(1.0f,1.0f,1.0f,1.0f)); + viewer.getCamera()->setClearMask(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + ((osg::MatrixTransform*)sceneGraph.get())->setMatrix(requestToModelPose(req)); + viewer.getCamera()->setViewMatrix(requestToCamPose(req)); + +// viewer.home(); + + // perform one viewer iteration + viewer.realize(); + viewer.frame(); + } +} + +osg::Matrix OSGConverter::requestToModelPose(const SendRequest& req) { + double pitch = (req.params.find("pitch") != req.params.end() ? strTo<int>(req.params.find("pitch")->second) : 0); + double roll = (req.params.find("roll") != req.params.end() ? strTo<int>(req.params.find("roll")->second) : 0); + double yaw = (req.params.find("yaw") != req.params.end() ? strTo<int>(req.params.find("yaw")->second) : 0); + + return eulerToMatrix(pitch, roll, yaw); +// osg::Matrix m; +// m.makeIdentity(); +// return m; +} + +osg::Matrix OSGConverter::requestToCamPose(const SendRequest& req) { + return eulerToMatrix(0, 0, 0); +} + +osg::ref_ptr<osg::Node> OSGConverter::setupGraph(const std::string filename) { + + /** + * root (model pose) + * - modelCenter (center model) + * - model (actual model) + */ + + long now = tthread::chrono::system_clock::now(); + + { + // get some privacy + tthread::lock_guard<tthread::recursive_mutex> lock(_cacheMutex); + + // do we have it in the cache? + if (_models.find(filename) == _models.end()) { + osg::ref_ptr<osg::Node> model = osgDB::readNodeFile(filename); + if (!model.valid()) { + LOG(ERROR) << "Cannot load model from " << filename; + return new osg::MatrixTransform(); + } + _models[filename] = std::make_pair(now, model); + } + + // remove old models from cache + std::map<std::string, std::pair<long, osg::ref_ptr<osg::Node> > >::iterator modelIter = _models.begin(); + while(modelIter != _models.end()) { + // delete every model unused for 1 minutes + if (now - modelIter->second.first > 60000) { + _models.erase(modelIter++); + } else { + modelIter++; + } + } + } + + osg::ref_ptr<osg::MatrixTransform> root = new osg::MatrixTransform(); + + osg::ref_ptr<osg::Node> model = _models[filename].second; + _models[filename].first = now; + + // translation matrix to move model into center + osg::ref_ptr<osg::MatrixTransform> modelCenter = new osg::MatrixTransform(); + modelCenter->addChild(model); + + // move bounding sphere center into origin + osg::BoundingSphere bs = model->getBound(); + modelCenter->setMatrix(osg::Matrix::translate(bs.center() *= -1)); + + // add to model pose matrix + root->addChild(modelCenter); + + return root; +} + +osg::Matrix OSGConverter::eulerToMatrix(double pitch, double roll, double yaw) { + // see http://www.flipcode.com/documents/matrfaq.html#Q36 + osg::Matrix m; + m.makeIdentity(); + + double A = cos(pitch); + double B = sin(pitch); + double C = cos(roll); + double D = sin(roll); + double E = cos(yaw); + double F = sin(yaw); + + double AD = A * D; + double BD = B * D; + + m(0,0) = C * E; + m(0,1) = -C * F; + m(0,2) = -D; + m(1,0) = -BD * E + A * F; + m(1,1) = BD * F + A * E; + m(1,2) = -B * C; + m(2,0) = AD * E + B * F; + m(2,1) = -AD * F + B * E; + m(2,2) = A * C; + + m(0,3) = m(1,3) = m(2,3) = m(3,0) = m(3,1) = m(3,2) = 0; + m(3,3) = 1; + + return m; +} + +void OSGConverter::matrixToEuler(const osg::Matrix& m, double& pitch, double& roll, double& yaw) { + // see: http://www.flipcode.com/documents/matrfaq.html#Q37 + double angle_x, angle_z; + double D = -1 * asin(m(0,2)); /* Calculate Y-axis angle */ + double angle_y = D; + double C = cos(angle_y); + + /* Gimball lock? */ + if ( fabs( C ) > 0.005 ) { + double tr_x = m(2,2) / C; /* No, so get X-axis angle */ + double tr_y = -1 * m(1,2) / C; + angle_x = atan2( tr_y, tr_x ); + tr_x = m(0,0) / C; /* Get Z-axis angle */ + tr_y = -1 * m(0,1) / C; + angle_z = atan2( tr_y, tr_x ); + } else { + /* Gimball lock has occurred */ + angle_x = 0; /* Set X-axis angle to zero */ + double tr_x = m(1,1); /* And calculate Z-axis angle */ + double tr_y = m(1,0); + angle_z = atan2( tr_y, tr_x ); + } + + pitch = fmod(angle_x, 2 * M_PI ); /* Clamp all angles to range */ + roll = fmod( angle_y, 2 * M_PI ); + yaw = fmod( angle_z, 2 * M_PI ); +} + +void OSGConverter::NameRespectingWriteToFile::operator()(const osg::Image& image, const unsigned int context_id) { + osgDB::writeImageFile(image, _filename); } }
\ No newline at end of file diff --git a/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.h b/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.h index c52b1ee..e96a4e9 100644 --- a/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.h +++ b/src/uscxml/plugins/invoker/graphics/openscenegraph/OSGConverter.h @@ -2,6 +2,8 @@ #define OSGCONVERTER_H_W09J90F0 #include <uscxml/Interpreter.h> +#include <osg/Node> +#include <osgViewer/ViewerEventHandlers> #ifdef BUILD_AS_PLUGINS #include "uscxml/plugins/Plugins.h" @@ -29,7 +31,35 @@ public: virtual void cancel(const std::string sendId); virtual void invoke(const InvokeRequest& req); + osg::Matrix requestToModelPose(const SendRequest& req); + osg::Matrix requestToCamPose(const SendRequest& req); + + static osg::Matrix eulerToMatrix(double pitch, double roll, double yaw); + static void matrixToEuler(const osg::Matrix& m, double& pitch, double& roll, double& yaw); + protected: + class NameRespectingWriteToFile : public osgViewer::ScreenCaptureHandler::WriteToFile { + public: + NameRespectingWriteToFile(const std::string& filename, + const std::string& extension, + SavePolicy savePolicy) : osgViewer::ScreenCaptureHandler::WriteToFile(filename, extension, savePolicy) { + } + + virtual void operator()(const osg::Image& image, const unsigned int context_id); + }; + + uscxml::concurrency::BlockingQueue<SendRequest> _workQueue; + osg::ref_ptr<osg::Node> setupGraph(const std::string filename); + + std::map<std::string, std::pair<long, osg::ref_ptr<osg::Node> > > _models; + tthread::recursive_mutex _cacheMutex; + + std::set<tthread::thread*> _threads; + + static void run(void*); + void process(const SendRequest& req); + + bool _isRunning; }; #ifdef BUILD_AS_PLUGINS |