Some refactor and tried to figure out the angles right

objects/solar.py

@@ -1,128 +1,27 @@
-"""Alle gemessenen Koordinaten der Quelle und der Sonne haben den Ursprung in der linken unteren Ecke des Clusters in einem rechtshaendigen flachen System.
-"""
+"""Alle gemessenen Koordinaten der Quelle und der Sonne haben den Ursprung in der linken unteren Ecke des Clusters in einem rechtshaendigen flachen System."""
+
+from objects.generic import Source, Target
 
 import math
 import objects.motor as motor
 import numpy as np
+from objects.calculator import get_angles
 
-# 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."""
-
-    def __init__(self, world):
-        self.world = world
-        self.pos = (0.0, 0.0, 0.0)  # (x, y, z) in local untilted coordinates
-
-    def get_pos_rotated(self):
-        """Return position rotated by world's tilt around y-axis."""
-        return self.world.rotate_point_y(self.pos)
-
-    def move(self, dx=0, dy=0, dz=0):
-        self.pos = (self.pos[0] + dx, self.pos[1] + dy, self.pos[2] + dz)
-
-class Target(MovingEntity):
-    def __init__(self, world, pos=(0.0, 0.0, 0.0)):
-        super().__init__(world)
-        self.pos = pos
-
-class Source(MovingEntity):
-    def __init__(self, world, pos=(10.0, 10.0, 10.0)):
-        super().__init__(world)
-        self.pos = pos
 
 class Mirror:
     def __init__(self, world, cluster_x=0, cluster_y=0):
-        self.world = world
+        self.world: World = world
         self.cluster_x = cluster_x
         self.cluster_y = cluster_y
 
         # Store the motors
-        self.phi = motor.Motor(self.world.board)
         self.theta = motor.Motor(self.world.board)
+        self.phi = 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)
+        self.pos = np.array(
+            [cluster_x * self.world.grid_size, cluster_y * self.world.grid_size, 0.0]
+        )
 
     def get_pos_rotated(self):
         return self.world.rotate_point_y(self.pos)
@@ -130,9 +29,9 @@ class Mirror:
     def set_angle_from_source_target(self, source: Source, target: Target):
         "Set the angles of a mirror from global source and target vectors."
 
-        rot_pos = np.array([self.get_pos_rotated()])
-        rel_source = np.array([source.pos]) - rot_pos
-        rel_target = np.array([target.pos]) - rot_pos
+        rot_pos = self.get_pos_rotated()
+        rel_source = source.pos - rot_pos
+        rel_target = target.pos - rot_pos
 
         phi, theta = get_angles(rel_source, rel_target)
 
@@ -143,13 +42,14 @@ class Mirror:
     def get_angles(self):
         return self.phi.angle, self.theta.angle
 
+
 class World:
     def __init__(self, board, tilt_deg=0.0):
         self.board = board
 
         self.grid_size = 10  # In cm
         self.tilt_deg = tilt_deg  # Tilt of the grid system around y-axis
-        self.mirrors = []
+        self.mirrors: list[Mirror] = []
 
     def add_mirror(self, mirror):
         self.mirrors.append(mirror)
@@ -167,4 +67,4 @@ class World:
         x_rot = x * cos_t + z * sin_t
         y_rot = y
         z_rot = -x * sin_t + z * cos_t
-        return (x_rot, y_rot, z_rot)
+        return np.array([x_rot, y_rot, z_rot])