Fixed heading calculation by using atan2

2 files changed, 16 insertions, 17 deletions

diff --git a/main.py b/main.py
index 125caa4..a4fad42 100644
--- a/main.py
+++ b/main.py
@@ -94,20 +94,19 @@ def main(argv):
             print("delta: " + str(delta))
             simulation.tick(delta=delta)
 
-        #TODO: draw floor, flame (continuity)
+        #TODO: draw body sprite, rocket sprite, clouds sprites, etc.
+        #TODO: implement height properly (body radius) + actually implement body
         #TODO: do max load on rocket so it blows up
-        #TODO: allow for x movement, speed, accel etc
         #TODO: allow multilanguage api for landing algorithms etc
-        #TODO: probs need cloud sprite that spawns and despawns as well as floor sprite
-
-def linear_gradient(start_color, end_color, length, value_at):
-    return [
-        int(start_color[j] + (float(value_at)/(length-1))*(end_color[j]-start_color[j]))
-        for j in range(3)
-    ]
 
 def draw_simulation(simulation_display: type[pygame.Surface], simulation: type[Simulation]) -> None:        
         #draw background
+        def linear_gradient(start_color, end_color, length, value_at):
+            return [
+                int(start_color[j] + (float(value_at)/(length-1))*(end_color[j]-start_color[j]))
+                for j in range(3)
+            ]
+            
         def get_color_for_height(height: float) -> (int, int, int):
             if height < 70000:
                 return linear_gradient((31,118,194), (0, 0, 0), 70000, int(height))
@@ -170,7 +169,7 @@ def draw_simulation(simulation_display: type[pygame.Surface], simulation: type[S
         flame_radius = 10
         flame_color = (255, 125, 100)
 
-        #TODO: Rotate rocket with gimbal? or with accel? probs with accel
+        #TODO: Rotate rocket with heading
         i = simulation.rocket.stages_spent
         stage_height = first_stage_height / (i + 1)
         stage_y = rocket_y + first_stage_height - stage_height
diff --git a/simulation.py b/simulation.py
index 70ea171..46a7c57 100644
--- a/simulation.py
+++ b/simulation.py
@@ -93,14 +93,14 @@ class Simulation():
         print("Speed x: " + str(self.speed_x))
         print("Speed y: " + str(self.speed_y))
 
-        #TODO: HEADING behaves a bit weird, just a bit and it goes forever and hard to cancel. WELL CALCULATED OR IMPLEMENTED?
-        #speedx / speedy
-        #1 = 45
-        #-1 = -45
-        #0 = 90
-        self.heading = math.degrees(self.speed_x / self.speed_y) #TODO? con speed, y luego heading influences thrust (gimbal), so pass as a parameter to func
+        #TODO: WELL CALCULATED? (angle well?)
+        ref_vec = (0, 1)
+        acc_vec = (self.speed_x, self.speed_y)
+        dot = (acc_vec[0] * ref_vec[0]) + (acc_vec[1] * ref_vec[1])
+        det = (acc_vec[0] * ref_vec[1]) - (acc_vec[1] * ref_vec[0])
+        self.heading = math.degrees(math.atan2(det, dot))
         print("Heading: " + str(self.heading))
-
+        
         #update position based on velocity and delta
         self.x += self.speed_x * delta