Fix rotation

map_to_kml.py

@@ -103,18 +103,29 @@ def add_polygone(root, coordinate_tuple_list, style_map_id):
   coordinates.text = ' '.join(
     (','.join(str(x) for x in coor_tuple) for coor_tuple in coordinate_tuple_list))
 
-def build_rectangle(root, lat, lon, length, width, height, style_map_id, map_info, map_size):
+def rotate(x, y, x_o, y_o, rotation):
+  """Apply rotation to coordinates."""
+  return (
+    x_o + math.cos(rotation) * (x - x_o) - math.sin(rotation) * (y - y_o),
+    y_o + math.sin(rotation) * (x - x_o) + math.cos(rotation) * (y - y_o),
+  )
+
+def build_rectangle(root, lat, lon, length, width, height, rotation,
+                    style_map_id, map_info, map_size):
   """Build a rectangle."""
   local_x, local_y = convert_to_local_coordinates(lat, lon, map_info, map_size)
-  max_lon, _ = convert_to_geo_coordinates(local_x + length, local_y, map_info, map_size)
-  _, max_lat = convert_to_geo_coordinates(local_x, local_y + width, map_info, map_size)
-  add_polygone(root, (
-    (lat, lon, height),
-    (lat, max_lon, height),
-    (max_lat, max_lon, height),
-    (max_lat, lon, height),
-    (lat, lon, height),
-  ), style_map_id)
+  min_x = local_x - length / 2
+  max_x = local_x + length / 2
+  min_y = local_y - width / 2
+  max_y = local_y + width / 2
+
+  polygone_coordinates = []
+  for x, y in ((min_x, min_y), (max_x, min_y), (max_x, max_y), (min_x, max_y), (min_x, min_y)):
+    rotated_x, rotated_y = rotate(x, y, local_x, local_y, rotation)
+    corner_lon, corner_lat = convert_to_geo_coordinates(rotated_x, rotated_y, map_info, map_size)
+    polygone_coordinates.append((corner_lat, corner_lon, height))
+
+  add_polygone(root, polygone_coordinates, style_map_id)
 
 
 def write_kml(filename, map_dict):
@@ -151,14 +162,14 @@ def write_kml(filename, map_dict):
   for obstacle in map_dict['obstacle_list']:
     position = obstacle['position']
     scale = obstacle['scale']
-    rotation = obstacle['rotation']['z'] * math.pi / 180
     build_rectangle(
       document,
       position['latitude'],
       position['longitude'],
-      scale['x'] * math.cos(rotation) + scale['y'] * math.sin(rotation),
-      - scale['x'] * math.sin(rotation) + scale['y'] * math.cos(rotation),
+      scale['x'],
+      scale['y'],
       position['altitude'] + scale['z'],
+      - obstacle['rotation']['z'] * math.pi / 180,
       'obstacle',
       map_info,
       map_size,
@@ -167,7 +178,7 @@ def write_kml(filename, map_dict):
   for flag in map_dict['flag_list']:
     position = flag['position']
     build_rectangle(document, position['latitude'], position['longitude'],
-                    1, 1, position['altitude'], 'flag', map_info, map_size)
+                    1, 1, position['altitude'], 0, 'flag', map_info, map_size)
 
   tree = ET.ElementTree(root)
   ET.indent(tree, '   ')