bool SceneDragProjectorImpl::startRotation(const SceneWidgetEvent& event)
{
initialPoint = event.point();
const Vector3 p = initialPosition.translation();
const Vector3 arm = initialPoint - (rotationAxis.dot(initialPoint - p) * rotationAxis + p);
const double armLength = arm.norm();
if(armLength < 1.0e-6){
dragMode = SceneDragProjector::DRAG_NONE;
return false;
}
rotationBaseX = arm.normalized();
rotationBaseY = rotationAxis.cross(rotationBaseX);
const Affine3 C = event.currentCameraPosition();
if(fabs(rotationAxis.dot(SgCamera::direction(C))) > 0.4){
projector.reset(new ScenePlaneProjector(rotationAxis, initialPoint));
} else {
Quat rotation;
rotation.setFromTwoVectors(Vector3::UnitZ(), rotationAxis);
projector.reset(
new SceneCylinderProjector(p, armLength, numeric_limits<double>::max(), rotation));
}
dragMode = SceneDragProjector::DRAG_ROTATION;
return true;
}
bool RectRegionMarkerImpl::onButtonPressEvent(const SceneWidgetEvent& event)
{
x0 = event.x();
y0 = event.y();
setRect(x0, y0, x0, y0);
showRectangle(true);
return true;
}
void RectRegionMarkerImpl::onSceneModeChanged(const SceneWidgetEvent& event)
{
if(event.sceneWidget()->isEditMode()) {
event.sceneWidget()->setCursor(editModeCursor);
} else {
showRectangle(false);
}
}
bool EditableSceneBodyImpl::onPointerMoveEvent(const SceneWidgetEvent& event)
{
if(!bodyItem->isEditable()){
return false;
}
if(dragMode == DRAG_NONE){
PointedType pointedType = findPointedObject(event.nodePath());
if(pointedSceneLink){
if(pointedSceneLink != outlinedLink){
if(outlinedLink){
outlinedLink->showBoundingBox(false);
}
if(!self->body()->isStaticModel()){
pointedSceneLink->showBoundingBox(true);
outlinedLink = pointedSceneLink;
}
}
}
static boost::format f(_("%1% / %2%"));
if(pointedSceneLink){
event.updateIndicator(str(f % bodyItem->name() % pointedSceneLink->link()->name()));
} else {
event.updateIndicator("");
}
} else {
if(!isDragging){
bodyItem->beginKinematicStateEdit();
isDragging = true;
}
switch(dragMode){
case LINK_IK_TRANSLATION:
dragIK(event);
break;
case LINK_FK_ROTATION:
dragFKRotation(event);
break;
case LINK_FK_TRANSLATION:
dragFKTranslation(event);
break;
case ZMP_TRANSLATION:
dragZmpTranslation(event);
break;
default:
break;
}
}
return true;
}
bool EditableSceneBodyImpl::onDoubleClickEvent(const SceneWidgetEvent& event)
{
if(findPointedObject(event.nodePath()) == PT_SCENE_LINK){
if(event.button() == Qt::LeftButton){
if(BodyBar::instance()->makeSingleSelection(bodyItem)){
LinkSelectionView::mainInstance()->makeSingleSelection(
bodyItem, pointedSceneLink->link()->index());
}
return true;
}
}
return false;
}
bool RotationDragger::onButtonPressEvent(const SceneWidgetEvent& event)
{
int axis;
int indexOfTopNode;
if(detectAxisFromNodePath(event.nodePath(), this, axis, indexOfTopNode)){
Affine3 T = calcTotalTransform(event.nodePath(), this);
dragProjector.setInitialPosition(T);
dragProjector.setRotationAxis(T.linear().col(axis));
if(dragProjector.startRotation(event)){
sigRotationStarted_();
return true;
}
}
return false;
}
bool EditableSceneBodyImpl::onKeyPressEvent(const SceneWidgetEvent& event)
{
if(!bodyItem->isEditable()){
return false;
}
if(!outlinedLink || self->body()->isStaticModel()){
return false;
}
bool handled = true;
switch(event.key()){
case Qt::Key_B:
toggleBaseLink(outlinedLink);
break;
case Qt::Key_R:
togglePin(outlinedLink, false, true);
break;
case Qt::Key_T:
togglePin(outlinedLink, true, false);
break;
default:
handled = false;
break;
}
return handled;
}
bool SceneDragProjectorImpl::startTranslation(const SceneWidgetEvent& event)
{
initialPoint = event.point();
switch(translationMode){
case TRANSLATION_1D:
{
const Affine3& C = event.currentCameraPosition();
const Vector3 eye = C.translation();
const Vector3 center = SgCamera::direction(C) + eye;
const Vector3 z = (eye - center).normalized();
const Vector3& a = translationAxis;
if(fabs(a.dot(z)) > 0.99){
return false;
}
const Vector3 normal = (AngleAxis(M_PI / 2.0, z) * a).cross(a).normalized();
projector.reset(new ScenePlaneProjector(normal, initialPoint));
break;
}
case TRANSLATION_2D:
{
projector.reset(new ScenePlaneProjector(translationPlaneNormal, initialPoint));
break;
}
case TRANSLATION_VIEW_PLANE:
{
const Affine3& C = event.currentCameraPosition();
projector.reset(new ScenePlaneProjector(-SgCamera::direction(C), initialPoint));
break;
}
default:
dragMode = SceneDragProjector::DRAG_NONE;
return false;
}
dragMode = SceneDragProjector::DRAG_TRANSLATION;
return true;
}
void EditableSceneBodyImpl::startVirtualElasticString(const SceneWidgetEvent& event)
{
const Vector3& point = event.point();
dragProjector.setInitialTranslation(point);
dragProjector.setTranslationAlongViewPlane();
if(dragProjector.startTranslation(event)){
pointedLinkLocalPoint = targetLink->T().inverse() * point;
dragMode = LINK_VIRTUAL_ELASTIC_STRING;
dragVirtualElasticString(event);
markerGroup->addChildOnce(virtualElasticStringLine, true);
}
}
bool RectRegionMarkerImpl::onButtonReleaseEvent(const SceneWidgetEvent& event)
{
if(left < right && bottom < top) {
Vector3 points[4];
event.sceneWidget()->unproject(left, top, 0.0, points[0]);
event.sceneWidget()->unproject(left, bottom, 0.0, points[1]);
event.sceneWidget()->unproject(right, bottom, 0.0, points[2]);
event.sceneWidget()->unproject(right, top, 0.0, points[3]);
const Vector3 c = event.currentCameraPosition().translation();
SgCamera* camera = event.sceneWidget()->renderer()->currentCamera();
region.clear();
if(dynamic_cast<SgPerspectiveCamera*>(camera)) {
for(int i=0; i < 4; ++i) {
const Vector3 normal = (points[(i + 1) % 4] - c).cross(points[i] - c).normalized();
region.addBoundingPlane(normal, points[i]);
}
} else if(dynamic_cast<SgOrthographicCamera*>(camera)) {
const Vector3 n0 = (points[3] - points[0]).cross(points[1] - points[0]).normalized();
for(int i=0; i < 4; ++i) {
const Vector3 normal = (points[(i + 1) % 4] - points[i]).cross(n0).normalized();
region.addBoundingPlane(normal, points[i]);
}
}
sigRegionFixed(region);
}
showRectangle(false);
return true;
}
void EditableSceneBodyImpl::dragVirtualElasticString(const SceneWidgetEvent& event)
{
if(dragMode == LINK_VIRTUAL_ELASTIC_STRING){
SimulatorItem* simulatorItem = activeSimulatorItem.lock();
if(simulatorItem && dragProjector.dragTranslation(event)){
Vector3 p = targetLink->T() * pointedLinkLocalPoint;
Vector3 d = dragProjector.position().translation() - p;
double k = 2.0;
if(event.modifiers() & Qt::ShiftModifier){
k *= 10.0;
if(event.modifiers() & Qt::ControlModifier){
k *= 10.0;
}
}
Vector3 end = p + k * self->boundingBox().boundingSphereRadius() * d;
SgVertexArray& points = *virtualElasticStringLine->vertices();
points[0] = p.cast<Vector3f::Scalar>();
points[1] = (p + d).cast<Vector3f::Scalar>();
virtualElasticStringLine->notifyUpdate();
simulatorItem->setVirtualElasticString(bodyItem, targetLink, pointedLinkLocalPoint, end);
}
}
}
bool TranslationDragger::onButtonPressEvent(const SceneWidgetEvent& event)
{
int axis;
int indexOfTopNode;
const SgNodePath& path = event.nodePath();
if(detectAxisFromNodePath(path, this, axis, indexOfTopNode)){
SgNodePath::const_iterator axisIter = path.begin() + indexOfTopNode + 1;
const Affine3 T_global = calcTotalTransform(path.begin(), axisIter);
dragProjector.setInitialPosition(T_global);
const Affine3 T_axis = calcTotalTransform(axisIter, path.end());
const Vector3 p_local = (T_global * T_axis).inverse() * event.point();
if(p_local.norm() < 2.0 * axisCylinderNormalizedRadius){
dragProjector.setTranslationAlongViewPlane();
} else {
dragProjector.setTranslationAxis(T_global.linear().col(axis));
}
if(dragProjector.startTranslation(event)){
sigTranslationStarted_();
return true;
}
}
return false;
}
void EditableSceneBodyImpl::onSceneModeChanged(const SceneWidgetEvent& event)
{
if(!bodyItem->isEditable()){
return;
}
isEditMode = event.sceneWidget()->isEditMode();
if(outlinedLink){
if(isEditMode){
outlinedLink->showBoundingBox(true);
} else {
outlinedLink->showBoundingBox(false);
outlinedLink = 0;
}
}
updateMarkersAndManipulators();
}
void EditableSceneBodyImpl::onContextMenuRequest(const SceneWidgetEvent& event, MenuManager& menuManager)
{
if(!bodyItem->isEditable()){
return;
}
if(self->body()->isStaticModel()){
return;
}
PointedType pointedType = findPointedObject(event.nodePath());
if(bodyItem && pointedType == PT_SCENE_LINK){
menuManager.addItem(_("Set Free"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::makeLinkFree, this, pointedSceneLink));
menuManager.addItem(_("Set Base"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::setBaseLink, this, pointedSceneLink));
menuManager.addItem(_("Set Translation Pin"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::togglePin, this, pointedSceneLink, true, false));
menuManager.addItem(_("Set Rotation Pin"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::togglePin, this, pointedSceneLink, false, true));
menuManager.addItem(_("Set Both Pins"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::togglePin, this, pointedSceneLink, true, true));
menuManager.addSeparator();
menuManager.addItem(_("Level Attitude"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::makeLinkAttitudeLevel, this));
menuManager.addSeparator();
menuManager.setPath(N_("/Markers"));
Action* item = menuManager.addCheckItem(_("Center of Mass"));
item->setChecked(isCmVisible);
item->sigToggled().connect(boost::bind(&EditableSceneBodyImpl::showCenterOfMass, this, _1));
item = menuManager.addCheckItem(_("ZMP"));
item->setChecked(isZmpVisible);
item->sigToggled().connect(boost::bind(&EditableSceneBodyImpl::showZmp, this, _1));
menuManager.setPath("/");
menuManager.addSeparator();
}
}
bool EditableSceneBodyImpl::onPointerMoveEvent(const SceneWidgetEvent& event)
{
if(!isEditable()){
return false;
}
if(dragMode == DRAG_NONE){
PointedType pointedType = findPointedObject(event.nodePath());
if(pointedSceneLink){
if(pointedSceneLink != outlinedLink){
if(outlinedLink){
outlinedLink->showBoundingBox(false);
}
pointedSceneLink->showBoundingBox(true);
outlinedLink = pointedSceneLink;
}
}
if(pointedSceneLink){
const Vector3 p = pointedSceneLink->T().inverse() * event.point();
event.updateIndicator(
(str(boost::format("%1% / %2% : (%3$ .3f, %4$ .3f, %5$ .3f)")
% bodyItem->name() % pointedSceneLink->link()->name()
% p.x() % p.y() % p.z())));
} else {
event.updateIndicator("");
}
} else {
if(!isDragging){
bodyItem->beginKinematicStateEdit();
isDragging = true;
}
switch(dragMode){
case LINK_IK_TRANSLATION:
dragIK(event);
break;
case LINK_FK_ROTATION:
dragFKRotation(event);
break;
case LINK_FK_TRANSLATION:
dragFKTranslation(event);
break;
case LINK_VIRTUAL_ELASTIC_STRING:
dragVirtualElasticString(event);
break;
case LINK_FORCED_POSITION:
dragForcedPosition(event);
break;
case ZMP_TRANSLATION:
dragZmpTranslation(event);
break;
default:
break;
}
}
return true;
}
bool RectRegionMarkerImpl::onPointerMoveEvent(const SceneWidgetEvent& event)
{
setRect(x0, y0, event.x(), event.y());
return true;
}
void EditableSceneBodyImpl::onContextMenuRequest(const SceneWidgetEvent& event, MenuManager& menuManager)
{
if(!isEditable()){
return;
}
PointedType pointedType = findPointedObject(event.nodePath());
if(bodyItem && pointedType == PT_SCENE_LINK){
activeSimulatorItem = SimulatorItem::findActiveSimulatorItemFor(bodyItem);
if(activeSimulatorItem){
if(pointedSceneLink->link()->isRoot()){
Action* item1 = menuManager.addCheckItem(_("Move Position"));
item1->setChecked(forcedPositionMode == MOVE_FORCED_POSITION);
item1->sigToggled().connect(
boost::bind(&EditableSceneBodyImpl::setForcedPositionMode, this, MOVE_FORCED_POSITION, _1));
Action* item2 = menuManager.addCheckItem(_("Keep Position"));
item2->setChecked(forcedPositionMode == KEEP_FORCED_POSITION);
item2->sigToggled().connect(
boost::bind(&EditableSceneBodyImpl::setForcedPositionMode, this, KEEP_FORCED_POSITION, _1));
menuManager.addSeparator();
}
} else {
menuManager.addItem(_("Set Free"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::makeLinkFree, this, pointedSceneLink));
menuManager.addItem(_("Set Base"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::setBaseLink, this, pointedSceneLink));
menuManager.addItem(_("Set Translation Pin"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::togglePin, this, pointedSceneLink, true, false));
menuManager.addItem(_("Set Rotation Pin"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::togglePin, this, pointedSceneLink, false, true));
menuManager.addItem(_("Set Both Pins"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::togglePin, this, pointedSceneLink, true, true));
menuManager.addSeparator();
menuManager.addItem(_("Level Attitude"))->sigTriggered().connect(
boost::bind(&EditableSceneBodyImpl::makeLinkAttitudeLevel, this));
menuManager.addSeparator();
}
menuManager.setPath(_("Markers"));
Action* item = menuManager.addCheckItem(_("Center of Mass"));
item->setChecked(isCmVisible);
item->sigToggled().connect(boost::bind(&EditableSceneBodyImpl::showCenterOfMass, this, _1));
item = menuManager.addCheckItem(_("Projection Point of CoM"));
item->setChecked(isPpcomVisible);
item->sigToggled().connect(boost::bind(&EditableSceneBodyImpl::showPpcom, this, _1));
item = menuManager.addCheckItem(_("ZMP"));
item->setChecked(isZmpVisible);
item->sigToggled().connect(boost::bind(&EditableSceneBodyImpl::showZmp, this, _1));
menuManager.setPath("/");
menuManager.addSeparator();
}
}
bool EditableSceneBodyImpl::onButtonPressEvent(const SceneWidgetEvent& event)
{
if(!bodyItem->isEditable()){
return false;
}
if(self->body()->isStaticModel()){
return false;
}
if(outlinedLink){
outlinedLink->showBoundingBox(false);
outlinedLink = 0;
}
PointedType pointedType = findPointedObject(event.nodePath());
if(pointedSceneLink){
pointedSceneLink->showBoundingBox(true);
outlinedLink = pointedSceneLink;
}
bool handled = false;
isDragging = false;
if(pointedType == PT_SCENE_LINK){
if(event.button() == Qt::LeftButton){
targetLink = pointedSceneLink->link();
updateMarkersAndManipulators();
ik.reset();
switch(kinematicsBar->mode()){
case KinematicsBar::AUTO_MODE:
ik = bodyItem->getDefaultIK(targetLink);
if(ik){
attachPositionDragger(targetLink);
startIK(event);
} else {
startFK(event);
}
break;
case KinematicsBar::FK_MODE:
if(targetLink == bodyItem->currentBaseLink()){
startIK(event);
} else {
startFK(event);
}
break;
case KinematicsBar::IK_MODE:
startIK(event);
break;
}
} else if(event.button() == Qt::MiddleButton){
togglePin(pointedSceneLink, true, true);
} else if(event.button() == Qt::RightButton){
}
handled = true;
} else if(pointedType == PT_ZMP){
startZmpTranslation(event);
handled = true;
}
if(dragMode != DRAG_NONE && outlinedLink){
outlinedLink->showBoundingBox(false);
self->notifyUpdate(modified);
}
return handled;
}
bool EditableSceneBodyImpl::onButtonPressEvent(const SceneWidgetEvent& event)
{
if(!isEditable()){
return false;
}
if(outlinedLink){
outlinedLink->showBoundingBox(false);
outlinedLink = 0;
}
PointedType pointedType = findPointedObject(event.nodePath());
if(pointedSceneLink){
pointedSceneLink->showBoundingBox(true);
outlinedLink = pointedSceneLink;
}
bool handled = false;
isDragging = false;
activeSimulatorItem = SimulatorItem::findActiveSimulatorItemFor(bodyItem);
if(activeSimulatorItem){
if(pointedType == PT_SCENE_LINK){
targetLink = pointedSceneLink->link();
if(event.button() == Qt::LeftButton){
if(targetLink->isRoot() && (forcedPositionMode != NO_FORCED_POSITION)){
startForcedPosition(event);
} else {
startVirtualElasticString(event);
}
}
handled = true;
}
} else {
if(pointedType == PT_SCENE_LINK){
if(event.button() == Qt::LeftButton){
targetLink = pointedSceneLink->link();
updateMarkersAndManipulators();
ik.reset();
switch(kinematicsBar->mode()){
case KinematicsBar::AUTO_MODE:
ik = bodyItem->getDefaultIK(targetLink);
if(ik){
startIK(event);
break;
}
case KinematicsBar::FK_MODE:
if(targetLink == bodyItem->currentBaseLink()){
// Translation of the base link
startIK(event);
} else {
startFK(event);
}
break;
case KinematicsBar::IK_MODE:
startIK(event);
break;
}
} else if(event.button() == Qt::MiddleButton){
togglePin(pointedSceneLink, true, true);
} else if(event.button() == Qt::RightButton){
}
handled = true;
} else if(pointedType == PT_ZMP){
startZmpTranslation(event);
handled = true;
}
}
if(dragMode != DRAG_NONE && outlinedLink){
outlinedLink->showBoundingBox(false);
self->notifyUpdate(modified);
}
return handled;
}
void PositionDragger::onSceneModeChanged(const SceneWidgetEvent& event)
{
if(!event.sceneWidget()->isEditMode()){
showDragMarkers(false);
}
}
