def rotate_ground_and_move(physics: SimPhysics,
controls: SimpleControllerState, dt: float,
normal: Vec3):
physics.velocity += Vec3(0, 0, -650 * dt)
remove_velocity_against_normal(physics.velocity, normal, dt)
orientation = Orientation(physics.rotation)
orig_rot_mat = orientation.to_rot_mat()
physics_prime = SimPhysics(
Vec3(0, 0, 0),
Vec3(
orientation.forward.dot(physics.velocity),
orientation.right.dot(physics.velocity),
0,
),
physics.angular_velocity, # ground move just ignores it
Rotator(0, 0, 0),
)
# print(f"Steer: {controls.steer}, Throttle: {controls.throttle}")
move_on_ground(physics_prime, controls, dt)
# need to combine orientations
old_yaw = physics.rotation.yaw
new_orientation = Orientation(physics_prime.rotation)
physics.rotation = rot_mat_to_rot(
dot(orientation.to_rot_mat(), new_orientation.to_rot_mat()))
# unrotate other vectors
# if physics.velocity.dot(orientation.forward) < 0:
# controls.steer = -controls.steer
# inverse_rotation = transpose(orig_rot_mat)
physics.location += (orientation.forward * physics_prime.location.x +
orientation.right * physics_prime.location.y)
physics.velocity = (orientation.forward * physics_prime.velocity.x +
orientation.right * physics_prime.velocity.y)
physics.angular_velocity = physics_prime.angular_velocity # should be unchanged
# what are the chances that this works first try! Very small.
clamp(physics)
# print(f"{old_yaw} + {physics_prime.rotation.yaw} =? {physics.rotation.yaw}")
return physics
def rotate_by_axis(orientation: Orientation,
original: Vec3,
target: Vec3,
percent=1) -> Rotator:
"""
Updates the given orientation's original direction to the target direction.
original should be `orientation.forward` or `orientation.up` or `orientation.right`
"""
angle = angle_between(to_rlu_vec(target), to_rlu_vec(original)) * percent
rotate_axis = cross(to_rlu_vec(target), to_rlu_vec(original))
ortho = normalize(rotate_axis) * -angle
rot_mat_initial: mat3 = orientation.to_rot_mat()
rot_mat_adj = axis_to_rotation(ortho)
rot_mat = dot(rot_mat_adj, rot_mat_initial)
r = rot_mat_to_rot(rot_mat)
# printO(orientation)
# printO(Orientation(r))
return r