int
RRTstar::Planner< typeparams >
::insertTrajectory (vertex_t& vertexStartIn, trajectory_t& trajectoryIn, vertex_t& vertexEndIn) {
// Update the costs
vertexEndIn.costFromParent = trajectoryIn.evaluateCost();
vertexEndIn.costFromRoot = vertexStartIn.costFromRoot + vertexEndIn.costFromParent;
checkUpdateBestVertex (vertexEndIn);
// Update the trajectory between the two vertices
if (vertexEndIn.trajFromParent)
delete vertexEndIn.trajFromParent;
vertexEndIn.trajFromParent = new trajectory_t (trajectoryIn);
// Update the parent to the end vertex
if (vertexEndIn.parent)
vertexEndIn.parent->children.erase (&vertexEndIn);
vertexEndIn.parent = &vertexStartIn;
// Add the end vertex to the set of chilren
vertexStartIn.children.insert (&vertexEndIn);
return 1;
}
