Refactoring + Rename jointMatrix -> trHomogene
Update trHomogene + Add test