def coll_box2box(box1, box2, **kwargs):
atol = cmp_atol
verbose = False
# handle all arguments
for key, value in kwargs.items():
if key == 'verbose':
verbose = value
elif key == 'atol':
atol = value
if verbose:
debug('calculating collision between two boxes')
debug(' atol=%g' % atol)
result = CollisionResult()
collision = box_inside_box(box1, box2, atol, verbose)
if not collision:
collision = box_inside_box(box2, box1, atol, verbose)
result['collision'] = collision
if verbose:
debug('collision:', result['collision'])
debug('Done.')
return result
def __init__(self, x1: Vector, x2: Vector, verbose: bool=False):
BasicObject.__init__(self, verbose=verbose)
self._x1 = x1
self._x2 = x2
self._center = (x2 - x1) / 2.
self._six, self._corners = self.get_box_planes_and_corners(self._x1,
self._x2)
self._volume = self.get_volume(self._center)
if self._verbose:
debug('reference volume:', self._volume)
def box_inside_box(box1, box2, atol, verbose):
# the algorithm based on the assumption that we can check if
# all corner points of box1 are not in box2 the box1 has no
# overlap with box2 (true, but one has to check the other way
# as well)!
for i in box1._corners:
i = box1.calculate_position(i)
vol = box2.get_volume(center=i)
if verbose:
debug(' c=%s vol=%g vol_ref=%g' % (i, vol, box2._volume))
if np.isclose(vol, box2._volume, rtol=atol, atol=atol):
return True
return False
def coll_plane2plane(plane1, plane2, **kwargs):
atol = cmp_atol
verbose = False
# handle all arguments
for key, value in kwargs.items():
if key == 'verbose':
verbose = value
elif key == 'atol':
atol = value
if verbose:
debug('calculating collision between two planes')
debug(' atol=%g' % atol)
result = CollisionResult()
cross = np.cross(plane1.norm_vector, plane2.norm_vector)
if verbose:
debug(' cross_check_vector=%s' % cross)
# if the cross vector has zero length, that there both
# norm vectors are parallel
if not np.isclose(nl.norm(cross), 0., atol=atol):
result['collision'] = True
result['type'] = 'crossing'
p, v = intersection_of_planes(plane1.norm_vector, plane1.distance,
plane2.norm_vector, plane2.distance)
result['intersection'] = 'line'
result['intersection_params'] = (p, v)
else:
if np.isclose(plane1.distance, plane2.distance, atol=atol):
result['collision'] = True
result['type'] = 'identical'
result['intersection'] = 'plane'
result['intersection_params'] = (plane1.distance, plane2.distance)
if verbose:
debug('collision:', result['collision'])
debug('Done.')
return result
def intersection_line_plane(edge, norm_vector, distance, atol=cmp_atol):
"""
intersection_line_plane
calculates the intersection point between a ray and a plane
"""
edge_direction = edge[0] - edge[1]
plane_point = point_of_plane(norm_vector, distance)
debug(' plane_point=%s' % plane_point)
ndotu = norm_vector.dot(edge_direction)
if abs(ndotu) < atol:
debug(' no intersection or line is within plane')
return None
w = edge[0] - plane_point
si = -norm_vector.dot(w) / ndotu
Psi = w + si * edge_direction + plane_point
print(Psi)
debug(' intersection point=%s' % Psi)
return Psi
def coll_sphere2sphere(sph1, sph2, **kwargs):
atol = cmp_atol
verbose = False
# handle all arguments
for key, value in kwargs.items():
if key == 'verbose':
verbose = value
elif key == 'atol':
atol = value
if verbose:
debug('calculating collision between two spheres')
result = CollisionResult()
# calculate the distance of the two spheres
distance = np.sqrt(np.sum((sph1.position - sph2.position)**2))
result['distance'] = distance
if verbose:
debug(' absolute distance is: %g' % distance)
distance -= sph1.radius + sph2.radius
result['outerdistance'] = distance
if verbose:
debug(' outer distance is: %g' % distance)
# every outside atol is clear
if distance < atol:
result['collision'] = True
else:
result['collision'] = np.isclose(distance, 0., atol=atol)
if verbose:
debug(' collision:', result['collision'])
debug('Done.')
return result
def coll_box2plane(box, plane, **kwargs):
atol = cmp_atol
verbose = False
# handle all arguments
for key, value in kwargs.items():
if key == 'verbose':
verbose = value
elif key == 'atol':
atol = value
if verbose:
debug('calculating collision between a box and a plane')
debug(' atol=%g' % atol)
result = CollisionResult()
distances = np.zeros(len(box._corners))
c = box.corners
for i in range(len(distances)):
distances[i] = distance_to_plane(c[i], plane.norm_vector,
plane.distance)
if verbose:
debug(' distances=%s' % distances)
nnp = nnm = nnn = 0
for i in distances:
if np.isclose(i, 0., atol=atol):
nnn += 1
elif i > atol:
nnp += 1
else:
nnm += 1
if verbose:
debug(' (nnp, nnm, nnn)=(%i, %i, %i)' % (nnp, nnm, nnn))
if nnn > 0:
# one or more points directly on the plane
result['collision'] = True
else:
if (nnp != 8) or (nnm != 8):
result['collision'] = True
if result['collision']:
# try to get the intersection figure
edges = box.edges
points = []
for e in edges:
# test a single edge ...
p = intersection_line_plane(e, plane.norm_vector, plane.distance)
if p is not None:
vol = box.get_volume(center=p)
if verbose:
debug(' c=%s vol=%g vol_ref=%g' % (i, vol, box._volume))
if np.isclose(vol, box._volume, rtol=atol, atol=atol):
points.append(p)
points = np.array(points)
if verbose:
debug(' points (unsorted) in the plane = %s' % points)
if len(points) > 3:
points = polygon_sort(points, plane.norm_vector, atol=atol)
if verbose:
debug(' points (sorted) in the plane = %s' % points)
if verbose:
debug('collision:', result['collision'])
debug('Done.')
return result
def coll_sphere2plane(sphere, plane, **kwargs):
atol = cmp_atol
verbose = False
# handle all arguments
for key, value in kwargs.items():
if key == 'verbose':
verbose = value
elif key == 'atol':
atol = value
if verbose:
debug('calculating collision between a sphere and a plane')
debug(' atol=%g' % atol)
result = CollisionResult()
v = sphere.position
# the vector s is the vector which is perpendicular to
# the plane pointing direct to the center of the sphere
# s = projection_vector(v, plane.norm_vector)
# distance = nl.norm(s) + plane.distance
distance = np.abs(distance_to_plane(v, plane.norm_vector, plane.distance))
result['distance'] = distance
if verbose:
debug(' absolute distance is: %g' % distance)
distance -= sphere.radius
result['outerdistance'] = distance
if verbose:
debug(' outer distance is: %g' % distance)
# every outside atol is clear
result['collision'] = np.isclose(distance, 0., atol=atol)
if verbose:
debug('collision:', result['collision'])
debug('Done.')
return result