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();
}
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();
}
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;
}
void SawtoothAngleEnvelope::setAngles(const AngleSet& angles)
{
Assert(!angles.empty());
setInterval(angles[0]);
flip_y();
SawtoothAngleEnvelope temp;
for(size_t i=1;i<angles.size();i++) {
temp.setInterval(angles[i]);
temp.flip_y();
upperEnvelope(temp);
}
flip_y();
/*
cout<<"Sawtooth ";
for(size_t i=0;i<v.size();i++) {
cout<<v[i]<<", ";
}
cout<<endl;
*/
}
//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);
}
}