HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; initializeDrawActionsCache(); refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; VertexSE3* that = static_cast<VertexSE3*>(element); glColor3f(POSE_VERTEX_COLOR); glPushMatrix(); glMultMatrixd(that->estimate().matrix().data()); opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f); drawCache(that->cacheContainer(), params_); drawUserData(that->userData(), params_); if(_showId && _showId->value()){ float scale = _idSize ? _idSize->value() : 1.f; drawId(std::to_string(that->id()), scale); } glPopMatrix(); return this; }
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; if (! _cacheDrawActions){ _cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw"); } refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; VertexSE3* that = static_cast<VertexSE3*>(element); glColor3f(0.5,0.5,0.8); glPushMatrix(); glTranslatef(that->estimate().translation().x(),that->estimate().translation().y(),that->estimate().translation().z()); AngleAxisd aa(that->estimate().rotation()); glRotatef(RAD2DEG(aa.angle()),aa.axis().x(),aa.axis().y(),aa.axis().z()); if (_triangleX && _triangleY){ drawTriangle(_triangleX->value(), _triangleY->value()); } CacheContainer* caches=that->cacheContainer(); if (caches){ for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){ Cache* c = it->second; (*_cacheDrawActions)(c, params_); } } glPopMatrix(); return this; }
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; if (! _cacheDrawActions){ _cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw"); } refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; VertexSE3* that = static_cast<VertexSE3*>(element); glColor3f(0.5f,0.5f,0.8f); glPushMatrix(); glMultMatrixd(that->estimate().matrix().data()); if (_triangleX && _triangleY){ drawTriangle(_triangleX->value(), _triangleY->value()); } CacheContainer* caches=that->cacheContainer(); if (caches){ for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){ Cache* c = it->second; (*_cacheDrawActions)(c, params_); } } OptimizableGraph::Data* d=that->userData(); while (d && _cacheDrawActions ){ (*_cacheDrawActions)(d, params_); d=d->next(); } glPopMatrix(); return this; }