Fix collider rendering with new se3

umrover · Feb 16, 2024 · fb09d58 · fb09d58
1 parent 831c73a
commit fb09d58
Showing 1 changed file with 8 additions and 8 deletions.
diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp
@@ -411,8 +411,8 @@ namespace mrover {
                         URDF::LinkMeta& meta = urdf.linkNameToMeta.at(link->name);
                         SE3d linkInWorld = urdf.linkInWorld(link->name);
                         SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin));
-                        SE3d modelInWorld = linkInWorld * modelInLink;
-                        renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld, R3::Ones()});
+                        SE3d modelToWorld = linkInWorld * modelInLink;
+                        renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelToWorld, R3::Ones()});
                         visualIndex++;
                     }
                 }
@@ -439,27 +439,27 @@ namespace mrover {
                     assert(collider);
                     btCollisionShape* shape = collider->getCollisionShape();
                     assert(shape);
-                    SE3d linkInWorld = urdf.linkInWorld(link->name);
+                    SE3d linkToWorld = urdf.linkInWorld(link->name);
 
                     if (auto* compound = dynamic_cast<btCompoundShape*>(shape)) {
                         for (int i = 0; i < compound->getNumChildShapes(); ++i) {
                             SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin));
-                            SE3d shapeToWorld = modelInLink * linkInWorld;
+                            SE3d modelInWorld = linkToWorld * modelInLink;
                             auto* shape = compound->getChildShape(i);
                             if (auto* box = dynamic_cast<btBoxShape const*>(shape)) {
                                 btVector3 extents = box->getHalfExtentsWithoutMargin() * 2;
-                                SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}};
+                                SIM3 modelToWorld{modelInWorld, R3{extents.x(), extents.y(), extents.z()}};
                                 renderModel(pass, mUriToModel.at(CUBE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld);
                             } else if (auto* sphere = dynamic_cast<btSphereShape const*>(shape)) {
                                 btScalar diameter = sphere->getRadius() * 2;
-                                SIM3 modelToWorld{shapeToWorld, R3{diameter, diameter, diameter}};
+                                SIM3 modelToWorld{modelInWorld, R3{diameter, diameter, diameter}};
                                 renderModel(pass, mUriToModel.at(SPHERE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld);
                             } else if (auto* cylinder = dynamic_cast<btCylinderShapeZ const*>(shape)) {
                                 btVector3 extents = cylinder->getHalfExtentsWithoutMargin() * 2;
-                                SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}};
+                                SIM3 modelToWorld{modelInWorld, R3{extents.x(), extents.y(), extents.z()}};
                                 renderModel(pass, mUriToModel.at(CYLINDER_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld);
                             } else if (auto* mesh = dynamic_cast<btBvhTriangleMeshShape const*>(shape)) {
-                                SIM3 modelToWorld{shapeToWorld, R3::Ones()};
+                                SIM3 modelToWorld{modelInWorld, R3::Ones()};
                                 renderModel(pass, mUriToModel.at(mMeshToUri.at(const_cast<btBvhTriangleMeshShape*>(mesh))), meta.collisionUniforms.at(i), modelToWorld);
                             } else {
                                 NODELET_WARN_STREAM_ONCE(std::format("Tried to render unsupported collision shape: {}", shape->getName()));