From ccda87aafa589e5c01733e9b49cf809e73663801 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 12 Sep 2023 07:57:12 -0700 Subject: [PATCH] Use `mesh_pos` and `mesh_quat` in SDF transformation. The SDF is defined in the `geom` frame. Using a mesh changes this frame so the inverse transformation is needed. So far the full `geom_pos` and `geom_quat` were used, which possibly contained user transformation. This CL uses the new structures which only contain transformations due to mesh processing. PiperOrigin-RevId: 564721461 Change-Id: I6fb28001b38eaa50663427bce452cb1f05a56ac0 --- src/engine/engine_collision_sdf.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 65006785..0565cfcf 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -259,8 +259,8 @@ static void undoTransformation(const mjModel* m, const mjData* d, int g, mjtNum* xmat = d->geom_xmat + 9 * g; if (m->geom_type[g]==mjGEOM_MESH || m->geom_type[g]==mjGEOM_SDF) { mjtNum negpos[3], negquat[4], xquat[4]; - mjtNum* pos = m->geom_pos + 3 * g; - mjtNum* quat = m->geom_quat + 4 * g; + mjtNum* pos = m->mesh_pos + 3 * m->geom_dataid[g]; + mjtNum* quat = m->mesh_quat + 4 * m->geom_dataid[g]; mju_mat2Quat(xquat, xmat); mju_negPose(negpos, negquat, pos, quat); mju_mulPose(sdf_xpos, sdf_quat, xpos, xquat, negpos, negquat);