Add rotate scenario script

diff --git a/rot_scen.py b/rot_scen.py
new file mode 100644 (file)
index 0000000..370b141
--- /dev/null
+++ b/rot_scen.py
@@ -0,0 +1,49 @@
+# -*- coding: utf-8 -*-
+"""Rotate scenario."""
+from json import dumps
+from math import cos, pi, sin
+from plot import load_scenario
+
+ROTI = 11
+RA = ROTI*pi/8
+TOLOAD = "rpar"
+SAVEAS = "rotlpar"
+
+if __name__ == "__main__":
+    s = load_scenario("{}.json".format(TOLOAD))
+    ns = {}
+    # init
+    (x, y, h) = s["init"]
+    nx = x * cos(RA) - y * sin(RA)
+    ny = x * sin(RA) + y * cos(RA)
+    nh = h + RA
+    ns["init"] = (nx, ny, nh)
+    # goal
+    (x, y, h) = s["goal"]
+    nx = x * cos(RA) - y * sin(RA)
+    ny = x * sin(RA) + y * cos(RA)
+    nh = h + RA
+    ns["goal"] = (nx, ny, nh)
+    # slot
+    ns["slot"] = {}
+    ns["slot"]["polygon"] = []
+    for (x, y) in s["slot"]["polygon"]:
+        nx = x * cos(RA) - y * sin(RA)
+        ny = x * sin(RA) + y * cos(RA)
+        ns["slot"]["polygon"].append((nx, ny))
+    #obst
+    ns["obst"] = []
+    for o in s["obst"]:
+        try:
+            no = {}
+            no["segment"] = []
+            for (x, y) in o["segment"]:
+                nx = x * cos(RA) - y * sin(RA)
+                ny = x * sin(RA) + y * cos(RA)
+                no["segment"].append((nx, ny))
+            ns["obst"].append(no)
+        except:
+            pass
+    # save
+    with open("{}.json".format(SAVEAS), "w") as f:
+        f.write(dumps(ns))