| 167 | } |
| 168 | |
| 169 | bool ccTrace::optimizePath(int maxIterations) |
| 170 | { |
| 171 | bool success = true; |
| 172 | |
| 173 | if (m_waypoints.size() < 2) |
| 174 | { |
| 175 | m_trace.clear(); |
| 176 | return false; //no segments... |
| 177 | } |
| 178 | |
| 179 | #ifdef DEBUG_PATH |
| 180 | int idx = m_cloud->getScalarFieldIndexByName("Search"); //look for scalar field to write search to |
| 181 | if (idx == -1) //doesn't exist - create |
| 182 | idx=m_cloud->addScalarField("Search"); |
| 183 | m_cloud->setCurrentScalarField(idx); |
| 184 | #endif |
| 185 | |
| 186 | //update stored cost function etc. |
| 187 | updateMetadata(); |
| 188 | |
| 189 | //update internal vars |
| 190 | m_maxIterations = maxIterations; |
| 191 | |
| 192 | //loop through segments and build/rebuild trace |
| 193 | int start = 0; |
| 194 | int end = 0; |
| 195 | int tID = 0; |
| 196 | for (unsigned i = 1; i < m_waypoints.size(); i++) |
| 197 | { |
| 198 | //calculate indices |
| 199 | start = m_waypoints[i - 1]; //global point id for the start waypoint |
| 200 | end = m_waypoints[i]; //global point id for the end waypoint |
| 201 | tID = i - 1; //id of the trace segment id (in m_trace vector) |
| 202 | |
| 203 | //are we adding to the end of the trace? |
| 204 | if (tID >= m_trace.size()) |
| 205 | { |
| 206 | std::deque<int> segment = optimizeSegment(start, end, m_search_r); //calculate segment |
| 207 | m_trace.push_back(segment); //store segment |
| 208 | success = success && !segment.empty(); //if the queue is empty, we failed |
| 209 | } else //no... we're somewhere in the middle - update segment if necessary |
| 210 | { |
| 211 | if (!m_trace[tID].empty() && (m_trace[tID][0] == start) && (m_trace[tID][m_trace[tID].size() - 1] == end)) //valid trace and start/end match |
| 212 | continue; //this trace has already been calculated - we can skip! :) |
| 213 | else |
| 214 | { |
| 215 | //calculate segment |
| 216 | std::deque<int> segment = optimizeSegment(start, end, m_search_r); //calculate segment |
| 217 | success = success && !segment.empty(); //if the queue is empty, we failed |
| 218 | |
| 219 | //add trace |
| 220 | if (m_trace[tID][m_trace[tID].size() - 1] == end) //end matches - we can replace the current trace & things should be sweet (all prior traces will have been updated already) |
| 221 | m_trace[tID] = segment; //end is correct - overwrite this block, then hopefully we will match in the next one |
| 222 | else //end doesn't match - we need to insert |
| 223 | m_trace.insert(m_trace.begin()+tID, segment); |
| 224 | } |
| 225 | } |
| 226 | } |
no test coverage detected