def test_create_from_bounds(self):
bounds = [[-1., 1., -1.], [2., 1., 0.]]
result = aambb.create_from_bounds(*bounds)
length = max(vector.length(bounds[0]), vector.length(bounds[1]))
self.assertTrue(
np.array_equal(
result,
[[-length, -length, -length], [length, length, length]]))
self.assertTrue(
np.array_equal(aambb.centre_point(result), [0.0, 0.0, 0.0]))
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 create_from_points( points ):
"""Creates an AAMBB from the list of specified points.
Points must be a 2D list. Ie::
numpy.array([
[ x, y, z ],
[ x, y, z ],
])
"""
# convert any negative values to positive
abs_points = numpy.absolute( points )
# extract the maximum extent as a vector
vec = numpy.amax( abs_points, axis = 0 )
# find the length of this vector
length = vector.length( vec )
# our AAMBB extends from +length to -length
# in all directions
return numpy.array(
[
[-length,-length,-length ],
[ length, length, length ]
]
)
def add_points( bb, points ):
"""Extends an AAMBB to encompass a list
of points.
It should be noted that this ensures that
the encompassed points can rotate freely.
Calling this using the min / max points from
the AAMBB will create an even bigger AAMBB.
"""
# add our AABB to the list of points
values = numpy.vstack( points, bb[ 0 ], bb[ 1 ] )
# convert any negative values to positive
abs_points = numpy.absolute( values )
# extract the maximum extent as a vector
vec = numpy.amax( abs_points, axis = 0 )
# find the length of this vector
length = vector.length( vec )
# our AAMBB extends from +length to -length
# in all directions
return numpy.array(
[
[-length,-length,-length ],
[ length, length, length ]
]
)
def test_add_aabbs(self):
a1 = aambb.create_from_bounds([-0.5,-0.5,-0.5], [0.5,0.5,0.5])
a2 = aambb.create_from_bounds([1.0,-2.0, 1.0], [2.0,-1.0, 1.0])
result = aambb.add_aabbs(a1, [a2])
length = np.amax(vector.length([a1, a2]))
self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]]), (result,))
self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def length( quat ):
"""Calculates the length of a quaternion.
:param numpy.array quat: The quaternion to measure.
:rtype: float, numpy.array
:return: If a 1d array was passed, it will be a scalar.
Otherwise the result will be an array of scalars with shape
vec.ndim with the last dimension being size 1.
"""
return vector.length( quat )
def test_add_point(self):
a = aambb.create_from_bounds([-0.5,-0.5,-0.5], [0.5,0.5,0.5])
points = np.array([
[ 2.0,-1.0,-1.0],
[ 1.0, 3.0,-1.0],
])
result = aambb.add_points(a, points)
length = np.amax(vector.length([a, points]))
self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]]), (result,))
self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def test_create_from_aabbs(self):
a1 = aambb.create_from_points([[0.0, 0.0, 0.0], [1.0, 1.0, -1.0]])
a2 = aambb.create_from_points([[0.0, 0.0, 2.0], [-1.0, -1.0, 1.0]])
result = aambb.create_from_aabbs([a1, a2])
length = np.amax(vector.length([a1, a2]))
self.assertTrue(
np.array_equal(
result,
[[-length, -length, -length], [length, length, length]]),
(result, ))
self.assertTrue(
np.array_equal(aambb.centre_point(result), [0.0, 0.0, 0.0]))
def single_vector():
vec = numpy.array( [ 1.0, 1.0, 1.0 ] )
result = vector.length( vec )
expected = math.sqrt( numpy.sum( vec ** 2 ) )
# ensure the calculated length is what we expect
self.assertEqual(
result,
expected,
"Vector length calculation incorrect"
)
def test_create_from_aabbs(self):
a1 = aambb.create_from_points([
[ 0.0, 0.0, 0.0],
[ 1.0, 1.0,-1.0]
])
a2 = aambb.create_from_points([
[ 0.0, 0.0, 2.0],
[-1.0,-1.0, 1.0]
])
result = aambb.create_from_aabbs([a1, a2])
length = np.amax(vector.length([a1, a2]))
self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]]), (result,))
self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def batch_lengths():
vec = numpy.array( [ 1.0, 1.0, 1.0 ] )
batch = numpy.tile( vec, (3,1) )
result = vector.length( batch )
expected = numpy.array(
[ math.sqrt( numpy.sum( vec ** 2 ) ) ]
)
expected = numpy.tile( expected, (3) )
self.assertTrue(
numpy.array_equal( result, expected ),
"Vector batch length calculation incorrect"
)
def apply( quat, vec3 ):
"""
v = numpy.array( vec )
return v + 2.0 * vector.cross(
quat[:-1],
vector.cross( quat[:-1], v ) + (quat[-1] * v)
)
"""
length = vector.length( vec3 )
vec3 = vector.normalise( vec3 )
# use the vector to create a new quaternion
# this is basically the vector3 to vector4 conversion with W = 0
vec_quat = numpy.array( [ vec3[ 0 ], vec3[ 1 ], vec3[ 2 ], 0.0 ] )
# quat * vec * quat^-1
result = cross( quat, cross( vec_quat, conjugate( quat ) ) )
return result[ :-1 ] * length
def test_create_from_points( self ):
vecs = numpy.array(
[
[ 0.0, 0.0, 0.0 ],
[ 5.0, 5.0, 5.0 ],
[ 0.0, 0.0, 5.0 ],
[-5.0, 0.0, 0.0 ],
],
dtype = numpy.float
)
# the biggest should be 5,5,5
result = sphere.create_from_points( vecs )
# centred around 0,0,0
# with MAX LENGTH as radius
lengths = vector.length( vecs )
expected = numpy.array( [ 0.0, 0.0, 0.0, lengths.max() ] )
self.assertTrue(
numpy.array_equal( result, expected ),
"Sphere not calculated correctly"
)
def sphere_penetration_sphere( s1, s2 ):
"""Calculates the distance two spheres have penetrated
into one another.
:param numpy.array s1: The first circle.
:param numpy.array s2: The second circle.
:rtype: float
:return: The total overlap of the two spheres.
This is essentially:
r1 + r2 - distance
Where r1 and r2 are the radii of circle 1 and 2
and distance is the length of the vector p2 - p1.
Will return 0.0 if the circles do not overlap.
"""
delta = s2[ :3 ] - s1[ :3 ]
distance = vector.length( delta )
combined_radii = s1[ 3 ] + s2[ 3 ]
penetration = combined_radii - distance
if penetration <= 0.0:
return 0.0
return penetration
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 test_create_from_bounds(self):
bounds = [[-1.,1.,-1.], [2.,1.,0.]]
result = aambb.create_from_bounds(*bounds)
length = max(vector.length(bounds[0]), vector.length(bounds[1]))
self.assertTrue(np.array_equal(result, [[-length,-length,-length],[length,length,length]]))
self.assertTrue(np.array_equal(aambb.centre_point(result), [0.0,0.0,0.0]))
def test_plate_to_world_to_plate():
vec_world = model.plate_to_world(test_vector.x, test_vector.y,
test_vector.z)
result = model.world_to_plate(vec_world.x, vec_world.y, vec_world.z)
delta = vector.length(result - test_vector)
assert delta < TOLERANCE
def _update_estimated_ball(self, ball: Vector3):
"""
Ray trace the ball position and an edge of the ball back to the camera
origin and use the collision points with the tilted plate to estimate
what a camera might perceive the ball position and size to be.
"""
# contact ray from camera to plate
camera = self._camera_pos()
displacement = camera - self.ball
displacement_radius = camera - (self.ball +
Vector3([self.ball_radius, 0, 0]))
ball_ray = ray.create(camera, displacement)
ball_radius_ray = ray.create(camera, displacement_radius)
surface_plane = self._surface_plane()
contact = Vector3(ray_intersect_plane(ball_ray, surface_plane, False))
radius_contact = Vector3(
ray_intersect_plane(ball_radius_ray, surface_plane, False))
x, y = contact.x, contact.y
r = math.fabs(contact.x - radius_contact.x)
# add the noise in
self.estimated_x = x + MoabModel.random_noise(self.ball_noise)
self.estimated_y = y + MoabModel.random_noise(self.ball_noise)
self.estimated_radius = r + MoabModel.random_noise(self.ball_noise)
# Use n-1 states to calculate an estimated velocity.
self.estimated_vel_x = (self.estimated_x -
self.prev_estimated_x) / self.step_time
self.estimated_vel_y = (self.estimated_y -
self.prev_estimated_y) / self.step_time
# distance to target
self.estimated_distance = MoabModel.distance_to_point(
self.estimated_x, self.estimated_y, self.target_x, self.target_y)
# update the derived states
self.estimated_speed = cast(
float,
vector.length([self.ball_vel.x, self.ball_vel.y, self.ball_vel.z]))
self.estimated_direction = MoabModel.heading_to_point(
self.estimated_x,
self.estimated_y,
self.estimated_vel_x,
self.estimated_vel_y,
self.target_x,
self.target_y,
)
# update for next time
self.prev_estimated_x = self.estimated_x
self.prev_estimated_y = self.estimated_y
# update ball position in plate origin coordinates, and obstacle distance and direction
self.ball_on_plate = self.world_to_plate(self.ball.x, self.ball.y,
self.ball.z)
self.obstacle_distance = self._get_obstacle_distance()
self.obstacle_direction = MoabModel.heading_to_point(
self.ball.x,
self.ball.y,
self.ball_vel.x,
self.ball_vel.y,
self.obstacle_x,
self.obstacle_y,
)
def distance_to(self, other) -> float:
return vector.length(self.vector_to(other))
