Reorder heading computation in next method

The reason is to be the same as used in the bcar C++ library.

diff --git a/path-to-traj.py b/path-to-traj.py
index a96fc3148443398bddc47956704ad2797bc40676..79df4d4bb215adf3cb990252165808dd6745c459 100755 (executable)
--- a/path-to-traj.py
+++ b/path-to-traj.py
@@ -194,13 +194,13 @@ class BicycleCar:
             elif self.acc > 0 and self.sp > 0:
                 self.acc = 0
                 self.sp = 0
+        self.x += DT * self.sp * cos(self.h)
+        self.y += DT * self.sp * sin(self.h)
         self.h += self.sp / self.wb * tan(self.wa) * DT
         while self.h > pi:
             self.h -= pi
         while self.h < -pi:
             self.h += pi
-        self.x += DT * self.sp * cos(self.h)
-        self.y += DT * self.sp * sin(self.h)
 
     def brake_within(self, d):
         bd = sgn(self.sp)