orientation and phase

src/hello_world/__init__.py

@@ -5,6 +5,10 @@ class  Vector(NamedTuple):
     x: float
     y: float
 
+class Orientation(NamedTuple):
+    theta_radians: float
+    theta_degrees: float
+
 current_pos = Vector(3,2)
 target_pos = Vector(7,5)
 zero_vector = Vector(0,0)
@@ -18,9 +22,20 @@ def main() -> None:
     distance =  length(displacement)
     print(distance)
 
-    direction = safe_normalize(displacement, 2)  # minimum distance = 0.001
+    direction = safe_normalize(displacement, 0.001)  # minimum distance = 0.001 or 1.0e-6 
+                                                    # the safe guard depends on the our coordinate scale (ex. cm or m)
     print(direction)
 
+    theta = orientation(displacement)
+    print(theta)
+
+    #phase = wrap_phase(theta.theta_radians)
+    phase = wrap_phase(6.5)
+    print(phase) 
+
+    phase = wrap_phase_v2(730)
+    print(phase)
+
 
 
 ## calculate the displacement from current position to target position
@@ -39,7 +54,9 @@ def length(vector: Vector) -> float:
     return math.sqrt(squared_x + squared_y)
 
 ## safe normalisation (keep the direction while ignoring the distance)
-# The minimum-distance check also prevents division by zero.
+# The minimum-distance is a safeguard. It  says that extremely small distances should be treated 
+    # as too small to produce a stable direction and also prevents division by zero.
+# the safe guard depends on the system coordinate scale (ex. cm or m)
 
 def safe_normalize(vector: Vector, minimum_distance: float) -> Vector:
     vector_length = length(vector)
@@ -50,3 +67,34 @@ def safe_normalize(vector: Vector, minimum_distance: float) -> Vector:
     einheitsVector = Vector(vector.x / vector_length, vector.y / vector_length)  # unit vector
     return einheitsVector
 
+## angle in radians an degrees betwen -pi and pi
+
+def orientation(displacement: Vector) -> Orientation:
+    angle =  math.atan2(displacement.y, displacement.x)
+    angle_degree = math.degrees(angle)
+    rv = Orientation(angle, angle_degree)
+    return rv
+
+##  wraping a phase
+
+def wrap_phase(phase: float) -> float:
+    full_turn = 2*math.pi
+    if phase >= full_turn:
+        return phase % full_turn
+
+    return phase
+
+def wrap_phase_v2(phase: int) -> int:
+    full_turn = 360
+    if phase >= full_turn:
+        return phase % full_turn
+
+    return phase
+
+
+
+
+
+
+
+