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 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 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)); }
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); } }
bool AngleInterval::intersects(const AngleInterval& i) const { return contains(i.c) || i.contains(c); }