Rigid trans

Could you review this code. Thanks!
Please template the class by Space and get point/vector types from Space