Movimentum - Implementation of rigid body constraints, constraint lifetime, and 3-d troubles

Implementation of rigid body constraints

I have now implemented the creation of the rigid body constraints. This required a change to the grammar and the model: I could not express the squares succinctly, so I added a "squared" (and a "cubed"; and a "square root") expression. Here is the code for the new constraints - it's longer than I would have expected:

    public partial class Script {
        public void AddRigidBodyConstraints() {
            if (Steps.Any()) {
                foreach (var th in _things) {
                    string[] anchorNames = th.Anchors.Keys.ToArray();
                    for (int i = 0; i < anchorNames.Length; i++) {
                        for (int j = i + 1; j < anchorNames.Length; j++) {
                            string anchorName1 = anchorNames[i];
                            string anchorName2 = anchorNames[j];
                            AddRigidBodyConstraint(th.Name,
                                anchorName1, anchorName2,
                                th.Anchors[anchorName1], th.Anchors[anchorName2]);
                        }
                    }
                }
            }
        }

        private void AddRigidBodyConstraint(string thing,
                string anchorName1, string anchorName2,
                ConstVector cv1, ConstVector cv2) {
            string auxVar = thing + "_" + anchorName1 + "_" + anchorName2;
            ScalarEqualityConstraint constraint1, constraint2;
            {
                var anchor1 = new Anchor(thing, anchorName1);
                var anchor2 = new Anchor(thing, anchorName2);
                UnaryScalarExpr xSquared = SquareCoord(UnaryScalarVectorOperator.X, anchor1, anchor2);
                UnaryScalarExpr ySquared = SquareCoord(UnaryScalarVectorOperator.Y, anchor1, anchor2);
                UnaryScalarExpr zSquared = SquareCoord(UnaryScalarVectorOperator.Z, anchor1, anchor2);
                BinaryScalarExpr squaredSum =
                    new BinaryScalarExpr(xSquared,
                        BinaryScalarOperator.PLUS,
                        new BinaryScalarExpr(ySquared,
                            BinaryScalarOperator.PLUS,
                            zSquared
                        )
                    );
                constraint1 = new ScalarEqualityConstraint(auxVar, squaredSum);
            }
            {
                double squaredDistance = Square(cv1.X - cv2.X)
                                       + Square(cv1.Y - cv2.Y)
                                       + Square(cv1.Z - cv2.Z);
                constraint2 = new ScalarEqualityConstraint(auxVar, new Constant(squaredDistance));
            }
            // AddRigidBodyConstraint is only called when Steps.Any() is true - therefore Steps.First() is safe.
            Steps.First().AddConstraint(constraint1);
            Steps.First().AddConstraint(constraint2);
        }

        private double Square(double v) {
            return v * v;
        }

        private static UnaryScalarExpr SquareCoord(UnaryScalarVectorOperator coordOp,
                                                   Anchor anchor1, Anchor anchor2) {
            return new UnaryScalarExpr(UnaryScalarOperator.SQUARED,
                new BinaryScalarExpr(
                    new UnaryScalarVectorExpr(anchor1, coordOp),
                    BinaryScalarOperator.MINUS,
                    new UnaryScalarVectorExpr(anchor2, coordOp))
                );
        }
    }

But - don't I add too many operators ad hoc and just so, like the SQUARED above?

Maybe I should stress the following at this point: At some point, all these mathematical operators will go into that magic constraint solver, and then "all hell will break loose," i.e., I will find out that constraint solving is hard, hard, hard. Therefore, I'll then revise the grammar by excluding operators - so all expression types now in the grammar are more a sort of suggestions. We will see which of them remain - and which are definitely necessary - in the end.

Still, I could not formulate the rigid body constraints directly as in the previous posting. The reason is that the left side of a constraint can only be a variable (which is also used as a key for the lifetime management of the constraint). So I had to formulate each constraints as a constraint pair like that:

   auxVar = (P.x - Q.x)² + (P.z - Q.z)² + (P.z - Q.z)²
   auxVar = constant
  
where the constant is the square of the length. This creates two more issues:

Constraint lifetime

The first issue is that we can have more than one constraint with the same key at the same time (here, "auxVar" is the key of two constraints which should hold simultaneously). Thus, the lifetime management of contraints will probably have to be: If, in a step, new constraints (plural!) are defined for some key, all current constraints with that key are removed; and then the new constraints are enacted.

3-d swiveling

The other issue is that the z coordinate I have added so happily in my 3d posting creates a big headache: Things can now "swing into the third dimension" - and there is no constraint preventing them to do this! What to do? Again, there are at least the following possibilities:

• Explicitly adding constraints to fix all anchor z-s at zero.
• Some implicit mechanism. As I allow a two-dimensional vector notation [a,b], the following might help: If an anchor is created with only two coordinates specified, a constraint fixing the z coordinate at 0 is created automatically.

I'll implement the code for the latter.

Still, an item with only two anchors could swivel around the axis defined by them. It seems that placing the images in 3-d will require additional rules - but right now, I separate the constraint solving from the final image creation: The former should create unique solutions also in 3-d, the latter is right now only defined for 2-d machines. This conforms to the item

• "2D only. Maybe we have 3D ideas later - not now (however, I think I'll give them vectors 3 coordinates from the start)."

in my item list.