C++ (Cpp) Joint::getP3dJointStruct 예제들

예제 #1

파일: collision_space.cpp 프로젝트: jmainpri/libmove3d-planners

void CollisionSpace::addEnvPoints() {
  // Add Static Obstacles
  for (int i = 0; i < XYZ_ENV->no; i++) {
    PointCloud& cloud = m_sampler->getPointCloud(XYZ_ENV->o[i]);

    for (unsigned int j = 0; j < cloud.size(); j++)
      m_points_to_add.push_back(cloud[j]);
  }

  // Add Moving Obstacles
  Scene* sc = global_Project->getActiveScene();

  for (unsigned int i = 0; i < sc->getNumberOfRobots(); i++) {
    Robot* mov_obst = sc->getRobot(i);

    // The robot is not a robot or a human
    if (!((mov_obst->getName().find("ROBOT") != std::string::npos) ||
          (mov_obst->getName().find("HUMAN") != std::string::npos) ||
          mov_obst->getName() == "rob1" || mov_obst->getName() == "rob2" ||
          mov_obst->getName() == "rob3" || mov_obst->getName() == "rob4")) {
      cout << "Adding : " << mov_obst->getName() << endl;

      Joint* jnt = mov_obst->getJoint(1);

      if (static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o == NULL) continue;

      PointCloud& cloud = m_sampler->getPointCloud(
          static_cast<p3d_jnt*>(jnt->getP3dJointStruct())->o);

      for (int j = 0; j < int(cloud.size()); j++) {
        m_points_to_add.push_back(jnt->getMatrixPos() * cloud[j]);
      }
    }
  }
}

예제 #2

파일: libmove3d_api.cpp 프로젝트: jmainpri/libmove3d-planners

Joint* move3d_joint_get_previous_joint(const Joint* J, Robot* R) {
    p3d_jnt* jntPt = (p3d_jnt*)(J->getP3dJointStruct());
    Joint* prevJnt = NULL;
    int found = 0;

    for (unsigned int i = 0; i < R->getNumberOfJoints(); i++) {
        Joint* jnt = R->getJoint(i);

        if (jntPt->prev_jnt == jnt->getP3dJointStruct()) {
            found++;
            prevJnt = jnt;
        }
    }

    if (found == 1) {
        return prevJnt;
    } else if (found > 1) {
        cout << "Found : " << found << " prev. joints!!!" << endl;
    }

    return NULL;
}