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