bool HollowBall::intersects(const AxisSweptPoint& c, AngleSet& arcs) const
{
Vector3 ccenter = c.center();
AngleInterval i,j;
i = AngleBracket_3D_Ball(c.p-ccenter,c.axis.direction,
center-ccenter,outerRadius+wsEpsilon);
if(i.isEmpty()) { arcs.resize(1); arcs[0]=i; return false; }
arcs.resize(1);
arcs[0]=i;
if(innerRadius > 0) {
j = AngleBracket_3D_Ball(c.p-ccenter,c.axis.direction,
center-ccenter,innerRadius-wsEpsilon);
if(!j.isEmpty()) {
AngleInterval p,q;
int num=AngleIntervalSubtract(i,j,p,q);
//cout<<"Subtracting interval!"<<endl;
//cout<<i<<" - "<<j<<endl;
//cout<<" = "<<p<<", "<<q<<endl;
Assert(num == 2);
arcs.resize(num);
arcs[0]=p;
arcs[1]=q;
}
}
return true;
}
//returns either the set of angles that allow a to intersect with b,
//or if no intersection, returns in i[0].c = Inf, i[0].d = angle of
//the closest point
void IntersectOrClosestAngle(const WorkspaceBound& b,const AxisSweptPoint& a,AngleSet& i)
{
if(!b.Intersects(a,i)) {
i.resize(1);
i[0].c = Inf;
i[0].d = b.ClosestPoint(a);
}
}
//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);
}
}
