void MonitorForGrabbingWithTelekinesis::grab() { // grabs nearest object cout << "grabbing nearest object" << endl; btTransform curPose = m_telekinesis ? m_telePose : m_manip->getTransform(); int iNear = -1; BulletObject::Ptr nearestObj = getNearestBody(m_bodies, curPose.getOrigin(), iNear); cout << "grab: " << m_i << endl; if (nearestObj->rigidBody->getCenterOfMassPosition().distance(curPose.getOrigin()) < .05*METERS) { // if (true) { m_grab = new Grab(nearestObj->rigidBody.get(), curPose.getOrigin(), m_world); m_i = iNear; nearestObj->setColor(0,0,1,1); } }
void MonitorForGrabbingWithTelekinesis::grab() { // grabs nearest object cout << "grabbing nearest object" << endl; btTransform curPose = m_telekinesis ? m_telePose : m_manip->getTransform(); int iNear = -1; BulletObject::Ptr nearestObj = getNearestBody(m_bodies, curPose.getOrigin(), iNear); cout << "grab: " << iNear << endl; const int adj_grab = 1; if (inGraspRegion(m_manip, nearestObj->rigidBody->getCenterOfMassPosition(), m_leftFinger, m_rightFinger)) { cout << "grab success!" << endl; for (int i = iNear-adj_grab; i <= iNear+adj_grab; ++i) { if (i < 0 || i >= m_bodies.size()) continue; BulletConstraint::Ptr grab = makeGrabConstraint(m_manip, m_bodies[i]); m_grabs.push_back(grab); m_env->addConstraint(grab); m_bodies[i]->setColor(0,0,1,1); } } }