Fix lift cabin graphics on rotated floor plans (#513)

Signed-off-by: tanjunkiat <[email protected]>

rmf_traffic_editor/gui/building.cpp

@@ -429,7 +429,8 @@ void Building::draw_lifts(QGraphicsScene* scene, const int level_idx)
       true,
       t.scale,
       t.dx,
-      t.dy);
+      t.dy,
+      t.rotation);
   }
 }
 
@@ -499,6 +500,7 @@ Building::Transform Building::compute_transform(
     t.scale = 1.0;
     t.dx = 0.0;
     t.dy = 0.0;
+    t.rotation = 0.0;
     return t;
   }
 
@@ -520,6 +522,25 @@ Building::Transform Building::compute_transform(
     }
   }
 
+  // calculate the rotation between each pair of fiducials
+  vector<std::pair<double, double>> rotations;
+  // we take the first fiducial as the reference point
+  std::pair<Fiducial, Fiducial> ref_rotation = make_pair(fiducials[0].first,
+      fiducials[0].second);
+  for (std::size_t f_idx = 1; f_idx < fiducials.size(); f_idx++)
+  {
+    rotations.push_back(
+      make_pair(
+        fiducials[f_idx].first.rotation(ref_rotation.first),
+        fiducials[f_idx].second.rotation(ref_rotation.second)));
+  }
+
+  // it is a crude method for now, but we will just compute the mean of the angles of all the fiducial pairs
+  double relative_rotation_sum = 0;
+  for (std::size_t i = 0; i < rotations.size(); i++)
+    relative_rotation_sum += rotations[i].first - rotations[i].second;
+  const double rotation = relative_rotation_sum / rotations.size();
+
   // calculate the distances between each fiducial on their levels
   vector<std::pair<double, double>> distances;
   for (std::size_t f0_idx = 0; f0_idx < fiducials.size(); f0_idx++)
@@ -549,8 +570,12 @@ Building::Transform Building::compute_transform(
   double trans_y_sum = 0;
   for (const auto& fiducial : fiducials)
   {
-    trans_x_sum += fiducial.second.x - fiducial.first.x * scale;
-    trans_y_sum += fiducial.second.y - fiducial.first.y * scale;
+    trans_x_sum += fiducial.second.x -
+      (std::cos(rotation) * fiducial.first.x + std::sin(rotation) *
+      fiducial.first.y) * scale;
+    trans_y_sum += fiducial.second.y -
+      (-std::sin(rotation) * fiducial.first.x + std::cos(rotation) *
+      fiducial.first.y) * scale;
   }
   const double trans_x = trans_x_sum / fiducials.size();
   const double trans_y = trans_y_sum / fiducials.size();
@@ -559,13 +584,16 @@ Building::Transform Building::compute_transform(
   t.scale = scale;
   t.dx = trans_x;
   t.dy = trans_y;
+  t.rotation = rotation;
 
-  printf("transform %d->%d: scale = %.5f translation = (%.2f, %.2f)\n",
+  printf(
+    "transform %d->%d: scale = %.5f translation = (%.2f, %.2f) rotation = %.5f\n",
     from_level_idx,
     to_level_idx,
     t.scale,
     t.dx,
-    t.dy);
+    t.dy,
+    t.rotation);
 
   return t;
 }
@@ -772,6 +800,16 @@ Polygon::EdgeDragPolygon Building::polygon_edge_drag_press(
   return levels[level_idx].polygon_edge_drag_press(polygon, x, y);
 }
 
+Lift Building::get_lift(const std::string& name) const
+{
+  for (const auto& lift : lifts)
+  {
+    if (lift.name == name)
+      return lift;
+  }
+  return Lift();
+}
+
 void Building::add_constraint(
   const int level_idx,
   const QUuid& a,

rmf_traffic_editor/gui/building.h

@@ -147,6 +147,7 @@ class Building
     double scale = 1.0;
     double dx = 0.0;
     double dy = 0.0;
+    double rotation = 0.0;
   };
   typedef std::map<LevelPair, Transform> TransformMap;
   TransformMap transforms;
@@ -195,6 +196,8 @@ class Building
     const double x,
     const double y);
 
+  Lift get_lift(const std::string& name) const;
+
 private:
   std::string filename;
 };

rmf_traffic_editor/gui/editor.cpp

@@ -2009,11 +2009,57 @@ void Editor::mouse_move(
     }
     else if (mouse_vertex_idx >= 0)
     {
+      if (!building.levels[level_idx].vertices[mouse_vertex_idx].lift_cabin()
+        .empty())
+      {
+        auto lift_name =
+          building.levels[level_idx].vertices[mouse_vertex_idx].lift_cabin();
+        auto lift = building.get_lift(lift_name);
+        auto lift_ref_level = lift.reference_floor_name;
+        if (lift_ref_level != building.levels[level_idx].name)
+        {
+          QMessageBox::critical(
+            this,
+            "Could not move vertex",
+            "Vertex is not on the reference level.");
+          return;
+        }
+      }
+
       // we're dragging a vertex
       Vertex& pt =
         building.levels[level_idx].vertices[mouse_vertex_idx];
       pt.x = p.x();
       pt.y = p.y();
+
+      // update the other levels if a lift cabin vertex is moved
+      if (!building.levels[level_idx].vertices[mouse_vertex_idx].lift_cabin()
+        .empty())
+      {
+        for (size_t i = 0; i < building.levels.size(); i++)
+        {
+          if (i == static_cast<size_t>(level_idx))
+            continue;
+
+          auto& level = building.levels[i];
+          auto& vertices = level.vertices;
+          auto t = building.get_transform(level_idx, i);
+
+          for (auto& vertex : level.vertices)
+          {
+            if (vertex.lift_cabin() == pt.lift_cabin())
+            {
+              auto rotated_x = std::cos(-t.rotation)*p.x() -
+                std::sin(-t.rotation)*p.y();
+              auto rotated_y = std::sin(-t.rotation)*p.x() +
+                std::cos(-t.rotation)*p.y();
+              vertex.x = rotated_x * t.scale + t.dx;
+              vertex.y = rotated_y * t.scale + t.dy;
+            }
+          }
+        }
+      }
+
       latest_move_vertex->set_final_destination(p.x(), p.y());
       create_scene();
     }

rmf_traffic_editor/gui/fiducial.cpp

@@ -90,3 +90,10 @@ double Fiducial::distance(const Fiducial& f)
   const double dy = f.y - y;
   return std::sqrt(dx*dx + dy*dy);
 }
+
+double Fiducial::rotation(const Fiducial& f)
+{
+  const double dx = x - f.x;
+  const double dy = y - f.y;
+  return std::atan2(dy, dx);
+}

rmf_traffic_editor/gui/fiducial.h

@@ -46,6 +46,7 @@ class Fiducial
   void draw(QGraphicsScene*, const double meters_per_pixel) const;
 
   double distance(const Fiducial& f);
+  double rotation(const Fiducial& f);
 };
 
 #endif

rmf_traffic_editor/gui/lift.cpp

@@ -145,7 +145,8 @@ void Lift::draw(
   const bool apply_transformation,
   const double scale,
   const double translate_x,
-  const double translate_y) const
+  const double translate_y,
+  const double rotation) const
 {
   if (elevation > highest_elevation || elevation < lowest_elevation)
     return;
@@ -216,8 +217,11 @@ void Lift::draw(
 
   if (apply_transformation)
   {
-    group->setRotation(-180.0 / 3.1415926 * yaw);
-    group->setPos(x * scale + translate_x, y * scale + translate_y);
+    auto rotated_x = std::cos(-rotation)*x - std::sin(-rotation)*y;
+    auto rotated_y = std::sin(-rotation)*x + std::cos(-rotation)*y;
+    group->setRotation(-180.0 / 3.1415926 * (yaw + rotation));
+    group->setPos(rotated_x * scale + translate_x,
+      rotated_y * scale + translate_y);
   }
 }
 

rmf_traffic_editor/gui/lift.h

@@ -92,7 +92,8 @@ class Lift
     const bool apply_transformation = true,
     const double scale = 1.0,
     const double translate_x = 0.0,
-    const double translate_y = 0.0) const;
+    const double translate_y = 0.0,
+    const double rotation = 0.0) const;
 
   bool level_door_opens(
     const std::string& level_name,