def heading_to_point(
start_x: float,
start_y: float,
vel_x: float,
vel_y: float,
point_x: float,
point_y: float,
):
"""
Return a heading, in 2D RH coordinate system.
x,y: the current position of the object
vel_x, vel_y: the current velocity vector of motion for the object
point_x, point_y: the destination point to head towards
returns: offset angle in radians in the range [-pi .. pi]
where:
0.0: object is moving directly towards the point
[-pi .. <0]: object is moving to the "right" of the point
[>0 .. -pi]: object is moving to the "left" of the point
[-pi, pi]: object is moving directly away from the point
"""
# vector to point
dx = point_x - start_x
dy = point_y - start_y
# if the ball is already at the target location or
# is not moving, return a heading of 0 so we don't
# attempt to normalize a zero-length vector
if dx == 0 and dy == 0:
return 0
if vel_x == 0 and vel_y == 0:
return 0
# vectors and lengths
u = vector.normalize([dx, dy, 0.0])
v = vector.normalize([vel_x, vel_y, 0.0])
ul = vector.length(u)
vl = vector.length(v)
# no velocity? already on the target?
angle = 0.0
if (ul != 0.0) and (vl != 0.0):
# angle between vectors
uv_dot = vector.dot(u, v)
# signed angle
x = u[0]
y = u[1]
angle = math.atan2(vector.dot([-y, x, 0.0], v), uv_dot)
if math.isnan(angle):
angle = 0.0
return angle
def behave(self, params):
if params.mouse.left_pressed:
if self._start is None:
self._start = params.mouse.location
self._origin = params.scene.camera_target
self._camera_pos = params.scene.camera_position
cam_dir = vector.normalize(self._camera_pos - self._origin)
self._right = np.cross(params.scene.up_vector, cam_dir)
self._up = np.cross(cam_dir, self._right)
else:
size = params.scene.size
end = params.mouse.location
deltaX = float(end[0] - self._start[0]) / size[0]
deltaY = float(end[1] - self._start[1]) / size[1]
Rx = matrix33.create_from_axis_rotation(
self._up, deltaX * 2 * np.pi)
Ry = matrix33.create_from_axis_rotation(
self._right, deltaY * 2 * np.pi)
R = Ry.dot(Rx)
newpos = self._origin + R.dot(self._camera_pos - self._origin)
newup = R.dot(self._up)
params.scene.camera_position = newpos
params.scene.up_vector = newup
params.refresh = True
else:
self._start = None
def behave(self, params):
rotations = params.mouse.wheel_rotation
if rotations != 0:
cam_position = params.scene.camera_position
cam_target = params.scene.camera_target
ray = vector.normalize(cam_target - cam_position)
cam_position += ray * self._delta * rotations
params.scene.camera_position = cam_position
params.refresh = True
def update_camera_vectors(self):
front = Vector3([0.0, 0.0, 0.0])
front.x = cos(radians(self.pitch)) * cos(radians(self.yaw))
front.y = sin(radians(self.pitch))
front.z = cos(radians(self.pitch)) * sin(radians(self.yaw))
self.camera_front = vector.normalize(front)
self.camera_right = vector.normalise(
vector3.cross(self.camera_front, Vector3([0.0, 1.0, 0.0])))
self.camera_up = vector.normalise(
vector3.cross(self.camera_right, self.camera_front))
def _plate_nor(self) -> Vector3:
x_rot = matrix44.create_from_axis_rotation(axis=X_AXIS,
theta=self.plate_theta_x)
y_rot = matrix44.create_from_axis_rotation(axis=Y_AXIS,
theta=self.plate_theta_y)
# pitch then roll
nor = matrix44.apply_to_vector(mat=x_rot, vec=Z_AXIS)
nor = matrix44.apply_to_vector(mat=y_rot, vec=nor)
nor = vector.normalize(nor)
return Vector3(nor)
def update_camera_vectors(self):
if not self.rotate_around_base:
front: Vector3 = Vector3([0.0, 0.0, 0.0])
front.x = cos(radians(self.yaw + self.yaw_offset)) * cos(
radians(self.pitch))
front.y = sin(radians(self.pitch))
front.z = sin(radians(self.yaw + self.yaw_offset)) * cos(
radians(self.pitch))
self.camera_front = vector.normalize(front)
self.camera_right = vector.normalize(
vector3.cross(self.camera_front, Vector3([0.0, 1.0, 0.0])))
self.camera_up = vector.normalise(
vector3.cross(self.camera_right, self.camera_front))
self.camera_pos = self.camera_pos + self.camera_right * self.move_vector.x * self.move_speed
self.camera_pos = self.camera_pos + self.camera_up * self.move_vector.y * self.move_speed
self.camera_pos = self.camera_pos + self.camera_front * self.move_vector.z * self.move_speed
else:
self.yaw_offset += self.rotation_speed
front: Vector3 = Vector3([0.0, 0.0, 0.0])
front.x = cos(radians(self.yaw + self.yaw_offset)) * cos(
radians(self.pitch))
front.y = sin(radians(self.pitch))
front.z = sin(radians(self.yaw + self.yaw_offset)) * cos(
radians(self.pitch))
front_offset = vector.normalize(front) - self.camera_front
self.camera_pos -= front_offset * vector.length(self.camera_pos -
self.base)
self.camera_front = vector.normalize(front)
self.camera_right = vector.normalize(
vector3.cross(self.camera_front, Vector3([0.0, 1.0, 0.0])))
self.camera_up = vector.normalise(
vector3.cross(self.camera_right, self.camera_front))
self.camera_pos = self.camera_pos + self.camera_right * self.move_vector.x * self.move_speed
self.camera_pos = self.camera_pos + self.camera_up * self.move_vector.y * self.move_speed
self.camera_pos = self.camera_pos + self.camera_front * self.move_vector.z * self.move_speed
def behave(self, params):
if params.mouse.middle_pressed:
if self._start is None:
self._start = params.mouse.location
self._target = params.scene.camera_target
cam_dir = params.scene.camera_position - self._target
cam_dir = vector.normalize(cam_dir)
self._right = np.cross(params.scene.up_vector, cam_dir)
self._up = np.cross(cam_dir, self._right)
else:
size = params.scene.size
end = params.mouse.location
deltaX = float(end[0] - self._start[0]) / size[0]
deltaY = float(end[1] - self._start[1]) / size[1]
newtarget = (self._target +
-self._delta * deltaX * self._right +
self._delta * deltaY * self._up)
params.scene.camera_target = newtarget
params.refresh = True
else:
self._start = None
def strafe_right(self):
self._camera_position = self._camera_position + vector.normalize(self._camera_front ^ self._camera_up) * self._move_horizontally
self.build_look_at()
def strafe_right(self):
self._camera_position = self._camera_position + vector.normalize(
self._camera_front ^ self._camera_up) * self._move_horizontally
self.build_look_at()
def strafe_right(self, x=0.1):
self._camera_position = self._camera_position + vector.normalize(self._camera_front ^ self._camera_up) * x
self.build_look_at()
def _right(self):
return vector.normalize(self._front ^ self._up)
