bool urdf_traverser::helpers::getSubdirPath(const std::string& from, const std::string& to, std::string& result) { if (!isDirectoryPath(from)) { ROS_ERROR_STREAM("Base path (" << from << ") must be a directory"); throw std::exception(); } if (from == to) { result = "."; return true; } boost::filesystem::path _from(from); boost::filesystem::path _to(to); boost::filesystem::path _absFrom = boost::filesystem::absolute(_from); boost::filesystem::path _absTo = boost::filesystem::absolute(_to); boost::filesystem::path::iterator it(_absFrom.begin()); boost::filesystem::path::iterator it_end(_absFrom.end()); boost::filesystem::path::iterator it2(_absTo.begin()); boost::filesystem::path::iterator it2_end(_absTo.end()); --it_end; // don't go to last entry, which will be either a '.' or a file for (; it != it_end; ++it, ++it2) { if (it2 == it2_end) return false; // cannot be proper sub-directory if (it->string() != it2->string()) { // ROS_INFO_STREAM("Comp: "<<it->string()<<", "<<it2->string()); return false; } } boost::filesystem::path buildPath; for (; it2 != it2_end; ++it2) { buildPath /= *it2; } result = buildPath.string(); // if the path was a directory, remove the last '.' which was inserted if (!result.empty() && (result[result.length() - 1] == '.')) result.erase(result.length() - 1, 1); // ROS_INFO_STREAM("PATH RESULT: "<<result); boost::filesystem::path _res(result); if (!_res.is_relative()) { ROS_ERROR_STREAM("Could not correctly construct a relative path, got " << result << " (input: " << from << " and " << to << ")"); return false; } return true; }
/* * Collide one edgeNode (all edges below it) of this Geom with the triangle * BVH of another geom (b), starting from btriNode. */ void Geom::CollideEdgesTris(int &maxContacts, const BVHNode *edgeNode, const matrix4x4d &transToB, Geom *b, const BVHNode *btriNode, void (*callback)(CollisionContact*)) { if (maxContacts <= 0) return; if (edgeNode->triIndicesStart) { const GeomTree::Edge *edges = this->GetGeomTree()->GetEdges(); int numContacts = 0; vector3f dir; isect_t isect; for (int i=0; i<edgeNode->numTris; i++) { int vtxNum = edges[ edgeNode->triIndicesStart[i] ].v1i; vector3d v1 = transToB * vector3d(&GetGeomTree()->m_vertices[vtxNum]); vector3f _from((float)v1.x, (float)v1.y, (float)v1.z); vector3d _dir( (double)edges[ edgeNode->triIndicesStart[i] ].dir.x, (double)edges[ edgeNode->triIndicesStart[i] ].dir.y, (double)edges[ edgeNode->triIndicesStart[i] ].dir.z); _dir = transToB.ApplyRotationOnly(_dir); dir = vector3f(&_dir.x); isect.dist = edges[ edgeNode->triIndicesStart[i] ].len; isect.triIdx = -1; b->GetGeomTree()->TraceRay(btriNode, _from, dir, &isect); if (isect.triIdx == -1) continue; numContacts++; const double depth = edges[ edgeNode->triIndicesStart[i] ].len - isect.dist; // in world coords CollisionContact contact; contact.pos = b->GetTransform() * (v1 + vector3d(&dir.x)*(double)isect.dist); vector3f n = b->m_geomtree->GetTriNormal(isect.triIdx); contact.normal = vector3d(n.x, n.y, n.z); contact.normal = b->GetTransform().ApplyRotationOnly(contact.normal); contact.dist = isect.dist; contact.depth = depth; contact.triIdx = isect.triIdx; contact.userData1 = m_data; contact.userData2 = b->m_data; // contact geomFlag is bitwise OR of triangle's and edge's flags contact.geomFlag = b->m_geomtree->GetTriFlag(isect.triIdx) | edges[ edgeNode->triIndicesStart[i] ].triFlag; callback(&contact); if (--maxContacts <= 0) return; } } else { CollideEdgesTris(maxContacts, edgeNode->kids[0], transToB, b, btriNode, callback); CollideEdgesTris(maxContacts, edgeNode->kids[1], transToB, b, btriNode, callback); } }
static status_t nfs4_rename(fs_volume* volume, fs_vnode* fromDir, const char* fromName, fs_vnode* toDir, const char* toName) { VnodeToInode* fromVti = reinterpret_cast<VnodeToInode*>(fromDir->private_node); VnodeToInode* toVti = reinterpret_cast<VnodeToInode*>(toDir->private_node); TRACE("volume = %p, fromDir = %" B_PRIi64 ", toDir = %" B_PRIi64 "," \ " fromName = %s, toName = %s", volume, fromVti->ID(), toVti->ID(), \ fromName, toName); VnodeToInodeLocker _from(fromVti); Inode* fromInode = fromVti->Get(); if (fromInode == NULL) return B_ENTRY_NOT_FOUND; VnodeToInodeLocker _to(toVti); Inode* toInode = toVti->Get(); if (toInode == NULL) return B_ENTRY_NOT_FOUND; ino_t id; ino_t oldID; status_t result = Inode::Rename(fromInode, toInode, fromName, toName, false, &id, &oldID); if (result != B_OK) return result; VnodeToInode* vti; if (oldID != 0) { // we have overriden an inode result = acquire_vnode(volume, oldID); if (result == B_OK) { result = get_vnode(volume, oldID, reinterpret_cast<void**>(&vti)); ASSERT(result == B_OK); if (vti->Unlink(toInode->fInfo.fNames, toName)) remove_vnode(volume, oldID); put_vnode(volume, oldID); put_vnode(volume, oldID); } } result = get_vnode(volume, id, reinterpret_cast<void**>(&vti)); if (result == B_OK) { Inode* child = vti->Get(); if (child == NULL) { put_vnode(volume, id); return B_ENTRY_NOT_FOUND; } unremove_vnode(volume, id); child->fInfo.fNames->RemoveName(fromInode->fInfo.fNames, fromName); child->fInfo.fNames->AddName(toInode->fInfo.fNames, toName); put_vnode(volume, id); } return B_OK; }