88module Linear.Constraint.Simple.Util where
99
1010import qualified Data.List as L
11- import Data.List.NonEmpty (NonEmpty (.. ))
12- import qualified Data.List.NonEmpty as NE
1311import qualified Data.Set as Set
1412import Linear.Constraint.Generic.Types
15- ( GenericConstraint ((:<=) , (:==) , (:>=) )
13+ ( GenericConstraint (.. )
1614 )
1715import Linear.Constraint.Simple.Types (SimpleConstraint )
1816import Linear.Constraint.Types (Constraint )
19- import Linear.Expr.Types (Expr (Expr ))
17+ import Linear.Expr.Types (Expr (.. ), ExprVarsOnly ( .. ))
2018import Linear.Expr.Util
21- ( exprToList
19+ ( exprToExprVarsOnly
20+ , exprToList
2221 , exprVars
22+ , exprVarsOnlyToExpr
2323 , listToExpr
2424 , simplifyExpr
25+ , simplifyExprVarsOnly
2526 , substVarExpr
27+ , substVarExprVarsOnly
2628 , subtractExpr
2729 , sumExprConstTerms
2830 , zeroConstExpr
2931 )
30- import Linear.Term.Types (Term (CoeffTerm , ConstTerm , VarTerm ))
32+ import Linear.Term.Types (Term (.. ), TermVarsOnly ( .. ))
3133import Linear.Var.Types (Var )
3234
33- substVarSimpleConstraint :: Var -> Expr -> SimpleConstraint -> SimpleConstraint
34- substVarSimpleConstraint var varReplacement (a :<= b) = substVarExpr var varReplacement a :<= b
35- substVarSimpleConstraint var varReplacement (a :>= b) = substVarExpr var varReplacement a :>= b
36- substVarSimpleConstraint var varReplacement (a :== b) = substVarExpr var varReplacement a :== b
35+ substVarSimpleConstraintExpr ::
36+ Var -> Expr -> SimpleConstraint -> SimpleConstraint
37+ substVarSimpleConstraintExpr var varReplacement (a :<= b) =
38+ let newExpr = substVarExpr var varReplacement (exprVarsOnlyToExpr a)
39+ newConstraint = newExpr :<= Expr [ConstTerm b]
40+ in constraintToSimpleConstraint newConstraint
41+ substVarSimpleConstraintExpr var varReplacement (a :>= b) =
42+ let newExpr = substVarExpr var varReplacement (exprVarsOnlyToExpr a)
43+ newConstraint = newExpr :>= Expr [ConstTerm b]
44+ in constraintToSimpleConstraint newConstraint
45+ substVarSimpleConstraintExpr var varReplacement (a :== b) =
46+ let newExpr = substVarExpr var varReplacement (exprVarsOnlyToExpr a)
47+ newConstraint = newExpr :== Expr [ConstTerm b]
48+ in constraintToSimpleConstraint newConstraint
49+
50+ substVarSimpleConstraint ::
51+ Var -> ExprVarsOnly -> SimpleConstraint -> SimpleConstraint
52+ substVarSimpleConstraint var varReplacement (a :<= b) = substVarExprVarsOnly var varReplacement a :<= b
53+ substVarSimpleConstraint var varReplacement (a :>= b) = substVarExprVarsOnly var varReplacement a :>= b
54+ substVarSimpleConstraint var varReplacement (a :== b) = substVarExprVarsOnly var varReplacement a :== b
3755
3856constraintToSimpleConstraint :: Constraint -> SimpleConstraint
3957constraintToSimpleConstraint constraint =
@@ -51,79 +69,77 @@ constraintToSimpleConstraint constraint =
5169 aWithoutConst = simplifyExpr . zeroConstExpr $ a
5270 bWithoutConst = simplifyExpr . zeroConstExpr $ b
5371
54- lhs = subtractExpr aWithoutConst bWithoutConst
55- calcRhs a b = rhs
56- where
57- aConsts = sumExprConstTerms a
58- bConsts = sumExprConstTerms b
59- rhs = bConsts - aConsts
60-
61- aWithoutConst = simplifyExpr . zeroConstExpr $ a
62- bWithoutConst = simplifyExpr . zeroConstExpr $ b
63-
64- lhs = subtractExpr aWithoutConst bWithoutConst
72+ lhs' = subtractExpr aWithoutConst bWithoutConst
73+ lhs = case exprToExprVarsOnly lhs' of
74+ Right exprVarsOnly -> exprVarsOnly
75+ Left err ->
76+ error $
77+ " constraintToSimpleConstraint: lhs is not ExprVarsOnly. Details: " <> err
6578
6679-- normalize simple constraints by moving all constants to the right
67- normalizeSimpleConstraint :: SimpleConstraint -> SimpleConstraint
68- normalizeSimpleConstraint (expr :<= num) =
69- let exprList = exprToList expr
80+ -- normalizeSimpleConstraint :: SimpleConstraint -> SimpleConstraint
81+ -- normalizeSimpleConstraint (expr :<= num) =
82+ -- let exprList = exprToList expr
7083
71- isConstTerm (ConstTerm _) = True
72- isConstTerm _ = False
84+ -- isConstTerm (ConstTerm _) = True
85+ -- isConstTerm _ = False
7386
74- (sumExprConstTerms, nonConstTerms) = L. partition isConstTerm exprList
87+ -- (sumExprConstTerms, nonConstTerms) = L.partition isConstTerm exprList
7588
76- constTermsVal = sum . map (\ case (ConstTerm c) -> c; _ -> 0 ) $ sumExprConstTerms
89+ -- constTermsVal = sum . map (\case (ConstTerm c) -> c; _ -> 0) $ sumExprConstTerms
7790
78- newExpr = listToExpr nonConstTerms
79- newNum = num - constTermsVal
80- in newExpr :<= newNum
81- normalizeSimpleConstraint (expr :>= num) =
82- let exprList = exprToList expr
91+ -- newExpr = listToExpr nonConstTerms
92+ -- newNum = num - constTermsVal
93+ -- in newExpr :<= newNum
94+ -- normalizeSimpleConstraint (expr :>= num) =
95+ -- let exprList = exprToList expr
8396
84- isConstTerm (ConstTerm _) = True
85- isConstTerm _ = False
97+ -- isConstTerm (ConstTerm _) = True
98+ -- isConstTerm _ = False
8699
87- (sumExprConstTerms, nonConstTerms) = L. partition isConstTerm exprList
100+ -- (sumExprConstTerms, nonConstTerms) = L.partition isConstTerm exprList
88101
89- constTermsVal = sum . map (\ case (ConstTerm c) -> c; _ -> 0 ) $ sumExprConstTerms
102+ -- constTermsVal = sum . map (\case (ConstTerm c) -> c; _ -> 0) $ sumExprConstTerms
90103
91- newExpr = listToExpr nonConstTerms
92- newNum = num - constTermsVal
93- in newExpr :>= newNum
94- normalizeSimpleConstraint (expr :== num) =
95- let exprList = exprToList expr
104+ -- newExpr = listToExpr nonConstTerms
105+ -- newNum = num - constTermsVal
106+ -- in newExpr :>= newNum
107+ -- normalizeSimpleConstraint (expr :== num) =
108+ -- let exprList = exprToList expr
96109
97- isConstTerm (ConstTerm _) = True
98- isConstTerm _ = False
110+ -- isConstTerm (ConstTerm _) = True
111+ -- isConstTerm _ = False
99112
100- (sumExprConstTerms, nonConstTerms) = L. partition isConstTerm exprList
113+ -- (sumExprConstTerms, nonConstTerms) = L.partition isConstTerm exprList
101114
102- constTermsVal = sum . map (\ case (ConstTerm c) -> c; _ -> 0 ) $ sumExprConstTerms
115+ -- constTermsVal = sum . map (\case (ConstTerm c) -> c; _ -> 0) $ sumExprConstTerms
103116
104- newExpr = listToExpr nonConstTerms
105- newNum = num - constTermsVal
106- in newExpr :== newNum
117+ -- newExpr = listToExpr nonConstTerms
118+ -- newNum = num - constTermsVal
119+ -- in newExpr :== newNum
107120
108121-- | Simplify coeff constraints by dividing the coefficient from both sides
109122simplifyCoeff :: SimpleConstraint -> SimpleConstraint
110- simplifyCoeff expr@ (Expr ( CoeffTerm coeff var :| [] ) :<= num)
123+ simplifyCoeff expr@ (ExprVarsOnly [ CoeffTermVO coeff var] :<= num)
111124 | coeff == 0 = expr
112- | coeff > 0 = Expr ( VarTerm var :| [] ) :<= (num / coeff)
113- | coeff < 0 = Expr ( VarTerm var :| [] ) :>= (num / coeff)
114- simplifyCoeff expr@ (Expr ( CoeffTerm coeff var :| [] ) :>= num)
125+ | coeff > 0 = ExprVarsOnly [ VarTermVO var] :<= (num / coeff)
126+ | coeff < 0 = ExprVarsOnly [ VarTermVO var] :>= (num / coeff)
127+ simplifyCoeff expr@ (ExprVarsOnly [ CoeffTermVO coeff var] :>= num)
115128 | coeff == 0 = expr
116- | coeff > 0 = Expr (VarTerm var :| [] ) :>= (num / coeff)
117- | coeff < 0 = Expr (VarTerm var :| [] ) :<= (num / coeff)
118- simplifyCoeff expr@ (Expr (CoeffTerm coeff var :| [] ) :== num) = if coeff == 0 then expr else Expr (VarTerm var :| [] ) :== (num / coeff)
129+ | coeff > 0 = ExprVarsOnly [VarTermVO var] :>= (num / coeff)
130+ | coeff < 0 = ExprVarsOnly [VarTermVO var] :<= (num / coeff)
131+ simplifyCoeff expr@ (ExprVarsOnly [CoeffTermVO coeff var] :== num) =
132+ if coeff == 0
133+ then expr
134+ else ExprVarsOnly [VarTermVO var] :== (num / coeff)
119135simplifyCoeff expr = expr
120136
121137simplifySimpleConstraint :: SimpleConstraint -> SimpleConstraint
122- simplifySimpleConstraint (expr :<= num) = simplifyCoeff . normalizeSimpleConstraint $ simplifyExpr expr :<= num
123- simplifySimpleConstraint (expr :>= num) = simplifyCoeff . normalizeSimpleConstraint $ simplifyExpr expr :>= num
124- simplifySimpleConstraint (expr :== num) = simplifyCoeff . normalizeSimpleConstraint $ simplifyExpr expr :== num
138+ simplifySimpleConstraint (expr :<= num) = simplifyCoeff $ simplifyExprVarsOnly expr :<= num
139+ simplifySimpleConstraint (expr :>= num) = simplifyCoeff $ simplifyExprVarsOnly expr :>= num
140+ simplifySimpleConstraint (expr :== num) = simplifyCoeff $ simplifyExprVarsOnly expr :== num
125141
126142simpleConstraintVars :: SimpleConstraint -> Set. Set Var
127- simpleConstraintVars (expr :<= _) = exprVars expr
128- simpleConstraintVars (expr :>= _) = exprVars expr
129- simpleConstraintVars (expr :== _) = exprVars expr
143+ simpleConstraintVars (expr :<= _) = exprVars . exprVarsOnlyToExpr $ expr
144+ simpleConstraintVars (expr :>= _) = exprVars . exprVarsOnlyToExpr $ expr
145+ simpleConstraintVars (expr :== _) = exprVars . exprVarsOnlyToExpr $ expr
0 commit comments