Fix everything?

.python-version

@@ -1 +1 @@
-3.12
+3.13

objects/solar.py

@@ -3,6 +3,89 @@
 
 import math
 import objects.motor as motor
+import numpy as np
+
+# Einheitsvektoren
+unit_x = np.array([1, 0, 0])
+unit_y = np.array([0, 1, 0])
+unit_z = np.array([0, 0, 1])
+
+def get_axis(axis):
+    "Axis are numbered from 1 to 3 from x to z."
+    match axis:
+        case 1:
+            ax = unit_x
+        case 2:
+            ax = unit_y
+        case 3:
+            ax = unit_z
+        case _:
+            ax = unit_x
+    return ax
+
+def proj(vec, axis: int =1):
+    """Simple vector projection onto an axis."""    
+    ax = get_axis(axis)
+    return np.dot(vec, ax) * ax
+
+def abs_custom(vec):
+    l = 0
+    for i in range(3):
+        l += vec[i] ** 2
+    return np.sqrt(l)
+
+def rotate(v, angle=90, axis=1):
+    "Rotate a vector with an angle around a axis with the right hand rule."
+    angle = angle/180 * np.pi
+    k = get_axis(axis)
+
+    return (
+        v * np.cos(angle)
+        + np.cross(k, v) * np.sin(angle)
+        + k * np.dot(k, v) * (1 - np.cos(angle))
+    )
+        
+def agl(a, b):
+    "Get the angle between two vectors. This is always between 0 and 180 degree."
+    return np.round(np.acos(np.dot(a, b)/(abs_custom(a) * abs_custom(b)))/(2 * np.pi) * 360)
+
+def normalize(vec):
+    l = abs_custom(vec)
+    return vec/l
+
+def get_angles(source, target):
+    """Main function to get the phi and theta angles for a source and a target vector. Both vectors must lie on the front half sphere.
+       Phi is from 0 to 180 where 0 means left when you look at the mirrors. The hardware is bounded between 45 and 135 degree. Thus the here provided angle needs to be subtracted by 45 and then doubled.
+       Theta is from 0 to 90 where 0 means up."""
+    source_planar = source - proj(source, 3)
+    target_planar = target - proj(target, 3)
+
+    source_phi = agl(source_planar, unit_x)
+    target_phi = agl(target_planar, unit_x)
+
+    source_theta = agl(rotate(source, 90 - source_phi, 3), unit_z)
+    target_theta = agl(rotate(target, 90 - target_phi, 3), unit_z)
+
+    phi = None
+    theta = None
+
+    theta_diff = None
+    phi_diff = agl(source_planar, target_planar)
+    if source_phi < target_phi:
+        rota = rotate(source_planar, phi_diff, 3)
+        theta_diff = agl(rota, target)
+        phi = source_phi + phi_diff/2
+    else:
+        rota = rotate(target_planar, phi_diff, 3)
+        theta_diff = agl(rota, source)
+        phi = target_phi + phi_diff/2
+
+    if source_theta < target_theta:
+        theta = target_theta + theta_diff/2
+    else:
+        theta = source_theta + theta_diff/2
+
+    return (phi, theta)
 
 class MovingEntity:
     """Embedded entity in the world with a position."""
@@ -35,8 +118,8 @@ class Mirror:
         self.cluster_y = cluster_y
 
         # Store the motors
-        self.yaw = motor.Motor(self.world.board)
+        self.phi = motor.Motor(self.world.board)
-        self.pitch = motor.Motor(self.world.board)
+        self.theta = motor.Motor(self.world.board)
 
         # Position in un-tilted coordinate system
         self.pos = (cluster_x * self.world.grid_size, cluster_y * self.world.grid_size, 0.0)
@@ -45,44 +128,20 @@ class Mirror:
         return self.world.rotate_point_y(self.pos)
 
     def set_angle_from_source_target(self, source: Source, target: Target):
-        # Get rotated positions
+        "Set the angles of a mirror from global source and target vectors."
-        pos_mirror = self.get_pos_rotated()
-        pos_source = source.get_pos_rotated()
-        pos_target = target.get_pos_rotated()
 
-        v_source = (
+        rot_pos = np.array([self.get_pos_rotated()])
-            pos_source[0] - pos_mirror[0],
+        rel_source = np.array([source.pos]) - rot_pos
-            pos_source[1] - pos_mirror[1],
+        rel_target = np.array([target.pos]) - rot_pos
-            pos_source[2] - pos_mirror[2],
-        )
-        v_target = (
-            pos_target[0] - pos_mirror[0],
-            pos_target[1] - pos_mirror[1],
-            pos_target[2] - pos_mirror[2],
-        )
 
-        def normalize(v):
+        phi, theta = get_angles(rel_source, rel_target)
-            length = math.sqrt(v[0] ** 2 + v[1] ** 2 + v[2] ** 2)
-            if length == 0:
-                return (0, 0, 0)
-            return (v[0] / length, v[1] / length, v[2] / length)
-
-        v_source_n = normalize(v_source)
-        v_target_n = normalize(v_target)
-
-        mirror_normal = (
-            v_source_n[0] + v_target_n[0],
-            v_source_n[1] + v_target_n[1],
-            v_source_n[2] + v_target_n[2],
-        )
-        mirror_normal = normalize(mirror_normal)
 
         # Update the angles based on the normals in rotated positions
-        self.yaw.set_angle(math.degrees(math.atan2(mirror_normal[0], mirror_normal[2])))
+        self.phi.set_angle(phi)
-        self.pitch.set_angle(math.degrees(math.atan2(mirror_normal[1], mirror_normal[2])))
+        self.theta.set_angle(theta)
 
     def get_angles(self):
-        return self.yaw.angle, self.pitch.angle
+        return self.phi.angle, self.theta.angle
 
 class World:
     def __init__(self, board, tilt_deg=0.0):

simulation.py

@@ -12,15 +12,15 @@ LOOP_DELAY = 0.005 # In seconds
 
 # Testing embedding the mirrors in the world
 board = Board()
-world = solar.World(board, tilt_deg=15)  # The world is tilted 15 degrees around y-axis
+world = solar.World(board, tilt_deg=0)
 
 HEIGHT = 30
 
-source = solar.Source(world, pos=(30, 50, 100))
+source = solar.Source(world, pos=(0, 50, 0))
-target = solar.Target(world, pos=(0, 0, 40))
+target = solar.Target(world, pos=(0, 50, 0))
 
 # Create mirrors in a 3x2 grid
-for x in range(3):
+for x in range(4):
     for y in range(2):
         mirror = solar.Mirror(world, cluster_x=x, cluster_y=y)
         world.add_mirror(mirror)