inline void
rdepth_estimator::update(V& particles, i_float2 stab)
{
for(unsigned i = 0; i < particles.size(); i++)
{
auto& p = particles[i];
i_float2 pos = p.pos() - foe_;
int age = p.age >= 3 ? (3) : p.age();
//i_float2 speed =i_float2(p.pos_history[age-1] - p.pos()) / float(age);
i_float2 speed =i_float2(p.speed() - stab);
double d;
bool to_far = false;
if (norml2(speed) < .4f)
{
d = 10000.f;
}
else
d = norml2(pos) / norml2(speed);
if (p.age < 1)
p.depth = d;
else
p.depth = p.depth * 0.9f + d * 0.1f;
}
}
bool ContactTransition::IsGoal( ContactTransition &nodeGoal ){
double x = this->g.getX();
double xg = nodeGoal.g.getX();
double y = this->g.getY();
double yg = nodeGoal.g.getY();
double t = this->g.getYawRadian();
double tg = nodeGoal.g.getYawRadian();
//return norml2(x,xg,y,yg) < 0.22 && sqrtf( (this->rel_yaw-yawg)*(this->rel_yaw-yawg))<M_PI/2;
return norml2(x,xg,y,yg) <= 0.15 && sqrtf( (t-tg)*(t-tg) ) <= M_PI/4;
}
double ContactTransition::GoalDistanceEstimate( ContactTransition &nodeGoal ){
//heuristic = distance to goal (classical l2 norm)
double xg = nodeGoal.g.getX();
double yg = nodeGoal.g.getY();
double yawg = nodeGoal.g.getYawRadian();
double x = this->g.getX();
double y = this->g.getY();
double t = this->g.getYawRadian();
double dn = norml2(x,xg,y,yg) + 0.05*sqrtf((t-yawg)*(t-yawg));
//using a penalty term in the vicinity of the goal to align the robot body
//with the goal direction. addditional, we assure that the heuristic has a
//smooth transition at the threshold
double threshold = 0.05;
if(dn > threshold){
return dn;
}else{
double dy = sqrt((t-yawg)*(t-yawg));
double maxy = sqrt((yawg-M_PI)*(yawg-M_PI));
double weightDist = 0.7;
return threshold*(weightDist*dn/threshold+ (1.0-weightDist)*dy/maxy)*0.5;
}
}
