C++ (Cpp) getNearestBody示例

示例#1

文件： rope_scenes.cpp 项目： NaohiroHayashi/bulletsim

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

示例#2

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