Refactor VO tests (#7)

* Intermediate commit

* Fix failing ORCA tests due to floating point error

* Rename bound.Within -> bound.In

* Fix end-state jitter

* Refactor ORCA agent VO

examples/agent/agent.go

@@ -37,20 +37,27 @@ type A struct {
 	r float64
 }
 
-func (a *A) P() vector.V { return a.p }
+// S returns the maximum speed of the agent. We are setting the agent speed to
+// be invariant -- this means agents can still move to avoid other agents even
+// after arriving at the destination.
+func (a *A) S() float64 { return a.s }
+
+// T returns the target velocity vector of the agent. In the demos provided, we
+// are setting the target to stop when "close enough" to the actual goal to
+// remove end-state jitter while preserving the flexibility agent to still
+// navigate to avoid collisions.
 func (a *A) T() vector.V {
-	v := vector.Sub(a.G(), a.P())
-	if epsilon.Within(vector.SquaredMagnitude(v), 0) {
-		return *vector.New(0, 0)
+	d := vector.Sub(a.G(), a.P())
+	m := vector.SquaredMagnitude(d)
+	if epsilon.Absolute(1e-5).Within(m, 0) {
+		d = *vector.New(0, 0)
 	}
-
-	// Reduce end-state jittering by forcing the agent to slow down.
-	s := math.Min(a.S(), a.S()*vector.SquaredMagnitude(v))
-	return vector.Scale(s, vector.Unit(v))
+	return vector.Scale(math.Min(a.S(), a.S()*m), d)
 }
+
+func (a *A) P() vector.V     { return a.p }
 func (a *A) V() vector.V     { return a.v }
 func (a *A) G() vector.V     { return a.g }
 func (a *A) R() float64      { return a.r }
-func (a *A) S() float64      { return a.s }
 func (a *A) SetV(v vector.V) { a.v = v }
 func (a *A) SetP(v vector.V) { a.p = v }

internal/solver/2d/solver.go

@@ -45,8 +45,8 @@ type M interface {
 	// bounds of M.
 	Bound(c c2d.C) (segment.S, bool)
 
-	// Within checks if the given vector satisifies the initial bounds of M.
-	Within(v vector.V) bool
+	// In checks if the given vector satisifies the initial bounds of M.
+	In(v vector.V) bool
 }
 
 // region describes an incremental 2D subspace embedded in 2D ambient space.
@@ -202,7 +202,7 @@ func (r *region) intersect(c constraint.C) (segment.S, bool) {
 // optimization functions, this is the v0 defined in Algorithm 2DBoundedLP of de
 // Berg.
 func Solve(m M, cs []constraint.C, o O, v vector.V) (vector.V, feasibility.F) {
-	if !m.Within(v) {
+	if !m.In(v) {
 		return vector.V{}, feasibility.Infeasible
 	}
 

internal/solver/3d/solver.go

@@ -251,7 +251,7 @@ func (r *region) project(c constraint.C) ([]constraint.C, bool) {
 // N.B: This is not a general-purpose 3D linear programming solver. Both the
 // bounding constraints M and input constraints are 2D-specific.
 func Solve(m M, cs []constraint.C, v vector.V) (vector.V, feasibility.F) {
-	if !m.Within(v) {
+	if !m.In(v) {
 		return vector.V{}, feasibility.Infeasible
 	}
 

internal/solver/bounds/circular/circular.go

@@ -10,6 +10,7 @@ import (
 	"github.com/downflux/go-geometry/2d/hypersphere"
 	"github.com/downflux/go-geometry/2d/segment"
 	"github.com/downflux/go-geometry/2d/vector"
+	"github.com/downflux/go-geometry/epsilon"
 )
 
 // M defines a 2D circular bounding constraint.
@@ -39,10 +40,13 @@ func (m M) Bound(c constraint.C) (segment.S, bool) {
 	return *segment.New(l, math.Min(t1, t2), math.Max(t1, t2)), ok
 }
 
-// Within checks if the input vector is contained within the circle.
-//
-// TODO(minkezhang): Rename to In instead.
-func (m M) Within(v vector.V) bool { return hypersphere.C(m).In(v) }
+// In checks if the input vector is contained within the circle.
+func (m M) In(v vector.V) bool {
+	return hypersphere.C(m).In(v) || epsilon.Absolute(1e-5).Within(
+		vector.SquaredMagnitude(vector.Sub(v, hypersphere.C(m).P())),
+		hypersphere.C(m).R()*hypersphere.C(m).R(),
+	)
+}
 
 // V transforms the input vector such that the output will lie on a edge of the
 // circle.

internal/solver/bounds/circular/circular_test.go

@@ -132,7 +132,7 @@ func TestV(t *testing.T) {
 	for _, c := range testConfigs {
 		t.Run(c.name, func(t *testing.T) {
 			m := *New(c.r)
-			if got := m.Within(m.V(c.v)); got != true {
+			if got := m.In(m.V(c.v)); got != true {
 				t.Errorf("In() = %v, want = %v", got, true)
 			}
 		})

internal/solver/bounds/unbounded/unbounded.go

@@ -17,5 +17,5 @@ func (M) Bound(c constraint.C) (segment.S, bool) {
 	return *segment.New(l, math.Inf(-1), math.Inf(0)), true
 }
 
-func (M) Within(v vector.V) bool { return true }
-func (M) V(v vector.V) vector.V  { return v }
+func (M) In(v vector.V) bool    { return true }
+func (M) V(v vector.V) vector.V { return v }

internal/solver/feasibility/feasibility.go

@@ -7,3 +7,15 @@ const (
 	Infeasible
 	Partial
 )
+
+func (f F) String() string {
+	s, ok := map[F]string{
+		Feasible:   "FEASIBLE",
+		Infeasible: "INFEASIBLE",
+		Partial:    "PARTIAL",
+	}[f]
+	if !ok {
+		s = "UNKNOWN"
+	}
+	return s
+}

internal/solver/solver.go

@@ -43,13 +43,14 @@ func Solve(cs []constraint.C, v vector.V, r float64) vector.V {
 	m := *circular.New(r)
 	// Ensure the desired target velocity is within the initial bounding
 	// constraints.
-	if !m.Within(v) {
+	if !m.In(v) {
 		v = m.V(v)
 	}
 
 	u, f := s2d.Solve(m, cs, func(s segment.S) vector.V {
 		return project(s, v)
 	}, v)
+
 	if f == feasibility.Partial {
 		u, f = s3d.Solve(m, cs, u)
 	}

internal/vo/agent/cache/cache.go

@@ -129,69 +129,22 @@ func New(o O) (*VO, error) {
 // ORCA returns the half-plane of permissable velocities for an agent, given the
 // an agent constraint.
 func (vo *VO) ORCA() (hyperplane.HP, error) {
-	u, err := vo.u()
-	if err != nil {
-		return hyperplane.HP{}, err
-	}
-	n, err := vo.n()
+	result, err := vo.preprocess()
 	if err != nil {
 		return hyperplane.HP{}, err
 	}
 	return *hyperplane.New(
-		vector.Add(vo.vopt(vo.agent), vector.Scale(float64(vo.weight), u)),
-		n,
+		vector.Add(vo.vopt(vo.agent), vector.Scale(float64(vo.weight), result.U)),
+		result.N,
 	), nil
 }
 
-// n returns the outward normal vector of u -- that is, if u is pointed towards
-// the internal of VO, n should be anti-parallel to u.
-func (vo *VO) n() (vector.V, error) {
-	u, err := vo.u()
-	if err != nil {
-		return vector.V{}, err
-	}
-
-	orientation := 1.0
-	switch d := vo.domain(); d {
-	case domain.Collision:
-		fallthrough
-	case domain.Circle:
-		tr := vo.r()
-		tw := vo.w()
-
-		if d == domain.Collision {
-			tr = r(vo.agent, vo.obstacle) / minTau
-			tw = w(vo.agent, vo.obstacle, minTau)
-		}
-
-		if vector.SquaredMagnitude(tw) > tr*tr {
-			orientation = -1.
-		}
-	case domain.Right:
-		fallthrough
-	case domain.Left:
-		l := vo.l()
-
-		// We check the side of v compared to the projected edge ℓ, with
-		// the convention that if v is to the "left" of ℓ, we chose n to
-		// be anti-parallel to u.
-		//
-		// N.B.: The "right" leg is represented anti-parallel to the
-		// orientation, and therefore already has an implicit negative
-		// sign attached, allowing the following determinant to be a
-		// continuous calculation from one leg to the other.
-		if vector.Determinant(l.D(), vo.v()) > 0 {
-			orientation = -1
-		}
-	default:
-		return vector.V{}, status.Errorf(codes.Internal, "invalid VO projection %v", d)
-	}
-	return vector.Unit(vector.Scale(orientation, u)), nil
+type result struct {
+	U vector.V
+	N vector.V
 }
 
-// u returns the calculated vector difference between the relative velocity v
-// and the closest part of the VO, pointing into the VO edge.
-func (vo *VO) u() (vector.V, error) {
+func (vo *VO) preprocess() (result, error) {
 	switch d := vo.domain(); d {
 	case domain.Collision:
 		fallthrough
@@ -204,7 +157,19 @@ func (vo *VO) u() (vector.V, error) {
 			tw = w(vo.agent, vo.obstacle, minTau)
 		}
 
-		return vector.Scale(tr/vector.Magnitude(tw)-1, tw), nil
+		u := vector.Scale(tr/vector.Magnitude(tw)-1, tw)
+		n := vector.Unit(vector.Scale(
+			map[bool]float64{
+				false: 1,
+				true:  -1,
+			}[vector.SquaredMagnitude(tw) > tr*tr],
+			u,
+		))
+
+		return result{
+			U: u,
+			N: n,
+		}, nil
 	case domain.Right:
 		fallthrough
 	case domain.Left:
@@ -228,12 +193,84 @@ func (vo *VO) u() (vector.V, error) {
 			vector.Dot(vo.v(), l.D())/vector.SquaredMagnitude(l.D()),
 			l.D(),
 		)
-		return vector.Sub(v, vo.v()), nil
+		u := vector.Sub(v, vo.v())
+		n := vector.Unit(vector.Scale(
+			// We check the side of v compared to the projected edge
+			// ℓ, with the convention that if v is to the "left" of
+			// ℓ, we chose n to be anti-parallel to u.
+			//
+			// N.B.: The "right" leg is represented anti-parallel to
+			// the orientation, and therefore already has an
+			// implicit negative sign attached, allowing the
+			// following determinant to be a continuous calculation
+			// from one leg to the other.
+			map[bool]float64{
+				false: 1,
+				true:  -1,
+			}[vector.Determinant(l.D(), vo.v()) > 0],
+			u,
+		))
+		return result{
+			U: u,
+			N: n,
+		}, nil
 	default:
-		return vector.V{}, status.Errorf(codes.Internal, "invalid VO projection %v", d)
+		return result{}, status.Errorf(codes.Internal, "invalid VO projection %v", d)
 	}
 }
 
+// domain returns the indicated edge of the truncated VO that is closest to w.
+func (vo *VO) domain() domain.D {
+	if !vo.domainIsCached {
+		vo.domainIsCached = true
+
+		vo.domainCache = func() domain.D {
+			beta, err := vo.beta()
+			// Retain parity with RVO2 behavior.
+			if err != nil {
+				return domain.Collision
+			}
+
+			theta, err := vo.theta()
+			// Retain parity with RVO2 behavior.
+			if err != nil {
+				return domain.Right
+			}
+
+			if theta < beta || math.Abs(2*math.Pi-theta) < beta {
+				return domain.Circle
+			}
+
+			if theta < math.Pi {
+				return domain.Left
+			}
+
+			return domain.Right
+		}()
+	}
+	return vo.domainCache
+}
+
+// n returns the outward normal vector of u -- that is, if u is pointed towards
+// the internal of VO, n should be anti-parallel to u.
+func (vo *VO) n() (vector.V, error) {
+	result, err := vo.preprocess()
+	if err != nil {
+		return vector.V{}, err
+	}
+	return result.N, nil
+}
+
+// u returns the calculated vector difference between the relative velocity v
+// and the closest part of the VO, pointing into the VO edge.
+func (vo *VO) u() (vector.V, error) {
+	result, err := vo.preprocess()
+	if err != nil {
+		return vector.V{}, err
+	}
+	return result.U, nil
+}
+
 // r calculates the radius of the unscaled velocity object.
 func (vo *VO) r() float64 {
 	if !vo.rIsCached {
@@ -373,38 +410,6 @@ func (vo *VO) theta() (float64, error) {
 	return vo.thetaCache, nil
 }
 
-// domain returns the indicated edge of the truncated VO that is closest to w.
-func (vo *VO) domain() domain.D {
-	if !vo.domainIsCached {
-		vo.domainIsCached = true
-
-		vo.domainCache = func() domain.D {
-			beta, err := vo.beta()
-			// Retain parity with RVO2 behavior.
-			if err != nil {
-				return domain.Collision
-			}
-
-			theta, err := vo.theta()
-			// Retain parity with RVO2 behavior.
-			if err != nil {
-				return domain.Right
-			}
-
-			if theta < beta || math.Abs(2*math.Pi-theta) < beta {
-				return domain.Circle
-			}
-
-			if theta < math.Pi {
-				return domain.Left
-			}
-
-			return domain.Right
-		}()
-	}
-	return vo.domainCache
-}
-
 // v is a utility function calculating the relative velocities between two
 // agents.
 //