def _ball_plate_contact(self, step_t: float) -> float:
# NOTE: the x_theta axis creates motion in the Y-axis, and vice versa
# x_theta, y_theta = self._xy_theta_from_nor(self.plate_nor.xyz)
x_theta = self.plate_theta_x
y_theta = self.plate_theta_y
# Equations for acceleration on a plate at rest
# accel = (mass * g * theta) / (mass + inertia / radius^2)
# (y_theta,x are intentional swapped here.)
theta = Vector3([y_theta, -x_theta, 0])
self.ball_acc = (theta / (self.ball_mass + self._ball_inertia() /
(self.ball_radius**2)) * self.ball_mass *
self.gravity)
# get contact displacement
disp, vel = self._motion_for_time(self.ball_vel, self.ball_acc, step_t)
# simplified ball mechanics against a plane
self.ball.x += disp.x
self.ball.y += disp.y
self._update_ball_z()
self.ball_vel = vel
# For rotation on plate motion we use infinite friction and
# perfect ball / plate coupling.
# Calculate the distance we traveled across the plate during
# this time slice.
rot_distance = math.hypot(disp.x, disp.y)
if rot_distance > 0:
# Calculate the fraction of the circumference that we traveled
# (in radians).
rot_angle = rot_distance / self.ball_radius
# Create a quaternion that represents the delta rotation for this time period.
# Note that we translate the (x, y) direction into (y, -x) because we're
# creating a vector that represents the axis of rotation which is normal
# to the direction the ball traveled in the x/y plane.
rot_q = quaternion.normalize(
np.array([
disp.y / rot_distance * math.sin(rot_angle / 2.0),
-disp.x / rot_distance * math.sin(rot_angle / 2.0),
0.0,
math.cos(rot_angle / 2.0),
]))
old_rot = self.ball_qat.xyzw
new_rot = quaternion.cross(quat1=old_rot, quat2=rot_q)
self.ball_qat.xyzw = quaternion.normalize(new_rot)
return 0.0
def test_normalize_non_identity(self):
# normalize an identity quaternion
result = quaternion.normalize([1., 2., 3., 4.])
np.testing.assert_almost_equal(result, [
1. / np.sqrt(30.),
np.sqrt(2. / 15.),
np.sqrt(3. / 10.), 2. * np.sqrt(2. / 15.)
],
decimal=5)
def test_normalize_batch(self):
# normalize an identity quaternion
result = quaternion.normalize([
[0., 0., 0., 1.],
[1., 2., 3., 4.],
])
expected = [
[0., 0., 0., 1.],
[1. / np.sqrt(30.), np.sqrt(2. / 15.), np.sqrt(3. / 10.), 2. * np.sqrt(2. / 15.)],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_normalize_batch(self):
# normalize an identity quaternion
result = quaternion.normalize([
[0., 0., 0., 1.],
[1., 2., 3., 4.],
])
expected = [
[0., 0., 0., 1.],
[1. / np.sqrt(30.), np.sqrt(2. / 15.), np.sqrt(3. / 10.), 2. * np.sqrt(2. / 15.)],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_normalize_identity(self):
# normalize an identity quaternion
result = quaternion.normalize([0., 0., 0., 1.])
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
def test_normalize_non_identity(self):
# normalize an identity quaternion
result = quaternion.normalize([1., 2., 3., 4.])
np.testing.assert_almost_equal(result, [1. / np.sqrt(30.), np.sqrt(2. / 15.), np.sqrt(3. / 10.), 2. * np.sqrt(2. / 15.)], decimal=5)
def test_normalize_identity(self):
# normalize an identity quaternion
result = quaternion.normalize([0., 0., 0., 1.])
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
