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;
}
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();
}
void AngleSet::Intersect(const AngleInterval& r)
{
AngleSet newIntervals;
AngleInterval temp;
for(size_t i=0; i<size(); i++) {
temp.setIntersection(operator[](i),r);
if(!temp.isEmpty())
newIntervals.push_back(temp);
}
::swap(*this,newIntervals);
}
void AngleSet::Intersect(const vector<AngleInterval>& s)
{
AngleSet newIntervals;
AngleInterval temp;
for(size_t i=0; i<size(); i++) {
for(size_t j=0;j<s.size();j++) {
temp.setIntersection(operator[](i),s[j]);
if(!temp.isEmpty())
newIntervals.push_back(temp);
}
}
::swap(*this,newIntervals);
}
Real Sample(const AngleSet& angles)
{
Real sum=0;
for(size_t i=0;i<angles.size();i++)
sum += angles[i].d;
Real u=Rand()*sum;
for(size_t i=0;i<angles.size();i++) {
if(u <= angles[i].d)
return AngleNormalize(angles[i].c+u);
u -= angles[i].d;
}
AssertNotReached();
return 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;
}
//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);
}
}
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);
}
}