void validity_checker_t::valid_trajectory(const space_t* space, const trajectory_t& input, trajectory_t& result)
{
result.clear();
foreach(state_t* state, input)
{
if( !is_valid(state) )
return;
result.copy_onto_back(state);
}
}
void validity_checker_t::valid_trajectory_prefix(const space_t* space, const trajectory_t& input, trajectory_t& result, unsigned int size)
{
unsigned int traj_size = 0;
result.clear();
foreach(state_t* state, input)
{
if( !is_valid(state) || traj_size == size )
return;
result.copy_onto_back(state);
traj_size++;
}
}
double default_uniform_t::trajectory_cost(const trajectory_t& t)
{
PRX_ASSERT(t.size()>0);
double cost = 0;
trajectory_t::const_iterator i = t.begin();
trajectory_t::const_iterator j = t.begin();
j++;
for ( ; j != t.end(); ++i,++j)
{
cost+=dist(*i,*j)*state_cost(*j);
}
return cost;
}
void validity_checker_t::valid_trajectory(trajectory_t& input)
{
unsigned int traj_size = 0;
foreach(state_t* state, input)
{
if( !is_valid(state) )
break;
traj_size++;
}
input.resize(traj_size);
}
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;
}
int smp::minimum_time_reachability<typeparams,NUM_DIMENSIONS>
::get_solution (trajectory_t &trajectory_out) {
if (!min_cost_vertex)
return 1;
trajectory_out.clear();
for (typename list<state_t*>::iterator it_state = min_cost_trajectory.list_states.begin();
it_state != min_cost_trajectory.list_states.end(); it_state++) {
trajectory_out.list_states.push_front (new state_t(**it_state));
}
for (typename list<input_t*>::iterator it_input = min_cost_trajectory.list_inputs.begin();
it_input != min_cost_trajectory.list_inputs.end(); it_input++) {
trajectory_out.list_inputs.push_front (new input_t(**it_input));
}
return 1;
}