void srs_env_model::EMOcTree::insertColoredScan(const typePointCloud& coloredScan,
const octomap::point3d& sensor_origin, double maxrange, bool pruning,
bool lazy_eval) {
// convert colored scan to octomap pcl
octomap::Pointcloud scan;
octomap::pointcloudPCLToOctomap(coloredScan, scan);
// insert octomap pcl to count new probabilities
octomap::KeySet free_cells, occupied_cells;
computeUpdate(scan, sensor_origin, free_cells, occupied_cells, maxrange);
// insert data into tree -----------------------
for (octomap::KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
updateNode(*it, false, lazy_eval);
}
for (octomap::KeySet::iterator it = occupied_cells.begin(); it
!= occupied_cells.end(); ++it) {
updateNode(*it, true, lazy_eval);
}
// TODO: does pruning make sense if we used "lazy_eval"?
if (pruning)
this->prune();
// update node colors
BOOST_FOREACH (const pcl::PointXYZRGB& pt, coloredScan.points)
{
// averageNodeColor or integrateNodeColor can be used to count final color
averageNodeColor(pt.x, pt.y, pt.z, (unsigned char)pt.r, (unsigned char)pt.g, (unsigned char)pt.b, (unsigned char)255);
}
}
void PatchGodunov::updateState(FArrayBox& a_U,
FluxBox& a_F,
Real& a_maxWaveSpeed,
const FArrayBox& a_S,
const Real& a_dt,
const Box& a_box)
{
CH_assert(isDefined());
CH_assert(a_box == m_currentBox);
int numPrim = m_gdnvPhysics->numPrimitives();
int numFlux = m_gdnvPhysics->numFluxes();
FluxBox whalf(a_box,numPrim);
whalf.setVal(0.0);
a_F.resize(a_box,numFlux);
a_F.setVal(0.0);
computeWHalf(whalf, a_U, a_S, a_dt, a_box);
FArrayBox dU(a_U.box(),a_U.nComp());
computeUpdate(dU, a_F, a_U, whalf, a_dt, a_box);
a_U += dU;
// Get and return the maximum wave speed on this patch/grid
a_maxWaveSpeed = m_gdnvPhysics->getMaxWaveSpeed(a_U, m_currentBox);
}
