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);
}
