HyperGraphElementAction* VertexPointXYZDrawAction::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; VertexPointXYZ* that = static_cast<VertexPointXYZ*>(element); glPushMatrix(); glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT); glDisable(GL_LIGHTING); glColor3f(LANDMARK_VERTEX_COLOR); float ps = _pointSize ? _pointSize->value() : 1.f; glTranslatef((float)that->estimate()(0),(float)that->estimate()(1),(float)that->estimate()(2)); opengl::drawPoint(ps); glPopAttrib(); drawCache(that->cacheContainer(), params); drawUserData(that->userData(), params); glPopMatrix(); return this; }
HyperGraphElementAction* VertexSE3PriorDrawAction::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; VertexSE3Prior* that = static_cast<VertexSE3Prior*>(element); glColor3f(POSE_VERTEX_PRIOR_COLOR); glPushMatrix(); glMultMatrixd(that->estimate().matrix().data()); opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f); drawCache(that->cacheContainer(), params_); drawUserData(that->userData(), params_); glPopMatrix(); return this; }