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