EditableSceneBodyImpl::EditableSceneBodyImpl(EditableSceneBody* self, BodyItemPtr& bodyItem) : self(self), bodyItem(bodyItem), kinematicsBar(KinematicsBar::instance()), modified(SgUpdate::MODIFIED) { pointedSceneLink = 0; outlinedLink = 0; targetLink = 0; positionDragger = new PositionDragger; positionDragger->setDraggerAlwaysShown(true); positionDragger->sigDragStarted().connect(boost::bind(&EditableSceneBodyImpl::onDraggerDragStarted, this)); positionDragger->sigPositionDragged().connect(boost::bind(&EditableSceneBodyImpl::onDraggerDragged, this)); positionDragger->sigDragFinished().connect(boost::bind(&EditableSceneBodyImpl::onDraggerDragFinished, this)); dragMode = DRAG_NONE; isDragging = false; isEditMode = false; markerGroup = new SgGroup; markerGroup->setName("Marker"); self->addChild(markerGroup); double radius = 0.0; const int n = self->numSceneLinks(); for(int i=0; i < n; ++i){ SceneLink* sLink = self->sceneLink(i); BoundingBox bb = sLink->boundingBox(); double radius0 = bb.size().norm() / 2.0; if(radius0 > radius){ radius = radius0; } } cmMarker = new CrossMarker(radius, Vector3f(0.0f, 1.0f, 0.0f), 2.0); cmMarker->setName("centerOfMass"); isCmVisible = false; ppcomMarker = new CrossMarker(radius, Vector3f(1.0f, 0.5f, 0.0f), 2.0); ppcomMarker->setName("ProjectionPointCoM"); isPpcomVisible = false; forcedPositionMode = NO_FORCED_POSITION; virtualElasticStringLine = new SgLineSet; virtualElasticStringLine->getOrCreateVertices()->resize(2); virtualElasticStringLine->addLine(0, 1); LeggedBodyHelperPtr legged = getLeggedBodyHelper(self->body()); if(legged->isValid() && legged->numFeet() > 0){ Link* footLink = legged->footLink(0); const double r = calcLinkMarkerRadius(self->sceneLink(footLink->index())); zmpMarker = new SphereMarker(r, Vector3f(0.0f, 1.0f, 0.0f), 0.3); zmpMarker->setName("ZMP"); zmpMarker->addChild(new CrossMarker(r * 2.5, Vector3f(0.0f, 1.0f, 0.0f), 2.0f)); } else { zmpMarker = new SphereMarker(0.1, Vector3f(0.0f, 1.0f, 0.0f), 0.3); } isZmpVisible = false; self->sigGraphConnection().connect(boost::bind(&EditableSceneBodyImpl::onSceneGraphConnection, this, _1)); }
EditableSceneBodyImpl::EditableSceneBodyImpl(EditableSceneBody* self, BodyItemPtr& bodyItem) : self(self), bodyItem(bodyItem), kinematicsBar(KinematicsBar::instance()), modified(SgUpdate::MODIFIED) { pointedSceneLink = 0; outlinedLink = 0; targetLink = 0; positionDragger = new PositionDragger; positionDragger->setDraggerAlwaysShown(true); positionDragger->sigDragStarted().connect(boost::bind(&EditableSceneBodyImpl::onDraggerIKstarted, this)); positionDragger->sigPositionDragged().connect(boost::bind(&EditableSceneBodyImpl::onDraggerIKdragged, this)); dragMode = DRAG_NONE; isDragging = false; isEditMode = false; markerGroup = new SgGroup; markerGroup->setName("Marker"); self->addChild(markerGroup); double radius = 0; const int n = self->numSceneLinks(); for(int i=0; i < n; ++i){ SceneLink* sLink = self->sceneLink(i); BoundingBox bb = sLink->boundingBox(); double radius0 = (bb.max() - bb.center()).norm(); if(radius0 > radius){ radius = radius0; } } cmMarker = new CrossMarker(0.25, Vector3f(0.0f, 1.0f, 0.0f), 2.0); cmMarker->setName("centerOfMass"); cmMarker->setSize(radius); isCmVisible = false; LeggedBodyHelperPtr legged = getLeggedBodyHelper(self->body()); if(legged->isValid() && legged->numFeet() > 0){ Link* footLink = legged->footLink(0); const double r = calcLinkMarkerRadius(self->sceneLink(footLink->index())); zmpMarker = new SphereMarker(r, Vector3f(0.0f, 1.0f, 0.0f), 0.3); zmpMarker->setName("ZMP"); zmpMarker->addChild(new CrossMarker(r * 2.5, Vector3f(0.0f, 1.0f, 0.0f), 2.0f)); } else { zmpMarker = new SphereMarker(0.1, Vector3f(0.0f, 1.0f, 0.0f), 0.3); } isZmpVisible = false; self->sigGraphConnection().connect(boost::bind(&EditableSceneBodyImpl::onSceneGraphConnection, this, _1)); }