Get the difference between 2 matrices

Question

Hi I'm using C# and MonoGame, and have the world matrices of 2 objects. I need to get the difference between them, so I can effectively parent the second object onto the first. The aim is to get to a point where any changes to the parent transform can be applied to the child object too, by doing child.Transform = parent.Transform * offset.

I'm using the following code:

// ...

Matrix m1 = info.BoneTargets[i].Transform;
Matrix m2 = rigidbodyDefaultWorlds[transform.RigidBodyIndex];

Matrix transformFromTargetToRigidBody = Matrix.Invert(m1) * m2;
Debug.Assert((m1 * transformFromTargetToRigidBody).Equals(m2)); // FAIL

// ...

The Assert fails though, so I assume my math is wrong. I did try all other variations of the invert(a) * b formula though and none of them worked either.

For completion here are the matrix values:

M1  {M11:1 M12: 2.384186E-07 M13:-6.211763E-22 M14:0} {M21:0             M22:0             M23:1            M24:0} {M31:2.384186E-07  M32:-1            M33:0             M34:0} {M41:0.0005292654 M42:-0.03780181 M43:0.752234  M44:1}
M2  {M11:1 M12:-2.384186E-07 M13:0             M14:0} {M21:-3.884332E-14 M22:-1.629207E-07 M23:1            M24:0} {M31:-2.384186E-07 M32:-1            M33:-1.629207E-07 M34:0} {M41:0.01058531   M42:0.7560362   M43:-17.54894 M44:1}
TR  {M11:1 M12:-4.768372E-07 M13:-3.884332E-14 M14:0} {M21:4.768372E-07  M22:1             M23:1.629207E-07 M24:0} {M31:-3.884332E-14 M32:-1.629207E-07 M33:1             M34:0} {M41:0.01005606   M42:0.7938381   M43:-18.30118 M44:1}
OUT {M11:1 M12:-2.384186E-07 M13:-6.211763E-22 M14:0} {M21:-3.884332E-14 M22:-1.629207E-07 M23:1            M24:0} {M31:-2.384186E-07 M32:-1            M33:-1.629207E-07 M34:0} {M41:0.01058531   M42:0.7560362   M43:-17.54894 M44:1}

---

EDIT: Actually, now I'm thinking the formula is this...

child.World = childTransformWithOffsetTranslation * parentWorld

Answer 1

Your problem is that you're trying to compare floating-point numbers for equality. The matrix inversion and multiplication, however, will inevitable introduce slight rounding errors that will make the numbers in the two matrices not exactly equal.

Still, let's compare your debug printouts side by side. I've deleted the M1 and TR rows, and left just the two that you're trying to compare:

M2  {M11:1 M12:-2.384186E-07 M13:0             M14:0} {M21:-3.884332E-14 M22:-1.629207E-07 M23:1            M24:0} {M31:-2.384186E-07 M32:-1            M33:-1.629207E-07 M34:0} {M41:0.01058531   M42:0.7560362   M43:-17.54894 M44:1}
OUT {M11:1 M12:-2.384186E-07 M13:-6.211763E-22 M14:0} {M21:-3.884332E-14 M22:-1.629207E-07 M23:1            M24:0} {M31:-2.384186E-07 M32:-1            M33:-1.629207E-07 M34:0} {M41:0.01058531   M42:0.7560362   M43:-17.54894 M44:1}

You can see that, in the printed output (which seems to be rounded to 7 significant digits of precision), the only visible difference is in cell (1,3), which is exactly zero in M2, but has a very small negative value -6.211763 × 10^-22 in the product. How small is that? Well, let's write it out fully, without using scientific notation:

-6.211763 × 10^-22 = -0.0000000000000000000006211763

Yeah, that's basically as close to zero as to make no difference. But the .Equals() method will consider it to be different from zero (and will probably fail on other equally negligible differences, which are not visible in the printed output, as well).

For a more reasonable comparison, you could compare the matrix entries yourself one by one, using a "fuzzy comparison" function like the nearlyEqual() function from this answer on SO by Michael Borgwardt, quoted below:

public static boolean nearlyEqual(float a, float b, float epsilon) {
    final float absA = Math.abs(a);
    final float absB = Math.abs(b);
    final float diff = Math.abs(a - b);

    if (a == b) { // shortcut, handles infinities
        return true;
    } else if (a == 0 || b == 0 || diff < Float.MIN_NORMAL) {
        // a or b is zero or both are extremely close to it
        // relative error is less meaningful here
        return diff < (epsilon * Float.MIN_NORMAL);
    } else { // use relative error
        return diff / (absA + absB) < epsilon;
    }
}

The code above is Java, but converting it into C# should be trivial. I expect that your two matrices should be "nearly equal" at least to a precision of epsilon = 1e-9, and probably even closer.

Answer 2

I don't really understand your question to be honest but I think you're trying to figure out how to find the world matrix of a child node. In other words, you essentially have a scene graph with parent nodes and child nodes and you want to know how to calculate the world matrix of one of the children.

Essentially it's a two step process. First you need to calculate the local transform given that you already know the local position, rotation and scale relative to it's parent.

public Matrix GetLocalTransform()
{
    var rotationMatrix = Matrix.CreateRotationZ(Rotation);
    var scaleMatrix = Matrix.CreateScale(new Vector3(Scale.X, Scale.Y, 1));
    var translationMatrix = Matrix.CreateTranslation(new Vector3(Position.X, Position.Y, 0));
    var tempMatrix = Matrix.Multiply(scaleMatrix, rotationMatrix);
    return Matrix.Multiply(tempMatrix, translationMatrix);
}

Then once you have the local transform for a node you can multiply it by it's parent's world transform to get the world transform of this child.

public Matrix GetWorldTransform()
{
    return Parent == null ? Matrix.Identity : Matrix.Multiply(GetLocalTransform(), Parent.GetWorldTransform());
}

Of course, the top most parent node won't have a parent, so in that case you just use the identity matrix.

This works for an entire tree of nodes recursively.

The code above is from the MonoGame.Extended library if you want to take a closer look. It's a 2D scene graph, but the same principles should work for 3D scene graphs as well.