Beispiel #1
0
bool WorkspaceBound::Intersects(const Arc3D& arc, AngleSet& arcs) const
{
  const AxisSweptPoint& c=arc;
  if(!Intersects(c,arcs)) return false;
  //intersect arcs with the arc's arc
  arcs.Intersect(arc.interval);
  return !arcs.empty();
}
Beispiel #2
0
bool HollowBall::intersects(const Arc3D& arc, AngleSet& arcs) const
{
  const AxisSweptPoint& c=arc;
  if(!intersects(c,arcs)) return false;
  //intersect arcs with the arc's arc
  arcs.Intersect(arc.interval);
  return !arcs.empty();
}
Beispiel #3
0
bool WorkspaceBound::Intersects(const AxisSweptPoint& c, AngleSet& arcs) const
{
  if(balls.empty()) {
    arcs.SetCircle();
    return true;
  }
  if(!balls[0].intersects(c,arcs)) return false;
  AngleSet temp;
  for(size_t i=1;i<balls.size();i++) {
    if(!balls[i].intersects(c,temp)) { return false; }
    arcs.Intersect(temp);
    if(arcs.empty()) return false;
  }
  return true;
}
Beispiel #4
0
//NOTE: requires empty angle sets to have one AngleInterval,
//whose c = Inf and d = closest angle
Real Sample(const vector<AngleSet>& angles,Real qmin,Real qmax)
{
  for(size_t j=0;j<angles.size();j++) {
    Assert(!angles[j].empty());
    if(angles[j].size() > 1) {
      for(size_t k=0;k<angles[j].size();k++)
	Assert(!angles[j][k].isEmpty());
    }
  }
  //try to satisfy all angles simultaneously
  AngleSet s; s.resize(1);
  s[0].setRange(AngleNormalize(qmin),AngleNormalize(qmax));
  for(size_t j=0;j<angles.size();j++) {
    /*
    if(angles[j].size() > 1) {
      cout<<"Intersect "<<s<<" with "<<angles[j]<<endl; 
    }
    */
    s.Intersect(angles[j]);
    /*
    if(angles[j].size() > 1) {
      cout<<"= "<<s<<endl; 
    }
    */
    if(s.empty()) break;
  }
  if(s.empty()) {
    if(angles.size() == 1 && angles[0][0].isEmpty()) {
      //cout<<"Setting automatic point interval "<<angles[0][0]<<endl;
      return FixAngle(angles[0][0].d,qmin,qmax);
    }
    else {
      //we've gotta do some kind of biasing toward valid values
      
      //minimax distance from angles
      //upper envelope of sawtooth function in range 0,2pi
      SawtoothAngleEnvelope temp,env;
      for(size_t j=0;j<angles.size();j++) {
	//in the case of empty angles, the closest valid value is stored in i.d
	if(angles[j][0].isEmpty()) {
	  AngleInterval itemp; itemp.setPoint(angles[j][0].d);
	  temp.setInterval(itemp);
	}
	else {
	  temp.setAngles(angles[j]);
	}

	env.upperEnvelope(temp);
      }

      Real q;
      Real v = env.minimumInterval(qmin,qmax,&q);
      /*
      cout<<"From desired angles in "<<qmin<<" "<<qmax<<endl;
      for(size_t i=0;i<angles.size();i++)
	cout<<angles[i]<<endl;
      cout<<"Sampling "<<q<<endl;
      cout<<"Not satisfying angles by "<<v<<endl;
      */
      return q;
    }
  }
  else {
    return Sample(s);
  }
}