Fix 3D solver returning infeasible solutions causing collisions

internal/geometry/2d/constraint/constraint.go

@@ -17,5 +17,6 @@ func New(c constraint.C, mutable bool) *C {
 	}
 }
 
-func (c C) In(v vector.V) bool { return c.c.In(v) }
+func (c C) C() constraint.C    { return c.c }
+func (c C) In(v vector.V) bool { return c.C().In(v) }
 func (c C) Mutable() bool      { return c.mutable }

internal/solver/2d/solver.go

@@ -4,10 +4,13 @@ package solver
 import (
 	"math"
 
-	"github.com/downflux/go-geometry/2d/constraint"
 	"github.com/downflux/go-geometry/2d/hyperplane"
 	"github.com/downflux/go-geometry/2d/segment"
 	"github.com/downflux/go-geometry/2d/vector"
+	"github.com/downflux/go-orca/internal/geometry/2d/constraint"
+	"github.com/downflux/go-orca/internal/solver/feasibility"
+
+	c2d "github.com/downflux/go-geometry/2d/constraint"
 )
 
 // O specifies an optimization function for the 2D lineaer problem. This
@@ -40,22 +43,12 @@ type M interface {
 	//
 	// Bound must return false if the input constraint lies outside the
 	// bounds of M.
-	Bound(c constraint.C) (segment.S, bool)
+	Bound(c c2d.C) (segment.S, bool)
 
 	// Within checks if the given vector satisifies the initial bounds of M.
 	Within(v vector.V) bool
 }
 
-// Unbounded is the null constraint and satisifies the M interface.
-type Unbounded struct {
-}
-
-func (Unbounded) Bound(c constraint.C) (segment.S, bool) {
-	l := hyperplane.Line(hyperplane.HP(c))
-	return *segment.New(l, math.Inf(-1), math.Inf(0)), true
-}
-func (Unbounded) Within(v vector.V) bool { return true }
-
 // region describes an incremental 2D subspace embedded in 2D ambient space.
 type region struct {
 	m           M
@@ -104,16 +97,16 @@ func (r *region) intersect(c constraint.C) (segment.S, bool) {
 		return segment.S{}, r.Feasible()
 	}
 
-	s, ok := r.m.Bound(c)
+	s, ok := r.m.Bound(c.C())
 	if !ok {
 		r.infeasible = true
 		return segment.S{}, r.Feasible()
 	}
 
-	l := hyperplane.Line(hyperplane.HP(c))
+	l := hyperplane.Line(hyperplane.HP(c.C()))
 	for _, d := range r.constraints {
 		i, ok := l.Intersect(
-			hyperplane.Line(hyperplane.HP(d)),
+			hyperplane.Line(hyperplane.HP(d.C())),
 		)
 
 		// Check for disjoint planes.
@@ -127,9 +120,9 @@ func (r *region) intersect(c constraint.C) (segment.S, bool) {
 		// lines -- the previous feasibility check should avoid the
 		// call.
 		if hyperplane.Disjoint(
-			hyperplane.HP(c),
-			hyperplane.HP(d),
-		) || (!ok && !d.In(hyperplane.HP(c).P())) {
+			hyperplane.HP(c.C()),
+			hyperplane.HP(d.C()),
+		) || (!ok && !d.In(hyperplane.HP(c.C()).P())) {
 			r.infeasible = true
 			return segment.S{}, r.Feasible()
 		}
@@ -208,9 +201,9 @@ func (r *region) intersect(c constraint.C) (segment.S, bool) {
 // optimal solution which satisfies the bounding constraints. For linear
 // 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, bool) {
+func Solve(m M, cs []constraint.C, o O, v vector.V) (vector.V, feasibility.F) {
 	if !m.Within(v) {
-		return vector.V{}, false
+		return vector.V{}, feasibility.Infeasible
 	}
 
 	r := &region{
@@ -219,15 +212,15 @@ func Solve(m M, cs []constraint.C, o O, v vector.V) (vector.V, bool) {
 		constraints: make([]constraint.C, 0, len(cs)),
 	}
 	for _, c := range cs {
-		var ok bool
-
 		if !c.In(v) {
-			if v, ok = r.Solve(c); !ok {
-				return vector.V{}, false
+			if u, ok := r.Solve(c); ok {
+				v = u
+			} else {
+				return v, feasibility.Partial
 			}
 		}
 
 		r.Append(c)
 	}
-	return v, true
+	return v, feasibility.Feasible
 }

internal/solver/2d/solver_test.go

@@ -5,18 +5,19 @@ import (
 	"math"
 	"testing"
 
-	"github.com/downflux/go-geometry/2d/constraint"
 	"github.com/downflux/go-geometry/2d/hyperplane"
 	"github.com/downflux/go-geometry/2d/segment"
 	"github.com/downflux/go-geometry/2d/vector"
 	"github.com/downflux/go-geometry/nd/line"
+	"github.com/downflux/go-orca/internal/geometry/2d/constraint"
+	"github.com/downflux/go-orca/internal/solver/bounds/unbounded"
+	"github.com/downflux/go-orca/internal/solver/feasibility"
 	"github.com/google/go-cmp/cmp"
 
+	c2d "github.com/downflux/go-geometry/2d/constraint"
 	l2d "github.com/downflux/go-geometry/2d/line"
 )
 
-var _ M = Unbounded{}
-
 func TestIntersect(t *testing.T) {
 	type config struct {
 		name    string
@@ -29,35 +30,35 @@ func TestIntersect(t *testing.T) {
 	testConfigs := []config{
 
 		func() config {
-			c := *constraint.New(
+			c := *c2d.New(
 				*vector.New(1, 0),
 				*vector.New(0, 1),
 			)
 
 			l := hyperplane.Line(hyperplane.HP(c))
 			return config{
 				name:    "FirstConstraint",
-				c:       c,
+				c:       *constraint.New(c, true),
 				cs:      []constraint.C{},
 				success: true,
 				want:    *segment.New(l, math.Inf(-1), math.Inf(0)),
 			}
 		}(),
 
 		func() config {
-			c := *constraint.New(
+			c := *c2d.New(
 				*vector.New(0, 1),
 				*vector.New(0, 1),
 			)
 
-			d := *constraint.New(
+			d := *c2d.New(
 				*vector.New(0, -1),
 				*vector.New(0, -1),
 			)
 			return config{
 				name:    "SingleConstraint/Infeasible/Disjoint",
-				c:       c,
-				cs:      []constraint.C{d},
+				c:       *constraint.New(c, true),
+				cs:      []constraint.C{*constraint.New(d, true)},
 				success: false,
 				want:    segment.S{},
 			}
@@ -70,21 +71,21 @@ func TestIntersect(t *testing.T) {
 		func() config {
 			p := *vector.New(1, 0)
 
-			c := *constraint.New(
+			c := *c2d.New(
 				p,
 				*vector.New(1, -1),
 			)
 
-			d := *constraint.New(
+			d := *c2d.New(
 				p,
 				*vector.New(1, 0),
 			)
 
 			l := hyperplane.Line(hyperplane.HP(c))
 			return config{
 				name:    "SingleConstraint/Feasible/SetTMin",
-				c:       c,
-				cs:      []constraint.C{d},
+				c:       *constraint.New(c, true),
+				cs:      []constraint.C{*constraint.New(d, true)},
 				success: true,
 				want:    *segment.New(l, 0, math.Inf(0)),
 			}
@@ -97,21 +98,21 @@ func TestIntersect(t *testing.T) {
 		func() config {
 			p := *vector.New(1, 0)
 
-			c := *constraint.New(
+			c := *c2d.New(
 				p,
 				*vector.New(1, 1),
 			)
 
-			d := *constraint.New(
+			d := *c2d.New(
 				p,
 				*vector.New(1, 0),
 			)
 
 			l := hyperplane.Line(hyperplane.HP(c))
 			return config{
 				name:    "SingleConstraint/Feasible/SetTMax",
-				c:       c,
-				cs:      []constraint.C{d},
+				c:       *constraint.New(c, true),
+				cs:      []constraint.C{*constraint.New(d, true)},
 				success: true,
 				want:    *segment.New(l, math.Inf(-1), 0),
 			}
@@ -134,12 +135,12 @@ func TestIntersect(t *testing.T) {
 		// "relaxing" the constraint if C is added after D, which will
 		// return an infeasibility error.
 		func() []config {
-			c := *constraint.New(
+			c := *c2d.New(
 				*vector.New(1, 0),
 				*vector.New(1, 0),
 			)
 
-			d := *constraint.New(
+			d := *c2d.New(
 				*vector.New(2, 0),
 				*vector.New(1, 0),
 			)
@@ -148,15 +149,15 @@ func TestIntersect(t *testing.T) {
 			return []config{
 				{
 					name:    "SingleConstraint/Infeasible/RelaxedParallelConstraint",
-					c:       c,
-					cs:      []constraint.C{d},
+					c:       *constraint.New(c, true),
+					cs:      []constraint.C{*constraint.New(d, true)},
 					success: false,
 					want:    segment.S{},
 				},
 				{
 					name:    "SingleConstraint/Feasible/ConstrainedParallelConstraint",
-					c:       d,
-					cs:      []constraint.C{c},
+					c:       *constraint.New(d, true),
+					cs:      []constraint.C{*constraint.New(c, true)},
 					success: true,
 					want:    *segment.New(m, math.Inf(-1), math.Inf(0)),
 				},
@@ -166,7 +167,7 @@ func TestIntersect(t *testing.T) {
 	for _, c := range testConfigs {
 		t.Run(c.name, func(t *testing.T) {
 			r := &region{
-				m:           Unbounded{},
+				m:           unbounded.M{},
 				constraints: c.cs,
 			}
 			got, ok := r.intersect(c.c)
@@ -194,27 +195,33 @@ func TestSolve(t *testing.T) {
 		cs      []constraint.C
 		o       O
 		v       vector.V
-		success bool
+		success feasibility.F
 		want    vector.V
 	}
 
 	testConfigs := []config{
 		{
-			name: "Infeasible",
-			m:    Unbounded{},
+			name: "Partial",
+			m:    unbounded.M{},
 			cs: []constraint.C{
 				*constraint.New(
-					*vector.New(0, 1),
-					*vector.New(0, 1),
+					*c2d.New(
+						*vector.New(0, 1),
+						*vector.New(0, 1),
+					),
+					true,
 				),
 				*constraint.New(
-					*vector.New(0, -1),
-					*vector.New(0, -1),
+					*c2d.New(
+						*vector.New(0, -1),
+						*vector.New(0, -1),
+					),
+					true,
 				),
 			},
 			o:       func(segment.S) vector.V { return vector.V{} },
 			v:       *vector.New(0, 1),
-			success: false,
+			success: feasibility.Partial,
 		},
 	}
 
@@ -229,16 +236,25 @@ func TestSolve(t *testing.T) {
 				// The graph shows a vertical x = 4 constraint,
 				// but the solution assumes x = 5.
 				*constraint.New(
-					*vector.New(5, 0),
-					*vector.New(-5, 0),
+					*c2d.New(
+						*vector.New(5, 0),
+						*vector.New(-5, 0),
+					),
+					true,
 				),
 				*constraint.New(
-					*vector.New(0, 4),
-					*vector.New(1, 2),
+					*c2d.New(
+						*vector.New(0, 4),
+						*vector.New(1, 2),
+					),
+					true,
 				),
 				*constraint.New(
-					*vector.New(0, 4),
-					*vector.New(-1, -4),
+					*c2d.New(
+						*vector.New(0, 4),
+						*vector.New(-1, -4),
+					),
+					true,
 				),
 			}
 			o := func(s segment.S) vector.V {
@@ -268,11 +284,11 @@ func TestSolve(t *testing.T) {
 			for _, v := range vs {
 				testConfigs = append(testConfigs, config{
 					name:    fmt.Sprintf("Simple/v0=%v", v),
-					m:       Unbounded{},
+					m:       unbounded.M{},
 					cs:      cs,
 					o:       o,
 					v:       v,
-					success: true,
+					success: feasibility.Feasible,
 					// This is the given solution and is a
 					// constant.
 					want: *vector.New(5, 2.75),
@@ -283,8 +299,8 @@ func TestSolve(t *testing.T) {
 
 	for _, c := range testConfigs {
 		t.Run(c.name, func(t *testing.T) {
-			if got, ok := Solve(c.m, c.cs, c.o, c.v); ok != c.success || !vector.Within(got, c.want) {
-				t.Errorf("Solve() = %v, %v, want = %v, %v", got, ok, c.want, c.success)
+			if got, f := Solve(c.m, c.cs, c.o, c.v); f != c.success || got != nil && c.want != nil && !vector.Within(got, c.want) {
+				t.Errorf("Solve() = %v, %v, want = %v, %v", got, f, c.want, c.success)
 			}
 		})
 	}