Invert affine matrix

Add invert_affine_matrix().

Also add a few almost_equal() functions to help test for equality up to a specified amount of decimal places.