/** \brief Create a single Cell by initializing his parameters * * \param char : a letter * \return Cell * */ Cell createCell(char c) { Cell cell; cell.letter = c; cell.score = scoreCell(c); cell.isVisited = 0; strcpy(cell.bonus, " "); return cell; }
double MapGridCostFunction::scoreTrajectory(Trajectory &traj) { double cost = 0.0; if (aggregationType_ == Product) { cost = 1.0; } double px, py, pth; double grid_dist; for (unsigned int i = 0; i < traj.getPointsSize(); ++i) { traj.getPoint(i, px, py, pth); grid_dist = scoreCell(px, py, pth); if(stop_on_failure_){ if (grid_dist == map_.obstacleCosts()) { return -3.0; } else if (grid_dist == map_.unreachableCellCosts()) { return -2.0; } } switch( aggregationType_ ) { case Last: cost = grid_dist; break; case Sum: cost += grid_dist; break; case Product: if (cost > 0) { cost *= grid_dist; } break; } } double factor = costmap_->getResolution() * 0.5; return cost * factor; }