void AngleInterval::setUnion(const AngleInterval& i1, const AngleInterval& i2) { if(i1.isEmpty()) { c=i2.c; d=i2.d; } else if(i2.isEmpty()) { c=i1.c; d=i1.d; } else if(i1.contains(i2.c)) { c = i1.c; d = Max(i1.d,i2.d+AngleCCWDiff(i2.c,i1.c)); } else if(i2.contains(i1.c)) { c = i2.c; d = Max(i2.d,i1.d+AngleCCWDiff(i1.c,i2.c)); } else { //we have 2 choices, pick the smaller interval Real d12 = AngleCCWDiff(i1.c+i1.d,i2.c); Real d21 = AngleCCWDiff(i2.c+i2.d,i1.c); if(d12 < d21) { c = i2.c; d = d12; } else { c = i1.c; d = d21; } } }
TEST(AngleIntervalTest, Containment) { AngleInterval a(0, M_PI, true); AngleInterval b(0, M_PI, false); AngleInterval c(M_PI, 0, true); AngleInterval d(M_PI, 0, false); AngleInterval e = AngleInterval::create_full(M_PI, true); EXPECT_TRUE(a.contains(1.)); EXPECT_FALSE(a.contains(5.)); EXPECT_EQ(a.extent(), M_PI); EXPECT_FALSE(b.contains(1.)); EXPECT_TRUE(b.contains(5.)); EXPECT_EQ(b.extent(), M_PI); EXPECT_FALSE(c.contains(1.)); EXPECT_TRUE(c.contains(5.)); EXPECT_EQ(c.extent(), M_PI); EXPECT_TRUE(d.contains(1.)); EXPECT_FALSE(d.contains(5.)); EXPECT_EQ(d.extent(), M_PI); EXPECT_TRUE(e.contains(1.)); EXPECT_TRUE(e.contains(5.)); EXPECT_EQ(e.extent(), 2*M_PI); }
void SolveCosSinGreater(Real a,Real b,Real c,AngleInterval& i) { Real scale,ofs; TransformCosSin_Cos(a,b,scale,ofs); i.setCosLess(c/scale); i.inplaceShift(-ofs); }
int AngleIntervalSubtract(const AngleInterval& a,const AngleInterval& b,AngleInterval& p,AngleInterval& q) { if(a.intersects(b)) { if(a.contains(b)) { p.setRange(a.c,b.c); q.setRange(AngleNormalize(b.c+b.d),AngleNormalize(a.c+a.d)); return 2; } else if(a.contains(b.c)) { p.setRange(a.c,b.c); return 1; } else if(a.contains(AngleNormalize(b.c+b.d))) { p.setRange(AngleNormalize(b.c+b.d),AngleNormalize(a.c+a.d)); return 1; } else { Assert(b.contains(a)); return 0; } } else { p=a; return 1; } }
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); }
void AngleInterval::setIntersection(const AngleInterval& i1, const AngleInterval& i2) { if(i1.isFull()) { c = i2.c; d = i2.d; } else if(i2.isFull()) { c = i1.c; d = i1.d; } else if(i1.contains(i2.c)) { c = i2.c; d = Min(i2.d,i1.d-AngleCCWDiff(i2.c,i1.c)); } else if(i2.contains(i1.c)) { c = i1.c; d = Min(i1.d,i2.d-AngleCCWDiff(i1.c,i2.c)); } else setEmpty(); if(!i1.contains(*this)) { printf("Error in i1\n"); printf("Intersect %f->%f with %f->%f\n",i1.c,i1.d,i2.c,i2.d); printf("Result: %f->%f\n",c,d); } if(!i2.contains(*this)) { printf("Error in i2\n"); printf("Intersect %f->%f with %f->%f\n",i1.c,i1.d,i2.c,i2.d); printf("Result: %f->%f\n",c,d); } Assert(i1.contains(*this)); Assert(i2.contains(*this)); }
void SawtoothAngleEnvelope::setInterval(const AngleInterval& i) { Assert(!i.isEmpty()); Vector2 amax,amin; amin.set(i.c+Half*i.d,-Half*i.d); amax.set(amin.x+Pi,amin.y+Pi); amin.x = AngleNormalize(amin.x); amax.x = AngleNormalize(amax.x); v.resize(4); if(amin.x < amax.x) { //hi lo hi lo v[0].x=0; v[0].y = amin.y+amin.x; v[1] = amin; v[2] = amax; v[3].x=TwoPi; v[3].y = amax.y+amax.x-TwoPi; Assert(FuzzyEquals(v[3].y,v[0].y)); } else { //lo hi lo hi v[0].x=0; v[0].y = amax.y-amax.x; v[1] = amax; v[2] = amin; v[3].x=TwoPi; v[3].y = amin.y+TwoPi-amin.x; Assert(FuzzyEquals(v[3].y,v[0].y)); } if(v[2].x == v[3].x) v.erase(v.begin()+3); if(v[0].x == v[1].x) v.erase(v.begin()); Assert(isValid()); }
bool AngleInterval::contains(const AngleInterval& i) const { if(i.isEmpty()) return true; if(isFull()) return true; if(isEmpty()) return false; if(i.d > d) return false; //tricky at pi if(i.d == Pi && d == Pi) return c==i.c; Real delta1 = AngleCCWDiff(i.c,c); Real delta2 = AngleCCWDiff(AngleNormalize(i.c+i.d),c); return delta1 <= d+Epsilon && delta2 <=d+Epsilon; }
TEST(AngleIntervalTest, TimeAtAngle) { Coord pi32 = (3./2.)*M_PI; AngleInterval a(M_PI, pi32, true); AngleInterval b(pi32, M_PI, true); AngleInterval c(M_PI, 0, false); AngleInterval d(M_PI/2, M_PI, false); AngleInterval e = AngleInterval::create_full(M_PI, true); AngleInterval f = AngleInterval::create_full(M_PI, false); Interval unit(0, 1); EXPECT_EQ(a.timeAtAngle(M_PI), 0); EXPECT_EQ(a.timeAtAngle(pi32), 1); EXPECT_EQ(a.extent(), M_PI/2); for (Coord t = -1; t <= 2; t += 0.125) { Coord angle = lerp(t, M_PI, pi32); Coord ti = a.timeAtAngle(angle); EXPECT_EQ(unit.contains(ti), a.contains(angle)); EXPECT_FLOAT_EQ(ti, t); } EXPECT_EQ(b.timeAtAngle(pi32), 0); EXPECT_EQ(b.timeAtAngle(M_PI), 1); EXPECT_EQ(b.extent(), pi32); EXPECT_FLOAT_EQ(b.timeAtAngle(M_PI/4), 0.5); EXPECT_FLOAT_EQ(b.timeAtAngle(0), 1./3.); EXPECT_FLOAT_EQ(b.timeAtAngle((11./8)*M_PI), -1./12); for (Coord t = -0.125; t <= 1.125; t += 0.0625) { Coord angle = lerp(t, pi32, 3*M_PI); Coord ti = b.timeAtAngle(angle); EXPECT_EQ(unit.contains(ti), b.contains(angle)); EXPECT_FLOAT_EQ(ti, t); } EXPECT_EQ(c.timeAtAngle(M_PI), 0); EXPECT_EQ(c.timeAtAngle(0), 1); EXPECT_EQ(c.extent(), M_PI); EXPECT_FLOAT_EQ(c.timeAtAngle(M_PI/2), 0.5); for (Coord t = -0.25; t <= 1.25; t += 0.125) { Coord angle = lerp(t, M_PI, 0); Coord ti = c.timeAtAngle(angle); EXPECT_EQ(unit.contains(ti), c.contains(angle)); EXPECT_FLOAT_EQ(ti, t); } EXPECT_EQ(d.timeAtAngle(M_PI/2), 0); EXPECT_EQ(d.timeAtAngle(M_PI), 1); EXPECT_EQ(d.extent(), pi32); EXPECT_FLOAT_EQ(d.timeAtAngle(-M_PI/4), 0.5); for (Coord t = -0.125; t <= 1.125; t += 0.0625) { Coord angle = lerp(t, M_PI/2, -M_PI); Coord ti = d.timeAtAngle(angle); EXPECT_EQ(unit.contains(ti), d.contains(angle)); EXPECT_FLOAT_EQ(ti, t); } EXPECT_EQ(e.timeAtAngle(M_PI), 0); EXPECT_EQ(e.extent(), 2*M_PI); EXPECT_FLOAT_EQ(e.timeAtAngle(0), 0.5); for (Coord t = 0; t < 1; t += 0.125) { Coord angle = lerp(t, M_PI, 3*M_PI); Coord ti = e.timeAtAngle(angle); EXPECT_EQ(unit.contains(ti), true); EXPECT_EQ(e.contains(angle), true); EXPECT_FLOAT_EQ(ti, t); } EXPECT_EQ(f.timeAtAngle(M_PI), 0); EXPECT_EQ(f.extent(), 2*M_PI); EXPECT_FLOAT_EQ(e.timeAtAngle(0), 0.5); for (Coord t = 0; t < 1; t += 0.125) { Coord angle = lerp(t, M_PI, -M_PI); Coord ti = f.timeAtAngle(angle); EXPECT_EQ(unit.contains(ti), true); EXPECT_EQ(f.contains(angle), true); EXPECT_FLOAT_EQ(ti, t); } }
//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); } }
bool AngleInterval::intersects(const AngleInterval& i) const { return contains(i.c) || i.contains(c); }