diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 4da2d4d3ce..e120740b5f 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -8,6 +8,12 @@ Version 0.142.0 - Add robotControllerAxisManufacturerCode so that servo drives from different manufacturer connected to daisy chain can be handled. - Fix unbounded growth of _vmimic +* Add ViewerBase::SetUserText to customize HUD text size + +Version 0.141.2 +=============== + +* Fix the issue that second-to-last configuration along the given path segment may not be checked in `Check` function. Version 0.141.1 =============== diff --git a/include/openrave/viewer.h b/include/openrave/viewer.h index cce8deec40..e048d8f62b 100644 --- a/include/openrave/viewer.h +++ b/include/openrave/viewer.h @@ -176,6 +176,8 @@ class OPENRAVE_API ViewerBase : public InterfaceBase virtual void SetUserText(const std::string& userText) OPENRAVE_DUMMY_IMPLEMENTATION; + virtual void SetTextSize(double size) OPENRAVE_DUMMY_IMPLEMENTATION; + /// \brief controls showing the viewer. /// /// \param showtype If zero, will hide all viewers. If != 0, should show viewers (dependent on plugin could have different meanings) diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index ccd78c6a76..e3a569a812 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -257,9 +257,14 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) // add the message texts SoSeparator* pmsgsep = new SoSeparator(); - SoTranslation* pmsgtrans0 = new SoTranslation(); - pmsgtrans0->translation.setValue(SbVec3f(-0.978f,0.93f,0)); - pmsgsep->addChild(pmsgtrans0); + + _messageFont = new SoFont(); + _messageFont->size = 10; + pmsgsep->addChild(_messageFont); + + _messageBaseTranslation = new SoTranslation(); + _messageBaseTranslation->translation.setValue(SbVec3f(-0.978f,0.874f,0)); + pmsgsep->addChild(_messageBaseTranslation); SoBaseColor* pcolor0 = new SoBaseColor(); pcolor0->rgb.setValue(0.0f,0.0f,0.0f); pmsgsep->addChild(pcolor0); @@ -267,7 +272,7 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) pmsgsep->addChild(_messageNodes[0]); _messageShadowTranslation = new SoTranslation(); - _messageShadowTranslation->translation.setValue(SbVec3f(-0.002f,0.032f,0)); + _messageShadowTranslation->translation.setValue(SbVec3f(-0.002f,0.0540f,0)); pmsgsep->addChild(_messageShadowTranslation); SoBaseColor* pcolor1 = new SoBaseColor(); pcolor1->rgb.setValue(0.99f,0.99f,0.99f); @@ -275,7 +280,11 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) _messageNodes[1] = new SoText2(); pmsgsep->addChild(_messageNodes[1]); - _ivRoot->addChild(pmsgsep); + _messageSwitch = new SoSwitch(); + _messageSwitch->whichChild.setValue(SO_SWITCH_ALL); + _messageSwitch->addChild(pmsgsep); + + _ivRoot->addChild(_messageSwitch); _ivRoot->addChild(_ivCamera); SoEventCallback * ecb = new SoEventCallback; @@ -640,6 +649,109 @@ void QtCoinViewer::SetUserText(const string& userText) _userText = userText; } +class SetTextSizeMessage : public QtCoinViewer::EnvMessage +{ +public: + SetTextSizeMessage(QtCoinViewerPtr pviewer, void** ppreturn, double size) + : EnvMessage(pviewer, ppreturn, false), _textSize(size) { + } + + virtual void viewerexecute() { + QtCoinViewerPtr pviewer = _pviewer.lock(); + if( !pviewer ) { + return; + } + pviewer->_SetTextSize(_textSize); + EnvMessage::viewerexecute(); + } + +private: + double _textSize; +}; + +void QtCoinViewer::SetTextSize(double size) +{ + if (_timerSensor->isScheduled() && _bUpdateEnvironment) { + EnvMessagePtr pmsg(new SetTextSizeMessage(shared_viewer(), (void**)NULL, size)); + pmsg->callerexecute(false); + } +} + +void QtCoinViewer::_SetTextSize(double size) +{ + if ( size >= 0 ) { + // TODO: use a font that does not rely on hardcoded breakpoints + // move down to next text size breakpoint so that message shadow aligns nicely + _messageFont->size = _GetTextBaseSize(size); + // hide HUD text if requested size is 0 + _messageSwitch->whichChild.setValue(size == 0 ? SO_SWITCH_NONE : SO_SWITCH_ALL); + // adjust the text offsets + _messageBaseTranslation->translation.setValue(_GetMessageBaseTranslation()); + _messageShadowTranslation->translation.setValue(_GetMessageShadowTranslation()); + } +} + +// determines a text size that works best with the default qtcoin font (one of 10px, 14px, 18px, 26px) +// \param size requested text size +double QtCoinViewer::_GetTextBaseSize(double size) +{ + if (size < 14.0) { + return 10.0; + } + if (size < 18.0) { + return 14.0; + } + if (size < 26.0) { + return 18.0; + } + return 26.0; +} + +// based on the current font size of HUD text, computes the 3D offset of the black message "base" +// node relative to the center of the text plane such that the message aligns nicely with +// the upper left corner across all preset text sizes +SbVec3f QtCoinViewer::_GetMessageBaseTranslation() +{ + SbViewportRegion v = _pviewer->getViewportRegion(); + float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; + float size = _messageFont->size.getValue(); + // magic window-size-independent constants that ensure that all preset text sizes + // are roughly vertically aligned by the top of the first line + if (size < 14.0f) { + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.07f*fhratio),0); + } + if (size < 18.0f) { + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.082f*fhratio),0); + } + if (size < 26.0f) { + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.106f*fhratio),0); + } + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.126f*fhratio),0); +} + +// based on the current font size of HUD text, computes the 3D offset of the white message "shadow" +// node relative to the message "base" node such that the shadow lies roughly one pixel leftward +// and upward of the base message, helping to create a shading effect that improves overall HUD text +// visibility +SbVec3f QtCoinViewer::_GetMessageShadowTranslation() +{ + SbViewportRegion v = _pviewer->getViewportRegion(); + float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; + float size = _messageFont->size.getValue(); + // magic window-size-independent constants that ensure that the message shadow + // sits roughly one pixel above the base message text + if (size < 14.0f) { + return SbVec3f(-0.002f*fwratio,(0.032f*fhratio)*(size/10.0f),0); + } + if (size < 18.0f) { + return SbVec3f(-0.002f*fwratio,(0.0448f*fhratio)*(size/14.0f),0); + } + if (size < 26.0f) { + return SbVec3f(-0.002f*fwratio,(0.0540f*fhratio)*(size/18.0f),0); + } + return SbVec3f(-0.002f*fwratio,(0.0777f*fhratio)*(size/26.0f),0); +} + bool QtCoinViewer::LoadModel(const string& pfilename) { SoInput mySceneInput; @@ -2800,10 +2912,9 @@ void QtCoinViewer::AdvanceFrame(bool bForward) ss << _userText << endl; } - // adjust the shadow text - SbViewportRegion v = _pviewer->getViewportRegion(); - float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; - _messageShadowTranslation->translation.setValue(SbVec3f(-0.002f*fwratio,0.032f*fhratio,0)); + // adjust the text offsets + _messageBaseTranslation->translation.setValue(_GetMessageBaseTranslation()); + _messageShadowTranslation->translation.setValue(_GetMessageShadowTranslation()); // search for all new lines string msg = ss.str(); diff --git a/plugins/qtcoinrave/qtcoinviewer.h b/plugins/qtcoinrave/qtcoinviewer.h index 1f8c04166d..064535ba9f 100644 --- a/plugins/qtcoinrave/qtcoinviewer.h +++ b/plugins/qtcoinrave/qtcoinviewer.h @@ -132,6 +132,7 @@ class QtCoinViewer : public QMainWindow, public ViewerBase return _name; } virtual void SetUserText(const string& userText); + virtual void SetTextSize(double size); virtual bool LoadModel(const string& filename); @@ -340,6 +341,10 @@ public slots: virtual void _SetTriangleMesh(SoSeparator* pparent, const float* ppoints, int stride, const int* pIndices, int numTriangles); virtual void _Reset(); virtual void _SetBkgndColor(const RaveVector& color); + virtual void _SetTextSize(double size); + virtual double _GetTextBaseSize(double size); + virtual SbVec3f _GetMessageBaseTranslation(); + virtual SbVec3f _GetMessageShadowTranslation(); virtual void _closegraph(SoSwitch* handle); virtual void _SetGraphTransform(SoSwitch* handle, const RaveTransform& t); virtual void _SetGraphShow(SoSwitch* handle, bool bshow); @@ -412,7 +417,10 @@ public slots: std::list< boost::shared_ptr > _plistdraggers; /// draggers drawn SoEventCallback* _eventKeyboardCB; + SoSwitch* _messageSwitch; + SoFont* _messageFont; boost::array _messageNodes; + SoTranslation* _messageBaseTranslation; SoTranslation* _messageShadowTranslation; bool _altDown[2]; @@ -526,6 +534,7 @@ public slots: friend class DeselectMessage; friend class ResetMessage; friend class SetBkgndColorMessage; + friend class SetTextSizeMessage; friend class StartPlaybackTimerMessage; friend class StopPlaybackTimerMessage; friend class SetGraphTransformMessage; diff --git a/plugins/qtosgrave/osgviewerwidget.cpp b/plugins/qtosgrave/osgviewerwidget.cpp index 837fc14420..def827498d 100644 --- a/plugins/qtosgrave/osgviewerwidget.cpp +++ b/plugins/qtosgrave/osgviewerwidget.cpp @@ -644,6 +644,7 @@ QOSGViewerWidget::QOSGViewerWidget(EnvironmentBasePtr penv, const std::string& u _osgWorldAxis->addChild(CreateOSGXYZAxes(32.0, 2.0)); _vecTextScreenOffset = osg::Vec2(10.0, 0.0); + _hudTextSize = 18.0; if( !!_osgCameraHUD ) { // in order to get the axes to render without lighting: @@ -1163,13 +1164,24 @@ void QOSGViewerWidget::SetViewport(int width, int height) osg::Camera *hudcamera = _osghudview->getCamera(); hudcamera->setViewport(0, 0, width * scale, height * scale); - double textheight = 18*scale; - _osgHudText->setCharacterSize(textheight); - _osgHudText->setFontResolution(textheight, textheight); - SetHUDTextOffset(_vecTextScreenOffset.x(), _vecTextScreenOffset.y()); + SetHUDTextSize(_hudTextSize); _UpdateHUDAxisTransform(width, height); } +void QOSGViewerWidget::SetHUDTextSize(double size) +{ + if ( size >= 0 ) { + _hudTextSize = size; + + float scale = this->devicePixelRatio(); + double textheight = _hudTextSize*scale; + _osgHudText->setCharacterSize(textheight); + _osgHudText->setFontResolution(textheight, textheight); + + SetHUDTextOffset(_vecTextScreenOffset.x(), _vecTextScreenOffset.y()); + } +} + osg::Vec2 QOSGViewerWidget::GetHUDTextOffset() { return osg::Vec2(_vecTextScreenOffset.x(), _vecTextScreenOffset.y()); @@ -1180,7 +1192,7 @@ void QOSGViewerWidget::SetHUDTextOffset(double xOffset, double yOffset) _vecTextScreenOffset.set(xOffset, yOffset); double scale = this->devicePixelRatio(); - double textheight = 18*scale; + double textheight = _hudTextSize*scale; _osgHudText->setPosition( osg::Vec3( -_osgview->getCamera()->getViewport()->width() / 2 + _vecTextScreenOffset.x(), diff --git a/plugins/qtosgrave/osgviewerwidget.h b/plugins/qtosgrave/osgviewerwidget.h index 35e8a2c0a8..aff20ab564 100644 --- a/plugins/qtosgrave/osgviewerwidget.h +++ b/plugins/qtosgrave/osgviewerwidget.h @@ -128,6 +128,9 @@ class QOSGViewerWidget : public QOpenGLWidget /// \brief called when the qt window size changes void SetViewport(int width, int height); + /// \brief sets the HUD text size, scaled off of devicePixelRatio + void SetHUDTextSize(double size); + /// \brief gets the screen offset of HUD text (default with no control buttons is (10.0, 0.0)) osg::Vec2 GetHUDTextOffset(); @@ -345,6 +348,7 @@ class QOSGViewerWidget : public QOpenGLWidget /// causing getProjectionMatrixAsXXX to return negative /// values. Therefore, we manage zNear ourselves double _currentOrthoFrustumSize; ///< coordinate for the right vertical clipping plane + double _hudTextSize; ///< size of HUD text, scaled off of devicePixelRatio void GetSwitchedButtonValue(unsigned int &button); diff --git a/plugins/qtosgrave/qtosgviewer.cpp b/plugins/qtosgrave/qtosgviewer.cpp index 18b269acec..9f3b4423d8 100644 --- a/plugins/qtosgrave/qtosgviewer.cpp +++ b/plugins/qtosgrave/qtosgviewer.cpp @@ -2041,6 +2041,11 @@ void QtOSGViewer::SetUserText(const string& userText) _posgWidget->SetUserHUDText(userText); } +void QtOSGViewer::SetTextSize(double size) +{ + _posgWidget->SetHUDTextSize(size); +} + bool QtOSGViewer::LoadModel(const string& filename) { if( filename == "") { diff --git a/plugins/qtosgrave/qtosgviewer.h b/plugins/qtosgrave/qtosgviewer.h index c02aebc140..c2ea2088c6 100644 --- a/plugins/qtosgrave/qtosgviewer.h +++ b/plugins/qtosgrave/qtosgviewer.h @@ -113,6 +113,9 @@ class QtOSGViewer : public QMainWindow, public ViewerBase /// \brief Set User-defined text to be displayed in the viewer window virtual void SetUserText(const string& userText); + /// \brief Set size of text to be displayed in the viewer window + virtual void SetTextSize(double size); + /// \brief notified when a body has been removed from the environment virtual void RemoveKinBody(KinBodyPtr pbody) { if( !!pbody ) { diff --git a/python/bindings/openravepy_viewer.cpp b/python/bindings/openravepy_viewer.cpp index 2dbbc41934..dcda193b2d 100644 --- a/python/bindings/openravepy_viewer.cpp +++ b/python/bindings/openravepy_viewer.cpp @@ -258,6 +258,9 @@ class PyViewerBase : public PyInterfaceBase void SetUserText(const std::string &userText) { _pviewer->SetUserText(userText); } + void SetTextSize(const double size) { + _pviewer->SetTextSize(size); + } std::string GetName() { return _pviewer->GetName(); } @@ -386,6 +389,7 @@ void init_openravepy_viewer() .def("Show",&PyViewerBase::Show, PY_ARGS("showtype") DOXY_FN(ViewerBase,Show)) .def("SetTitle",&PyViewerBase::SetName, PY_ARGS("title") DOXY_FN(ViewerBase,SetName)) .def("SetUserText",&PyViewerBase::SetUserText, PY_ARGS("userText") DOXY_FN(ViewerBase,SetUserText)) + .def("SetTextSize",&PyViewerBase::SetTextSize, PY_ARGS("size") DOXY_FN(ViewerBase,SetTextSize)) .def("SetName",&PyViewerBase::SetName, PY_ARGS("title") DOXY_FN(ViewerBase,SetName)) .def("GetName",&PyViewerBase::GetName, DOXY_FN(ViewerBase,GetName)) .def("RegisterCallback",&PyViewerBase::RegisterCallback, PY_ARGS("properties", "callback") DOXY_FN(ViewerBase,RegisterItemSelectionCallback)) diff --git a/src/libopenrave/planningutils.cpp b/src/libopenrave/planningutils.cpp index 00efc4680d..64099b1065 100644 --- a/src/libopenrave/planningutils.cpp +++ b/src/libopenrave/planningutils.cpp @@ -3234,33 +3234,29 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } - if( bCheckEnd && bHasNewTempConfigToAdd && bHasRampDeviatedFromInterpolation ) { - dReal dist = params->_distmetricfn(_vtempconfig, q1); + if( bHasNewTempConfigToAdd ) { + // At this point, _vtempconfig is expected to have reached q1. But if not, then need to check _vtempconfig. + const dReal dist = params->_distmetricfn(_vtempconfig, q1); if( dist > 1e-7 ) { - RAVELOG_DEBUG_FORMAT("env=%d, ramp has deviated, so most likely q1 is not following constraints and there's a difference dist=%f", _listCheckBodies.front()->GetEnv()->GetId()%dist); - bCheckEnd = false; // to prevent adding the last point - - if( !!filterreturn ) { - if( options & CFO_FillCheckedConfiguration ) { - int nstateret = 0; - if( istep >= start ) { - nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); - bHasNewTempConfigToAdd = false; - if( !!params->_getstatefn ) { - params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits - } - if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { - filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); - filterreturn->_configurationtimes.push_back(timestep); - } - } - if( nstateret != 0 ) { - if( !!filterreturn ) { - filterreturn->_returncode = nstateret; - } - return nstateret; - } + bCheckEnd = false; // _vtempconfig ends up far from q1 and we haven't checked the segment connecting _vtempconfig and q1 so prevent adding q1 to the list of checked configuration to tell the caller that the checked segment ends here. + int nstateret = 0; + nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); + bHasNewTempConfigToAdd = false; + if( !!params->_getstatefn ) { + params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits + } + if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { + filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); + filterreturn->_configurationtimes.push_back(timestep); + } + if( nstateret != 0 ) { + if( !!filterreturn ) { + filterreturn->_returncode = nstateret; + filterreturn->_invalidvalues = _vtempconfig; + filterreturn->_invalidvelocities = _vtempvelconfig; + filterreturn->_fTimeWhenInvalid = timestep; } + return nstateret; } } } @@ -3277,13 +3273,7 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: *it *= fisteps; } - // just in case, have to set the current values to _vtempconfig since neighstatefn expects the state to be set. - if( params->SetStateValues(_vtempconfig, 0) != 0 ) { - if( !!filterreturn ) { - filterreturn->_returncode = CFO_StateSettingError; - } - return CFO_StateSettingError; - } + const bool validVelocities = (timeelapsed > 0) && (dq0.size() == _vtempconfig.size()) && (dq1.size() == _vtempconfig.size()); _vdiffconfig.resize(dQ.size()); _vstepconfig.resize(dQ.size()); @@ -3366,8 +3356,13 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } // end checking configurations between _vtempconfig2 (the previous _vtempconfig) and _vtempconfig (the new one) } // end if maxnumsteps > 1 + } // end check neighstatus + if( validVelocities ) { + // Compute the next velocity + for( size_t idof = 0; idof < dQ.size(); ++idof ) { + _vtempvelconfig.at(idof) = dq0.at(idof) + _vtempveldelta.at(idof); + } } - // Else, _neighstatefn returns _vtempconfig + dQ. } _vprevtempconfig.resize(dQ.size()); @@ -3485,11 +3480,40 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } // end collision checking } // end if maxnumsteps > 1 + } // end check neighstatus + if( validVelocities ) { + // Compute the next velocity + for( size_t idof = 0; idof < dQ.size(); ++idof ) { + _vtempvelconfig.at(idof) = dq0.at(idof) + dReal(f + 1)*_vtempveldelta.at(idof); + } + } + } // end for + + // At this point, _vtempconfig is not checked yet! + const dReal dist = params->_distmetricfn(_vtempconfig, q1); + if( dist > 1e-7 ) { + // _vtempconfig is different from q1 so must check it. + int nstateret = 0; + nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); + if( !!params->_getstatefn ) { + params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits + } + if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { + filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); + filterreturn->_configurationtimes.push_back(1.0); + } + if( nstateret != 0 ) { + if( !!filterreturn ) { + filterreturn->_returncode = nstateret; + filterreturn->_invalidvalues = _vtempconfig; + if( validVelocities ) { + filterreturn->_invalidvelocities = _vtempvelconfig; + } + filterreturn->_fTimeWhenInvalid = 1.0; + } + return nstateret; } - } - // check if _vtempconfig is close to q1! - { // the neighbor function could be a constraint function and might move _vtempconfig by more than the specified dQ! so double check the straight light distance between them justin case? // TODO check if acceleration limits are satisfied between _vtempconfig, _vprevtempconfig, and _vprevtempvelconfig int numPostNeighSteps = 1; @@ -3505,14 +3529,8 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } if( numPostNeighSteps > 1 ) { - bHasRampDeviatedFromInterpolation = true; - // should never happen, but just in case _neighstatefn is some non-linear constraint projection - if( _listCheckBodies.size() > 0 ) { - RAVELOG_WARN_FORMAT("env=%d, have to divide the arc in %d steps even after original interpolation is done, interval=%d", _listCheckBodies.front()->GetEnv()->GetId()%numPostNeighSteps%interval); - } - else { - RAVELOG_WARN_FORMAT("have to divide the arc in %d steps even after original interpolation is done, interval=%d", numPostNeighSteps%interval); - } + bHasRampDeviatedFromInterpolation = true; // set here again just in case + RAVELOG_WARN_FORMAT("env=%s, have to divide the arc in %d steps even after original interpolation is done, interval=%d", _listCheckBodies.front()->GetEnv()->GetNameId()%numPostNeighSteps%interval); // this case should be rare, so can create a vector here. don't look at constraints since we would never converge... // note that circular constraints would break here @@ -3528,7 +3546,7 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: _vprevtempconfig = _vtempconfig; _vprevtempvelconfig = _vtempvelconfig; // do only numPostNeighSteps-1 since the last step should be checked by _vtempconfig - for(int ipoststep = 0; ipoststep+1 < numPostNeighSteps; ++ipoststep) { + for(int ipoststep = 0; ipoststep < numPostNeighSteps; ++ipoststep) { for( int idof = 0; idof < (int)_vtempconfig.size(); ++idof) { _vprevtempconfig[idof] += vpostdq[idof]; } @@ -3538,24 +3556,21 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } - int nstateret = _SetAndCheckState(params, _vprevtempconfig, _vprevtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); -// if( !!params->_getstatefn ) { -// params->_getstatefn(_vprevtempconfig); // query again in order to get normalizations/joint limits -// } - // since the timeelapsed is not clear, it is dangerous to write filterreturn->_configurations and filterreturn->_configurationtimes since it could force programing using those times to accelerate too fast. so don't write -// if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { -// filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); -// filterreturn->_configurationtimes.push_back(timestep); -// } - if( nstateret != 0 ) { + int npostneighret = _SetAndCheckState(params, _vprevtempconfig, _vprevtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); + if( npostneighret != 0 ) { if( !!filterreturn ) { - filterreturn->_returncode = nstateret; + filterreturn->_returncode = npostneighret; + filterreturn->_invalidvalues = _vprevtempconfig; + if( validVelocities ) { + filterreturn->_invalidvelocities = _vprevtempvelconfig; + } + filterreturn->_fTimeWhenInvalid = 1.0; } - return nstateret; + return npostneighret; } - } - } - } + } // end for ipoststep + } // end numPostNeighSteps > 1 + } // end dist > 1e-7 } if( !!filterreturn ) {