diff --git a/primitive_process/src/base/subface.cpp b/primitive_process/src/base/subface.cpp index 18a4a17..0973c63 100644 --- a/primitive_process/src/base/subface.cpp +++ b/primitive_process/src/base/subface.cpp @@ -31,8 +31,9 @@ std::pair subface::apply_transform(intern temp.world_to_local.linear() = temp.world_to_local.linear() * rotation_matrix.transpose(); } break; case internal::transform_type::translation: { - temp.local_to_world.translation() += param.head<3>(); - temp.world_to_local.translation() -= param.head<3>(); + Eigen::Translation3d translation(param.head<3>()); + temp.local_to_world = translation * temp.local_to_world; + temp.world_to_local = temp.world_to_local * translation.inverse(); } break; default: throw std::invalid_argument("Invalid transform type"); }