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