void WallOverlapComputation::findOverlapPoints()
{
// convert to list polygons for insertion of points
convertPolygonsToLists(polygons, list_polygons);
// init loc_to_list_poly_idx
// for (unsigned int poly_idx = 0; poly_idx < list_polygons.size(); poly_idx++)
// {
// for (ListPolygon::iterator it = list_polygons[poly_idx].begin(); it != list_polygons[poly_idx].end(); ++it)
// {
// loc_to_list_poly_idx.emplace(*it, it);
// }
// }
for (unsigned int poly_idx = 0; poly_idx < list_polygons.size(); poly_idx++)
{
ListPolygon& poly = list_polygons[poly_idx];
for (unsigned int poly2_idx = 0; poly2_idx <= poly_idx; poly2_idx++)
{
for (ListPolygon::iterator it = poly.begin(); it != poly.end(); ++it)
{
ListPolyIt lpi(poly, it);
if (poly_idx == poly2_idx)
{
findOverlapPoints(lpi, poly2_idx, it);
}
else
{
findOverlapPoints(lpi, poly2_idx);
}
}
}
}
// convert list polygons back
for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++)
{
convertListPolygonToPolygon(list_polygons[poly_idx], polygons[poly_idx]);
// DEBUG_SHOW(list_polygons[poly_idx].size());
}
// for (unsigned int poly_idx = 0; poly_idx < polygons.size(); poly_idx++)
// {
// PolygonRef poly = polygons[poly_idx];
// for (unsigned int poly2_idx = 0; poly2_idx <= poly_idx; poly2_idx++)
// {
// PolygonRef poly2 = polygons[poly2_idx];
// for (unsigned int point_idx = 0; point_idx < poly.size(); point_idx++)
// {
// Point& p = poly[point_idx];
// findOverlapPoints(p, poly2);
// }
// }
// }
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "cb_local_planner_interface");
// Transform listener
tf::TransformListener tf(ros::Duration(10));
// Setup the global costmap
costmap_2d::Costmap2DROS costmap("local_costmap", tf);
// Create the base controller interface
cb_local_planner::LocalPlannerInterface lpi(&costmap);
ros::spin();
return 0;
}
void Foam::reconstructLagrangianPositions
(
const polyMesh& mesh,
const word& cloudName,
const PtrList<fvMesh>& meshes,
const PtrList<labelIOList>& faceProcAddressing,
const PtrList<labelIOList>& cellProcAddressing
)
{
Cloud<passiveParticle> lagrangianPositions
(
mesh,
cloudName,
IDLList<passiveParticle>()
);
forAll(meshes, i)
{
const labelList& cellMap = cellProcAddressing[i];
Cloud<passiveParticle> lpi(meshes[i], cloudName, false);
forAllIter(Cloud<passiveParticle>, lpi, iter)
{
const passiveParticle& ppi = iter();
lagrangianPositions.append
(
new passiveParticle
(
lagrangianPositions,
ppi.position(),
cellMap[ppi.cell()]
)
);
}
}
IOPosition<passiveParticle>(lagrangianPositions).write();
}
/*! Dumps the internal structures. For debugging only! */
void TagFileParser::dump()
{
msg("Result:\n");
QListIterator<TagClassInfo> lci(m_tagFileClasses);
//============== CLASSES
TagClassInfo *cd;
for (;(cd=lci.current());++lci)
{
msg("class `%s'\n",cd->name.data());
msg(" filename `%s'\n",cd->filename.data());
if (cd->bases)
{
QListIterator<BaseInfo> bii(*cd->bases);
BaseInfo *bi;
for ( bii.toFirst() ; (bi=bii.current()) ; ++bii)
{
msg( " base: %s \n", bi->name.data() );
}
}
QListIterator<TagMemberInfo> mci(cd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
}
//============== NAMESPACES
QListIterator<TagNamespaceInfo> lni(m_tagFileNamespaces);
TagNamespaceInfo *nd;
for (;(nd=lni.current());++lni)
{
msg("namespace `%s'\n",nd->name.data());
msg(" filename `%s'\n",nd->filename.data());
QStringList::Iterator it;
for ( it = nd->classList.begin();
it != nd->classList.end(); ++it )
{
msg( " class: %s \n", (*it).latin1() );
}
QListIterator<TagMemberInfo> mci(nd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
}
//============== FILES
QListIterator<TagFileInfo> lfi(m_tagFileFiles);
TagFileInfo *fd;
for (;(fd=lfi.current());++lfi)
{
msg("file `%s'\n",fd->name.data());
msg(" filename `%s'\n",fd->filename.data());
QStringList::Iterator it;
for ( it = fd->namespaceList.begin();
it != fd->namespaceList.end(); ++it )
{
msg( " namespace: %s \n", (*it).latin1() );
}
for ( it = fd->classList.begin();
it != fd->classList.end(); ++it )
{
msg( " class: %s \n", (*it).latin1() );
}
QListIterator<TagMemberInfo> mci(fd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
QListIterator<TagIncludeInfo> mii(fd->includes);
TagIncludeInfo *ii;
for (;(ii=mii.current());++mii)
{
msg(" includes id: %s name: %s\n",ii->id.data(),ii->name.data());
}
}
//============== GROUPS
QListIterator<TagGroupInfo> lgi(m_tagFileGroups);
TagGroupInfo *gd;
for (;(gd=lgi.current());++lgi)
{
msg("group `%s'\n",gd->name.data());
msg(" filename `%s'\n",gd->filename.data());
QStringList::Iterator it;
for ( it = gd->namespaceList.begin();
it != gd->namespaceList.end(); ++it )
{
msg( " namespace: %s \n", (*it).latin1() );
}
for ( it = gd->classList.begin();
it != gd->classList.end(); ++it )
{
msg( " class: %s \n", (*it).latin1() );
}
for ( it = gd->fileList.begin();
it != gd->fileList.end(); ++it )
{
msg( " file: %s \n", (*it).latin1() );
}
for ( it = gd->subgroupList.begin();
it != gd->subgroupList.end(); ++it )
{
msg( " subgroup: %s \n", (*it).latin1() );
}
for ( it = gd->pageList.begin();
it != gd->pageList.end(); ++it )
{
msg( " page: %s \n", (*it).latin1() );
}
QListIterator<TagMemberInfo> mci(gd->members);
TagMemberInfo *md;
for (;(md=mci.current());++mci)
{
msg(" member:\n");
msg(" kind: `%s'\n",md->kind.data());
msg(" name: `%s'\n",md->name.data());
msg(" anchor: `%s'\n",md->anchor.data());
msg(" arglist: `%s'\n",md->arglist.data());
}
}
//============== PAGES
QListIterator<TagPageInfo> lpi(m_tagFilePages);
TagPageInfo *pd;
for (;(pd=lpi.current());++lpi)
{
msg("page `%s'\n",pd->name.data());
msg(" title `%s'\n",pd->title.data());
msg(" filename `%s'\n",pd->filename.data());
}
//============== DIRS
QListIterator<TagDirInfo> ldi(m_tagFileDirs);
TagDirInfo *dd;
for (;(dd=ldi.current());++ldi)
{
msg("dir `%s'\n",dd->name.data());
msg(" path `%s'\n",dd->path.data());
QStringList::Iterator it;
for ( it = dd->fileList.begin();
it != dd->fileList.end(); ++it )
{
msg( " file: %s \n", (*it).latin1() );
}
for ( it = dd->subdirList.begin();
it != dd->subdirList.end(); ++it )
{
msg( " subdir: %s \n", (*it).latin1() );
}
}
}
