Moved se2ToMatrix method into SE2 class.