Movimentum - The solver algorithm rewrites formal square roots

Our first test script did not work. It ran into the following dead end:

---- 1 dead nodes ----
SolverNode 1020 < 0=C-1019 < 0=V+C-1018 < 0=C-1017 < ...
  ! {EqualsZeroConstraint/}0 = 60+-(65+root 25)
  ! {EqualsZeroConstraint/}0 = 0+-(0+root 0)
  ! {EqualsZeroConstraint/}0 = 60+-(90+root 900)
  ! {EqualsZeroConstraint/}0 = 65+-(65+root 0)
  ! {EqualsZeroConstraint/}0 = 65+-(90+root 625)
  B.P.X = 60
  B.P.Y = 60
  B.P.Z = 0
  B.Q.X = 65
  B.Q.Y = 65
  B.Q.Z = 0
  B.R.X = 65
  B.R.Y = 90
  B.R.Z = 0
  ZERO = 0

What do we see here? Actually, all variables got values—so we sort of do have a solution! The only problem is that the rigid body constraints of the hockey stick were not reduced to 0=0 constraints that could be removed by the 0=C rule. And the reason for this is that there are formal square roots in the expressions which cannot be evaluated.

Manual computation shows that each constraint can be reduced to 0=0. For example, if we select –5 as root(25) in the first constraint, we get for the right side

0 = 60 – (65 + root 25)
0 = 60 – (65 + (– 5))
0 = 60 – 60
0 = 0

But how do we handle formal square roots in general? Obviously, what we want to do is to replace a formal square root with a standard square root. However, in the middle of solving an arbitrary set of constraints, we cannot decide whether the positive or the negative root will lead to a solution. So we have to follow both ways. In an earlier attempt, I tried to replace all formal square roots with pairs of square roots; for a constraint with N formal square roots, this leads to 2^N resulting constraints that are placed in the same number of SolverNodes. However, the code becomes much smaller if we replace only one formal square root. The code for this rewriting requires three parts:

• code to find a formal square root;
• code to create two SolverNodes where the formal square root is rewritten to a positive and a negative square root, respectively;
• glue code to create a rule action from the two pieces above.

Finding a formal square root is done by a small visitor:

internal class FindFormalSquarerootVisitor :
         ISolverModelExprVisitor<FindFormalSquarerootVisitor> {
     private UnaryExpression _someFormalSquareRoot;

     public UnaryExpression SomeFormalSquareroot {
         get { return _someFormalSquareRoot; }
     }
     public FindFormalSquarerootVisitor Visit( 
             Constant constant, Ignore p) {
         return this;
     }
     public FindFormalSquarerootVisitor Visit( 
             NamedVariable namedVariable, Ignore p) {
         return this;
     }
     public FindFormalSquarerootVisitor Visit( 
             AnchorVariable anchorVariable, Ignore p) {
         return this;
     }
     public FindFormalSquarerootVisitor Visit( 
             UnaryExpression unaryExpression, Ignore p) {
         if (unaryExpression.Op is FormalSquareroot) {
             _someFormalSquareRoot = unaryExpression;
         } else {
             unaryExpression.Inner.Accept(this, Ig.nore);
         }
         return this;
     }
     public FindFormalSquarerootVisitor Visit( 
             BinaryExpression binaryExpression, Ignore p) {
         binaryExpression.Lhs.Accept(this, Ig.nore);
         binaryExpression.Rhs.Accept(this, Ig.nore);
         return this;
     }
 }

Rewriting the formal square root is not that hard. The only interesting part is that we add an additional constraint that limits the argument of the square root to non-negative values so that solutions with a negative argument are suppressed:

    private static IEnumerable<SolverNode> RewriteFormalSquareRoot(
             SolverNode origin, UnaryExpression someFormalSquareroot) {

         UnaryExpression positiveRoot =
             new UnaryExpression(someFormalSquareroot.Inner,  
                                 new PositiveSquareroot());
         UnaryExpression negativeRoot = -positiveRoot;

         ScalarConstraint argumentIsAtLeastZeroConstraint =
             new AtLeastZeroConstraint(someFormalSquareroot.Inner);

         return new[] {
             CreateSolverNodeWithOneFormalSquareRootExpanded(
                 origin, positiveRoot,
                 argumentIsAtLeastZeroConstraint, someFormalSquareroot),
             CreateSolverNodeWithOneFormalSquareRootExpanded(
                 origin, negativeRoot,
                 argumentIsAtLeastZeroConstraint, someFormalSquareroot)
         };
     }

Finally, we must put everything together—and here I found that the MatcherRuleActions from that posting were too limiting. So I rewrote that machinery again, about as follows:

• There is only one class RuleAction, which has a method SuccessfulMatch(SolverNode, Constraint), with return type IEnumerable
• If this method decides that the RuleAction does match, it creates the new SolverNodes and returns them.
• If there is no match, it returns null.
• Using this protocol, the Expand methods works as follows:

    public IEnumerable<SolverNode> Expand(...) {
         foreach (var ra in _ruleActions) {
             foreach (var c in Constraints.OfType<ScalarConstraint>()) {
                 IEnumerable<SolverNode> resultOrNull =
                     ra.SuccessfulMatch(this, c);
                 if (resultOrNull != null) {
                     return resultOrNull;
                 }
             }
         }
         return Enumerable.Empty<SolverNode>();
     }

Each RuleAction gets three delegates:

• The first delegate creates a "match result"—an object that contains data assembled during the matching attempt. If this method returns null, this directly means that no match was found.
• The second delegate can check on the non-null "match result" whether it counts as a match .
• Finally, the third delegate is the actual rewrite.

Here is the source that encodes this algorithm:

    public override IEnumerable<SolverNode> SuccessfulMatch( 
             SolverNode current, ScalarConstraint constraint) {
         TMatchResult matchResult = _tryMatch(constraint);
         if (matchResult == null) {
             return null;
         } else if (_isMatch(matchResult)) {
             return _onMatch(current, matchResult, constraint);
         } else {
             return null;
         }
     }

Using the visitor and rewrite method from above, the rule for rewriting formal square roots can now be formulated as follows:

    new RuleAction<FindFormalSquarerootVisitor>("root",
         constraint =>
             constraint.Expr.Accept(new FindFormalSquarerootVisitor(), Ig.nore),
         formalRootFinder =>
             formalRootFinder.SomeFormalSquareroot != null,
         (node, formalRootFinder, constraint) =>
             RewriteFormalSquareRoot(node, formalRootFinder.SomeFormalSquareroot)
         );

With this additional rule, we can again try to run the moving hockey stick test (of course, I had to modify the MatcherRuleActions, but I will not show that modifications here). Again, the test runs red—here is the list of dead nodes (I have deleted all the variable assignments—they are not our problem):

TestMovingHockeyStick : Failed---- 7 dead node(s) ----
SolverNode 1021 < root*1020 < 0=C-1019 < 0=V+C-1018 < 0=C-1017 < 0=V+C...
  ! {EqualsZeroConstraint}0 = -10
  ! {EqualsZeroConstraint}0 = 0+-(0+root 0)
  ! {EqualsZeroConstraint}0 = 60+-(90+root 900)
  ! {EqualsZeroConstraint}0 = 65+-(65+root 0)
  ! {EqualsZeroConstraint}0 = 65+-(90+root 625)
  ! {AtLeastZeroConstraint}0 <= 25
  B.P.X = 60
  ......
SolverNode 1027 < root*1026 < 0=C-1024 < root*1023 < 0=C-1022 < root...
  ! {EqualsZeroConstraint}0 = -60
  ! {EqualsZeroConstraint}0 = 65+-(90+root 625)
  ! {AtLeastZeroConstraint}0 <= 25
  ! {AtLeastZeroConstraint}0 <= 0
  ! {AtLeastZeroConstraint}0 <= 900
  B.P.X = 60
  ......
SolverNode 1030 < root*1029 < 0=C-1028 < root*1026 < 0=C-1024 < root...
  ! {EqualsZeroConstraint}0 = -50
  ! {AtLeastZeroConstraint}0 <= 25
  ! {AtLeastZeroConstraint}0 <= 0
  ! {AtLeastZeroConstraint}0 <= 900
  ! {AtLeastZeroConstraint}0 <= 625
  B.P.X = 60
  ......
SolverNode 1032 < 0=C-1031 < root*1029 < 0=C-1028 < root*1026 < 0=C...
  ! {AtLeastZeroConstraint}0 <= 25
  ! {AtLeastZeroConstraint}0 <= 0
  ! {AtLeastZeroConstraint}0 <= 900
  ! {AtLeastZeroConstraint}0 <= 625
  B.P.X = 60
  ......
SolverNode 1034 < root*1033 < 0=C-1025 < root*1023 < 0=C-1022 < root...
  ! {EqualsZeroConstraint}0 = -60
  ! {EqualsZeroConstraint}0 = 65+-(90+root 625)
  ! {AtLeastZeroConstraint}0 <= 25
  ! {AtLeastZeroConstraint}0 <= 0
  ! {AtLeastZeroConstraint}0 <= 900
  B.P.X = 60
  ......
SolverNode 1037 < root*1036 < 0=C-1035 < root*1033 < 0=C-1025 < root...
  ! {EqualsZeroConstraint}0 = -50
  ! {AtLeastZeroConstraint}0 <= 25
  ! {AtLeastZeroConstraint}0 <= 0
  ! {AtLeastZeroConstraint}0 <= 900
  ! {AtLeastZeroConstraint}0 <= 625
  B.P.X = 60
  ......
SolverNode 1039 < 0=C-1038 < root*1036 < 0=C-1035 < root*1033 < 0=C...
  ! {AtLeastZeroConstraint}0 <= 25
  ! {AtLeastZeroConstraint}0 <= 0
  ! {AtLeastZeroConstraint}0 <= 900
  ! {AtLeastZeroConstraint}0 <= 625
  B.P.X = 60
  ......

We'll analyze in the next posting what this means.