Movimentum - Converting input constraints to solver constraints

New rigid body constraints

Before we do the constraint conversion, there's one thing that has come up at some point when I tried some solving ideas: I changed the representation of the rigid body constraints. In that posting, the constraints were of the following form:

For each pair of anchor points P and Q, two constraints were formulated:

• auxVarPQ = (P.x – Q.x)² + (P.y – Q.y)² + (P.z – Q.z)²
• auxVarPQ = constant (of value |P – Q|²)

However, the solver that started to emerge from my first trials was a "variable rewriting solver," which likes to have constraints of the form V = E, where V is a variable and E an expression. But in the rigid body constraints above, the variables P.x, P.y etc. are "deeply embedded in squares". So, I decided to replace these constraints with "solved rigid body constraints." A current, very incomplete solver does not like too many explicit constraints (for reasons I will explain some five postings down the line), and so I settled, right now, for rigid body constraints of the form

• P.x = ...
• P.y = ...
• P.z = ...

Solving the equation

c = (P.x – Q.x)² + (P.y – Q.y)² + (P.z – Q.z)²

for P.x, P.y, and P.z yields

• P.x = Q.x + √(c – (P.y – Q.y)² – (P.z - Q.z)²)
• P.y = Q.y + √(c – (P.x – Q.x)² – (P.z - Q.z)²)
• P.z = Q.z + √(c – (P.x – Q.x)² – (P.y - Q.y)²)

and these are the constraints we are going to add.
It is important to note here that the square root √ is a formal square root: Its result might be the positive or the negative square root of the argument! But we will deal with this problem in the actual solver.
The code for the "new rigid body constraints" is here.

Converting input constraints to solver constraints

Let us start with the obvious: Each constraint gets a method that converts it to solver constraints. It is obvious that a single input constraint can result in multiple solver constraints—that is certainly true for vector constraints:

    public abstract partial class Constraint {
        public abstract AbstractConstraint[] CreateSolverConstraints(double t, double iv);
    }

The conversion methods have parameters for the "fixed variables" .t and .iv—for a specific frame to be solved, they are mere constants in the resulting solver constraints.

Let us start with the simplest conversion, namely from a scalar equality input constraint to a EqualsZeroConstraint for the solver. The code I'd like to write is:

    public partial class ScalarEqualityConstraint {
        public override AbstractConstraint[] CreateSolverConstraints(double t, double iv) {
            return new[] { new EqualsZeroConstraint(
                            BinaryScalarOperator.MINUS.CreateSolverExpression(
                                new NamedVariable(_variable),
                                _rhs.CreateSolverExpression(t, iv)))
            };
        }
    }

In other words, the input constraint V = E is converted to the solver constraint 0 = V' - E', where V' and E' are the conversions of V and E. And we have delegated the actual conversion job to the input model operator. How does it know what to do? Well, let's give every operator a strategy—here, I start with the scalar binary operators (and we see that the operators in the solver model we wrote for creating test expressions now come in handy!):

    public partial class BinaryScalarOperator {
        private Func<AbstractExpr, AbstractExpr, AbstractExpr> _create;
        public AbstractExpr CreateSolverExpression(AbstractExpr lhsExpr, AbstractExpr rhsExpr) {
            return _create(lhsExpr, rhsExpr);
        }

        static void BinaryScalarOperatorInit() {
            PLUS._create = (lhsExpr, rhsExpr) => lhsExpr +  rhsExpr;
            MINUS._create = (lhsExpr, rhsExpr) => lhsExpr + (-rhsExpr);
            TIMES._create = (lhsExpr, rhsExpr) => lhsExpr *  rhsExpr;
            DIVIDE._create = (lhsExpr, rhsExpr) => lhsExpr / rhsExpr;
        }
    }

I think the code is quite obvious: Each such operator gets a _create delegate that does the work. It gets the already converted subexpressions, and builds a new solver expression. The setup of these delegates (or strategies) happens by assignments in a (or rather the) static constructor of the corresponding (input model) operator class, as you can see above.

This is actually contrary to my belief that each such setup should be done by a constructor. One can see that I contradict my design rules from the fact that the _create instance field is not readonly—which it should be, as this strategy certainly never changes during Movimentum's runtime! So why this deviation? The reason is "conceptual decoupling:" I want to put the code for constraint conversion into a separate source file; and only there I want to set up such strategies. The constructors, on the other hand, are already "taken" by the setup code in the input model file, so I assign the _create values later. If only C# hat some sort of "partial constructor parameter lists" ...

Actually, the MINUS operator we need for converting a ScalarEqualityConstraint has a more complex conversion strategy than the other three operators. The reason is that I do not supply a minus operator in the solver constraints (hopefully reducing some rewrite work there). Therefore, V = E is actually converted to 0 = V' + (-E')!

Let us look at two more pieces of conversion code that might be interesting. Converting a BinaryVectorExpression with a PLUS or MINUS operator uses the following code:

    public partial class BinaryVectorOperator {
        private Func<AbstractExpr[], AbstractExpr[], AbstractExpr[]> _create;
        public AbstractExpr[] CreateSolverExpressions(AbstractExpr[] lhsExpr, AbstractExpr[] rhsExpr) {
            return _create(lhsExpr, rhsExpr);
        }

        static void BinaryVectorOperatorInit() {
            PLUS._create = (lhsExpr, rhsExpr) => new[] {
                BinaryScalarOperator.PLUS.CreateSolverExpression(lhsExpr[0], rhsExpr[0]),
                BinaryScalarOperator.PLUS.CreateSolverExpression(lhsExpr[1], rhsExpr[1]),
                BinaryScalarOperator.PLUS.CreateSolverExpression(lhsExpr[2], rhsExpr[2])
            };
            MINUS._create = (lhsExpr, rhsExpr) => new[] {
                BinaryScalarOperator.MINUS.CreateSolverExpression(lhsExpr[0], rhsExpr[0]),
                BinaryScalarOperator.MINUS.CreateSolverExpression(lhsExpr[1], rhsExpr[1]),
                BinaryScalarOperator.MINUS.CreateSolverExpression(lhsExpr[2], rhsExpr[2])
            };
        }
    }

The operators now get as strategy a delegate that gets two arrays of (three) converted solver expressions; and returns an array of (three) new solver expressions. The code reuses the CreateSolverExpression methods already hooked up to the BinaryScalarOperators.

As a last example, here is the conversion code for the ROTATE2D and TIMES operators of the input model, i.e. operators that combine a vector and a scalar:

    public partial class BinaryScalarVectorOperator {
        private Func<AbstractExpr[], AbstractExpr, AbstractExpr[]> _create;
        public AbstractExpr[] CreateSolverExpressions(AbstractExpr[] lhsExpr, AbstractExpr rhsExpr) {
            return _create(lhsExpr, rhsExpr);
        }

        private static AbstractExpr[] Rotate2D(AbstractExpr[] lhsExpr, AbstractExpr rhsExpr) {
            // X = x cos phi - y sin phi
            // Y = x sin phi + y cos phi
            // Z = z

            var sin = new UnaryExpression(rhsExpr, new Sin());
            var cos = new UnaryExpression(rhsExpr, new Cos());
            var a11 = BinaryScalarOperator.TIMES.CreateSolverExpression(lhsExpr[0], cos);
            var a12 = BinaryScalarOperator.TIMES.CreateSolverExpression(lhsExpr[1], sin);
            var a21 = BinaryScalarOperator.TIMES.CreateSolverExpression(lhsExpr[0], sin);
            var a22 = BinaryScalarOperator.TIMES.CreateSolverExpression(lhsExpr[1], cos);

            return new[] {
                BinaryScalarOperator.MINUS.CreateSolverExpression(a11, a12),
                BinaryScalarOperator.PLUS.CreateSolverExpression(a21, a22),
                lhsExpr[2]
            };
        }

        private static AbstractExpr[] Times(AbstractExpr[] lhsExpr, AbstractExpr rhsExpr) {
            return new[] {
                BinaryScalarOperator.TIMES.CreateSolverExpression(lhsExpr[0], rhsExpr),
                BinaryScalarOperator.TIMES.CreateSolverExpression(lhsExpr[1], rhsExpr),
                BinaryScalarOperator.TIMES.CreateSolverExpression(lhsExpr[2], rhsExpr),
            };
        }

        static void BinaryScalarVectorOperatorInit() {
            ROTATE2D._create = Rotate2D;
            TIMES._create = Times;
        }
    }

All the other conversions are either much simpler (e.g., converting a T just returns new Constant(t)) or similar to the ones above ...

... oh yes, we should maybe take a short look at the anchor variables, i.e., our "way back from the solver model to the input model." But their conversion is trivial—we just hook in there the right anchor plus an indication of the coordinate (X, Y, or Z):

    public partial class Anchor {
        public enum Coordinate { X, Y, Z }

        public override AbstractExpr[] CreateSolverExpressions(double t,
                                                               double iv) {
            return new[] {
                new AnchorVariable(this, Coordinate.X),
                new AnchorVariable(this, Coordinate.Y),
                new AnchorVariable(this, Coordinate.Z),
            };
        }
    }

The whole code of the conversion can be found here (or in file ModelCreateSolverModel.cs on GitHub).

Next item on the agenda: Planning the overall solving algorithm!