//returns the cspace distance of the path
Real KinodynamicMilestonePath::PathLength() const
{
Real l=0;
for(size_t i=0;i<paths.size();i++) {
CSpace* c = Space();
for(size_t j=0;j+1<paths[i].size();j++)
l += c->Distance(paths[i][j],paths[i][j+1]);
}
return l;
}
int PointToSetMotionPlanner::AddMilestone(const Config& q)
{
int n=mp->AddMilestone(q);
if(goalSpace->IsFeasible(q))
goalNodes.push_back(n);
return n;
}
void ApplyTo(CSpace& c_space, CDirectionalLEDEntity& c_entity) {
/* Disable the entity - this ensures that the entity is
* removed from the directional LED medium */
c_entity.Disable();
/* Remove the directional LED entity from space */
c_space.RemoveEntity(c_entity);
}
void ApplyTo(CSpace& c_space, CDirectionalLEDEntity& c_entity) {
/* Add entity to space - this ensures that the directional LED entity
* gets an id before being added to the directional LED medium */
c_space.AddEntity(c_entity);
/* Enable the directional LED entity, if it's enabled - this ensures that
* the entity gets added to the directional LED medium if it's enabled */
c_entity.SetEnabled(c_entity.IsEnabled());
}
void TimedMilestonePath::Split(Real dt,TimedMilestonePath& before,TimedMilestonePath& after) const
{
before.edges.resize(0);
before.durations.resize(0);
after.edges.resize(0);
after.durations.resize(0);
CSpace* cspace = Space();
if(dt < 0) { //dt is before path
before.edges.push_back(cspace->LocalPlanner(Begin(),Begin()));
before.durations.push_back(0);
after.edges.push_back(cspace->LocalPlanner(Begin(),Begin()));
after.durations.push_back(-dt);
}
for(size_t i=0;i<edges.size();i++) {
if(dt < 0) {
after.edges.push_back(edges[i]);
after.durations.push_back(durations[i]);
}
else {
if(dt <= durations[i]) {
//cut current path
Config x;
if(durations[i] == 0) x=edges[i]->Start();
else edges[i]->Eval(dt/durations[i],x);
before.edges.push_back(cspace->LocalPlanner(edges[i]->Start(),x));
before.durations.push_back(dt);
after.edges.push_back(cspace->LocalPlanner(x,edges[i]->Goal()));
after.durations.push_back(durations[i]-dt);
}
else {
before.edges.push_back(edges[i]);
before.durations.push_back(durations[i]);
}
dt -= durations[i];
}
}
if(dt > 0) { //dt is longer than path
before.edges.push_back(cspace->LocalPlanner(End(),End()));
before.durations.push_back(dt);
after.edges.push_back(cspace->LocalPlanner(End(),End()));
after.durations.push_back(0);
}
}
int PointToSetMotionPlanner::PlanMore()
{
if(mp->CanAddMilestone()) {
sampleGoalCounter++;
if(sampleGoalCounter >= sampleGoalPeriod*((int)goalNodes.size()+1)) {
sampleGoalCounter = 0;
Config q;
if(SampleGoal(q)) {
return AddMilestone(q);
}
else
return -1;
}
}
int res = mp->PlanMore();
if(res >= 0) {
Config q;
mp->GetMilestone(res,q);
if(goalSpace->IsFeasible(q))
goalNodes.push_back(res);
}
return res;
}
bool PointToSetMotionPlanner::SampleGoal(Config& q)
{
goalSpace->Sample(q);
return goalSpace->IsFeasible(q);
}
void ApplyTo(CSpace& c_space, CLEDEntity& c_entity) {
if(c_space.IsUsingSpaceHash()) {
c_space.GetLEDEntitiesSpaceHash().RemoveElement(c_entity);
}
c_space.RemoveEntity(c_entity);
}
void ApplyTo(CSpace& c_space, CLEDEntity& c_entity) {
c_space.AddEntity(c_entity);
if(c_space.IsUsingSpaceHash()) {
c_space.GetLEDEntitiesSpaceHash().AddElement(c_entity);
}
}
