Rigid trans

Could you review this code. Thanks!
Origin[i] must be cast to double using 'NumberTraits<Point::Coordinate>::castToDouble(origin[I]))'

You can also cast the point to realpoint in the constructor