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;
}
