Implemented dHomogTrans. Will move it to drakeGeometryUtil later.
dHomogTransInv.