HyperGraphElementAction* CacheSE3OffsetDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params){ if (typeid(*element).name()!=_typeName) return 0; CacheSE3Offset* that = static_cast<CacheSE3Offset*>(element); refreshPropertyPtrs(params); if (! _previousParams) return this; if (_show && !_show->value()) return this; glPushMatrix(); const Vector3d& offsetT=that->offsetParam()->offset().translation(); AngleAxisd aa(that->offsetParam()->offset().rotation()); glTranslatef(offsetT.x(), offsetT.y(), offsetT.z()); glRotatef(RAD2DEG(aa.angle()),aa.axis().x(),aa.axis().y(),aa.axis().z()); // if (_cubeSide) // drawMyPyramid(_cubeSide->value(), _cubeSide->value()); glPopMatrix(); return this; }
HyperGraphElementAction* CacheSE3OffsetDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; CacheSE3Offset* that = static_cast<CacheSE3Offset*>(element); refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; float cs = _cubeSide ? _cubeSide->value() : 1.0f; glPushAttrib(GL_COLOR); glColor3f(POSE_PARAMETER_COLOR); glPushMatrix(); glMultMatrixd(that->offsetParam()->offset().data()); opengl::drawBox(cs,cs,cs); glPopMatrix(); glPopAttrib(); return this; }