code tidy, added tests, added helper for creating translation matrixes