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();
}
}
}
}