43 lines
1.4 KiB
Python
43 lines
1.4 KiB
Python
import numpy as np
|
|
|
|
from objects.generic import Source, Target
|
|
from objects.motor import Motor
|
|
|
|
from calculator import get_angles
|
|
|
|
class Mirror:
|
|
def __init__(self, world, cluster_x=0, cluster_y=0):
|
|
self.world = world
|
|
self.cluster_x = cluster_x
|
|
self.cluster_y = cluster_y
|
|
|
|
# Store the motors
|
|
# Need to get first the theta because
|
|
# of the ordeing of the cables on the board
|
|
self.motor_theta: Motor = Motor(self.world.board)
|
|
self.motor_phi: Motor = Motor(self.world.board)
|
|
|
|
# Position in un-tilted coordinate system
|
|
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)
|
|
|
|
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 = self.get_pos_rotated()
|
|
rel_source = source.pos - rot_pos
|
|
rel_target = target.pos - rot_pos
|
|
|
|
phi, theta = get_angles(rel_source, rel_target) # ty:ignore[unresolved-reference]
|
|
|
|
# Update the angles based on the normals in rotated positions
|
|
self.motor_phi.set_angle(phi)
|
|
self.motor_theta.set_angle(theta)
|
|
|
|
def get_angles(self):
|
|
return self.motor_phi.angle, self.motor_theta.angle
|