Esempio n. 1
0
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();
        }
    }
}
Esempio n. 2
0
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();
            }
        }
    }
}