void BodyLinkViewImpl::doInverseKinematics(Vector3 p, Matrix3 R) { InverseKinematicsPtr ik = currentBodyItem->getCurrentIK(currentLink); if(ik){ currentBodyItem->beginKinematicStateEdit(); if(KinematicsBar::instance()->isPenetrationBlockMode()){ PenetrationBlockerPtr blocker = currentBodyItem->createPenetrationBlocker(currentLink, true); if(blocker){ Position T; T.translation() = p; T.linear() = R; if(blocker->adjust(T, Vector3(p - currentLink->p()))){ p = T.translation(); R = T.linear(); } } } if(ik->calcInverseKinematics(p, R)){ currentBodyItem->notifyKinematicStateChange(true); currentBodyItem->acceptKinematicStateEdit(); } } }
void EditableSceneBodyImpl::makeLinkAttitudeLevel() { if(pointedSceneLink){ Link* targetLink = outlinedLink->link(); InverseKinematicsPtr ik = bodyItem->getCurrentIK(targetLink); if(ik){ const Position& T = targetLink->T(); const double theta = acos(T(2, 2)); const Vector3 z(T(0,2), T(1, 2), T(2, 2)); const Vector3 axis = z.cross(Vector3::UnitZ()).normalized(); const Matrix3 R2 = AngleAxisd(theta, axis) * T.linear(); bodyItem->beginKinematicStateEdit(); if(ik->calcInverseKinematics(targetLink->p(), R2)){ bodyItem->notifyKinematicStateChange(true); bodyItem->acceptKinematicStateEdit(); } } } }