def _sphere_sphere_collision(p_g0, radius0, p_g1, radius1):
""" Get information on sphere/sphere collision.
:param p_g0: position of the center of sphere0 relative to the ground
:type p_g0: (3,)-array
:param float radius0: radius of sphere0
:param p_g1: position of the center of sphere1 relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of sphere1
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the two spheres
* *H_gc0*: the pose from the ground to the closest contact point on sphere 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/sphere_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, zeros
>>> p_g0 = zeros((3))
>>> p_g1 = array( (2., 2., 1.) )
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.1, p_g1, 1.2)
>>> print(sdist)
0.7
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 0.73333333]
[-0.70710678 0.23570226 0.66666667 0.73333333]
[ 0. -0.94280904 0.33333333 0.36666667]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 1.2 ]
[-0.70710678 0.23570226 0.66666667 1.2 ]
[ 0. -0.94280904 0.33333333 0.6 ]
[ 0. 0. 0. 1. ]]
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.5, p_g1, 1.6)
>>> print(sdist)
-0.1
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 1. ]
[-0.70710678 0.23570226 0.66666667 1. ]
[ 0. -0.94280904 0.33333333 0.5 ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 0.93333333]
[-0.70710678 0.23570226 0.66666667 0.93333333]
[ 0. -0.94280904 0.33333333 0.46666667]
[ 0. 0. 0. 1. ]]
"""
vec = p_g1 - p_g0
sdist = norm(vec) - radius0 - radius1
normal = vec / norm(vec)
H_gc0 = Hg.zaligned(normal)
z = H_gc0[0:3, 2]
H_gc0[0:3, 3] = p_g0 + radius0 * z
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] += sdist * z
return (sdist, H_gc0, H_gc1)
def _sphere_sphere_collision(p_g0, radius0, p_g1, radius1):
""" Get information on sphere/sphere collision.
:param p_g0: position of the center of sphere0 relative to the ground
:type p_g0: (3,)-array
:param float radius0: radius of sphere0
:param p_g1: position of the center of sphere1 relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of sphere1
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the two spheres
* *H_gc0*: the pose from the ground to the closest contact point on sphere 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/sphere_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, zeros
>>> p_g0 = zeros((3))
>>> p_g1 = array( (2., 2., 1.) )
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.1, p_g1, 1.2)
>>> print(sdist)
0.7
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 0.73333333]
[-0.70710678 0.23570226 0.66666667 0.73333333]
[ 0. -0.94280904 0.33333333 0.36666667]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 1.2 ]
[-0.70710678 0.23570226 0.66666667 1.2 ]
[ 0. -0.94280904 0.33333333 0.6 ]
[ 0. 0. 0. 1. ]]
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.5, p_g1, 1.6)
>>> print(sdist)
-0.1
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 1. ]
[-0.70710678 0.23570226 0.66666667 1. ]
[ 0. -0.94280904 0.33333333 0.5 ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 0.93333333]
[-0.70710678 0.23570226 0.66666667 0.93333333]
[ 0. -0.94280904 0.33333333 0.46666667]
[ 0. 0. 0. 1. ]]
"""
vec = p_g1 - p_g0
sdist = norm(vec) - radius0 - radius1
normal = vec/norm(vec)
H_gc0 = Hg.zaligned(normal)
z = H_gc0[0:3, 2]
H_gc0[0:3, 3] = p_g0 + radius0*z
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] += sdist*z
return (sdist, H_gc0, H_gc1)
def create_plane(self, coeffs, color, name=None):
H = zaligned(coeffs[0:3])
H[0:3, 3] = coeffs[3] * coeffs[0:3]
node = self.create_transform(H, is_constant=True, name=name)
scale = SubElement(node, QN('scale'))
scale.text = "{0} {1} 0.".format(*self._options["plane half extents"])
elem = SubElement(node, QN("instance_geometry"),
{"url": self._shapes+"#plane"})
self._add_color(elem, color)
return node
def create_plane(self, coeffs, color, name=None):
H = zaligned(coeffs[0:3])
H[0:3, 3] = coeffs[3] * coeffs[0:3]
node = self.create_transform(H, is_constant=True, name=name)
scale = SubElement(node, QN('scale'))
scale.text = "{0} {1} 0.".format(*self._options["plane half extents"])
elem = SubElement(node, QN("instance_geometry"),
{"url": self._shapes + "#plane"})
self._add_color(elem, color)
return node
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
""" Get information on plane/sphere collision.
:param H_g0: pose of the center of the plane relative to the ground
:type H_g0: (4,4)-array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,)-array
:param p_g1: position of center of the sphere relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the plane and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on plane 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/plane_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
""" Get information on plane/sphere collision.
:param H_g0: pose of the center of the plane relative to the ground
:type H_g0: (4,4)-array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,)-array
:param p_g1: position of center of the sphere relative to the ground
:type p_g1: (3,)-array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the plane and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on plane 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/plane_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def create_plane(self, coeffs, color, name=""):
if self.dae.geometries.get("plane_geometry-mesh") is None:
self.dae.geometries.append(self.shapes_dae.geometries.get("plane_geometry-mesh"))
plane_geom = self.dae.geometries.get("plane_geometry-mesh")
H = zaligned(coeffs[0:3])
H[0:3, 3] = coeffs[3] * coeffs[0:3]
plane_he = self._options["plane half extents"]
node = self._add_new_geometry(name, color, plane_geom)
node.transforms.append(collada.scene.MatrixTransform(H.flatten()))
node.transforms.append(collada.scene.ScaleTransform(plane_he[0], plane_he[1], 0.0))
return node
def create_line(self, start, end, color, name=None):
vector = (array(end) - array(start))
vector_norm = linalg.norm(vector)
assert vector_norm > 0.
n_vector = vector / vector_norm
H = zaligned(n_vector)
H[0:3, 3] = start
node = self.create_transform(H, is_constant=True, name=name)
scale = SubElement(node, QN('scale'))
scale.text = "0. 0. {0}".format(vector_norm)
elem = SubElement(node, QN("instance_geometry"),
{"url": self._shapes + "#line"})
self._add_color(elem, color)
return node
def create_line(self, start, end, color, name=None):
vector = (array(end) - array(start))
vector_norm = linalg.norm(vector)
assert vector_norm > 0.
n_vector = vector/vector_norm
H = zaligned(n_vector)
H[0:3, 3] = start
node = self.create_transform(H, is_constant=True, name=name)
scale = SubElement(node, QN('scale'))
scale.text = "0. 0. {0}".format(vector_norm)
elem = SubElement(node, QN("instance_geometry"),
{"url": self._shapes+"#line"})
self._add_color(elem, color)
return node
def create_plane(self, coeffs, color, name=""):
if self.dae.geometries.get("plane_geometry-mesh") is None:
self.dae.geometries.append(
self.shapes_dae.geometries.get("plane_geometry-mesh"))
plane_geom = self.dae.geometries.get("plane_geometry-mesh")
H = zaligned(coeffs[0:3])
H[0:3, 3] = coeffs[3] * coeffs[0:3]
plane_he = self._options["plane half extents"]
node = self._add_new_geometry(name, color, plane_geom)
node.transforms.append(collada.scene.MatrixTransform(H.flatten()))
node.transforms.append(
collada.scene.ScaleTransform(plane_he[0], plane_he[1], 0.))
return node
def _sphere_sphere_collision(p_g0, radius0, p_g1, radius1):
"""
.. image:: img/sphere_sphere_collision.png
**Tests:**
>>> from numpy import array, zeros
>>> p_g0 = zeros((3))
>>> p_g1 = array( (2., 2., 1.) )
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.1, p_g1, 1.2)
>>> print(sdist)
0.7
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 0.73333333]
[-0.70710678 0.23570226 0.66666667 0.73333333]
[ 0. -0.94280904 0.33333333 0.36666667]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 1.2 ]
[-0.70710678 0.23570226 0.66666667 1.2 ]
[ 0. -0.94280904 0.33333333 0.6 ]
[ 0. 0. 0. 1. ]]
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.5, p_g1, 1.6)
>>> print(sdist)
-0.1
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 1. ]
[-0.70710678 0.23570226 0.66666667 1. ]
[ 0. -0.94280904 0.33333333 0.5 ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 0.93333333]
[-0.70710678 0.23570226 0.66666667 0.93333333]
[ 0. -0.94280904 0.33333333 0.46666667]
[ 0. 0. 0. 1. ]]
"""
vec = p_g1 - p_g0
sdist = norm(vec) - radius0 - radius1
normal = vec/norm(vec)
H_gc0 = Hg.zaligned(normal)
z = H_gc0[0:3, 2]
H_gc0[0:3, 3] = p_g0 + radius0*z
H_gc1 = H_gc0.copy()
H_gc1[0:3,3] += sdist*z
return (sdist, H_gc0, H_gc1)
def _sphere_sphere_collision(p_g0, radius0, p_g1, radius1):
"""
.. image:: img/sphere_sphere_collision.png
**Tests:**
>>> from numpy import array, zeros
>>> p_g0 = zeros((3))
>>> p_g1 = array( (2., 2., 1.) )
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.1, p_g1, 1.2)
>>> print(sdist)
0.7
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 0.73333333]
[-0.70710678 0.23570226 0.66666667 0.73333333]
[ 0. -0.94280904 0.33333333 0.36666667]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 1.2 ]
[-0.70710678 0.23570226 0.66666667 1.2 ]
[ 0. -0.94280904 0.33333333 0.6 ]
[ 0. 0. 0. 1. ]]
>>> (sdist, H_gc0, H_gc1) = _sphere_sphere_collision(p_g0, 1.5, p_g1, 1.6)
>>> print(sdist)
-0.1
>>> print(H_gc0)
[[ 0.70710678 0.23570226 0.66666667 1. ]
[-0.70710678 0.23570226 0.66666667 1. ]
[ 0. -0.94280904 0.33333333 0.5 ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[ 0.70710678 0.23570226 0.66666667 0.93333333]
[-0.70710678 0.23570226 0.66666667 0.93333333]
[ 0. -0.94280904 0.33333333 0.46666667]
[ 0. 0. 0. 1. ]]
"""
vec = p_g1 - p_g0
sdist = norm(vec) - radius0 - radius1
normal = vec / norm(vec)
H_gc0 = Hg.zaligned(normal)
z = H_gc0[0:3, 2]
H_gc0[0:3, 3] = p_g0 + radius0 * z
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] += sdist * z
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
"""
:param H_g0: pose of the plane `H_{g0}`
:type H_g0: (4,4) array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3,3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3,3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def _plane_sphere_collision(H_g0, coeffs0, p_g1, radius1):
"""
:param H_g0: pose of the plane `H_{g0}`
:type H_g0: (4,4) array
:param coeffs0: coefficients from the plane equation
:type coeffs0: (4,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> coeffs0 = array([0., 1., 0., -5.])
>>> r1 = 0.1
>>> p_g1 = array([2., 4., 3.])
>>> (sdist, H_gc0, H_gc1) = _plane_sphere_collision(H_g0, coeffs0, p_g1, r1)
>>> print sdist
8.9
>>> print H_gc0
[[ 0. 1. 0. 2.]
[-0. 0. 1. -5.]
[ 1. -0. 0. 3.]
[ 0. 0. 0. 1.]]
>>> print H_gc1
[[ 0. 1. 0. 2. ]
[-0. 0. 1. 3.9]
[ 1. -0. 0. 3. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
assert norm(coeffs0[0:3]) == 1.
assert radius1 >= 0.
normal = coeffs0[0:3] # the plane normal
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
csdist = dot(normal, p_01) - coeffs0[3] # signed distance from the center
sdist = csdist - radius1
H_gc0 = Hg.zaligned(normal)
H_gc0[0:3, 3] = p_01 - csdist * normal
H_gc1 = H_gc0.copy()
H_gc1[0:3, 3] = p_01 - sign(sdist) * radius1 * normal
return (sdist, H_gc0, H_gc1)
def create_line(self, start, end, color, name=""):
if self.dae.geometries.get("line_geometry-mesh") is None:
self.dae.geometries.append(self.shapes_dae.geometries.get("line_geometry-mesh"))
line_geom = self.dae.geometries.get("line_geometry-mesh")
node = _get_void_node()
vector = (array(end) - array(start))
vector_norm = linalg.norm(vector)
assert vector_norm > 0.
n_vector = vector/vector_norm
H = zaligned(n_vector)
H[0:3, 3] = start
node = self._add_new_geometry(name, color, line_geom)
node.transforms.append(collada.scene.ScaleTransform(vector_norm, vector_norm, vector_norm))
node.transforms.append(collada.scene.MatrixTransform(H.flatten()))
self._add_osg_description(node, "link")
return node
def create_line(self, start, end, color, name=""):
if self.dae.geometries.get("line_geometry-mesh") is None:
self.dae.geometries.append(
self.shapes_dae.geometries.get("line_geometry-mesh"))
line_geom = self.dae.geometries.get("line_geometry-mesh")
node = _get_void_node()
vector = (array(end) - array(start))
vector_norm = linalg.norm(vector)
assert vector_norm > 0.
n_vector = vector / vector_norm
H = zaligned(n_vector)
H[0:3, 3] = start
node = self._add_new_geometry(name, color, line_geom)
node.transforms.append(
collada.scene.ScaleTransform(vector_norm, vector_norm,
vector_norm))
node.transforms.append(collada.scene.MatrixTransform(H.flatten()))
self._add_osg_description(node, "link")
return node
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
""" Get information on box/sphere collision.
:param H_g0: pose of the center of the box relative to the ground
:type H_g0: (4,4)-array
:param half_extents0: half lengths of the box
:type half_extents0: (3,)-array
:param p_g1: position of the center of the sphere relative to the ground
:type p_g1: (3,) array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the box and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on box 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/box_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
near_face = zeros(6)
near_face[0:3] = half_extents0 - p_01
near_face[3:6] = half_extents0 + p_01
i = argmin(near_face)
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i-3] = -half_extents0[i-3]
normal[i-3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1)-radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in arange(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec/norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1*normal
return (sdist, H_gc0, H_gc1)
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
"""
:param H_g0: pose of the box `H_{g0}`
:type H_g0: (4,4) array
:param half_extents0: half lengths of the box
:type half_extents0: (3,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
.. image:: img/box_sphere_collision.png
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
i = argmin(hstack((half_extents0 - p_01, half_extents0 + p_01)))
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i-3] = -half_extents0[i-3]
normal[i-3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1)-radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in range(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec/norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1*normal
return (sdist, H_gc0, H_gc1)
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
""" Get information on box/sphere collision.
:param H_g0: pose of the center of the box relative to the ground
:type H_g0: (4,4)-array
:param half_extents0: half lengths of the box
:type half_extents0: (3,)-array
:param p_g1: position of the center of the sphere relative to the ground
:type p_g1: (3,) array
:param float radius1: radius of the sphere
:return: a tuple (*sdist*, *H_gc0*, *H_gc1*) with:
* *sdist*: the minimal distance between the box and the sphere
* *H_gc0*: the pose from the ground to the closest contact point on box 0 (normal along z)
* *H_gc1*: the pose from the ground to the closest contact point on sphere 1 (normal along z)
.. image:: img/box_sphere_collision.svg
:width: 300px
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1)=_box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
near_face = zeros(6)
near_face[0:3] = half_extents0 - p_01
near_face[3:6] = half_extents0 + p_01
i = argmin(near_face)
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i - 3] = -half_extents0[i - 3]
normal[i - 3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1) - radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in arange(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec / norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1 * normal
return (sdist, H_gc0, H_gc1)
def _box_sphere_collision(H_g0, half_extents0, p_g1, radius1):
"""
:param H_g0: pose of the box `H_{g0}`
:type H_g0: (4,4) array
:param half_extents0: half lengths of the box
:type half_extents0: (3,) array
:param p_g1: center of the sphere
:type p_g1: (3,) array
:param radius1: radius of the sphere
:type radius1: float
.. image:: img/box_sphere_collision.png
**Tests:**
>>> from numpy import array, eye
>>> H_g0 = eye(4)
>>> lengths0 = array([1., 2., 3.])
>>> r1 = 0.1
>>> p_g1 = array([0., 3., 1.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
1.9
>>> print(H_gc0)
[[ 0. 1. 0. 0.]
[-0. 0. 1. 1.]
[ 1. -0. 0. 1.]
[ 0. 0. 0. 1.]]
>>> print(H_gc1)
[[ 0. 1. 0. 0. ]
[-0. 0. 1. 2.9]
[ 1. -0. 0. 1. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.55, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.05
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.45]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> p_g1 = array([0.45, 0., 0.])
>>> (sdist, H_gc0, H_gc1) = _box_sphere_collision(H_g0, lengths0/2, p_g1, r1)
>>> print(sdist)
-0.15
>>> print(H_gc0)
[[-0. 0. 1. 0.5]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
>>> print(H_gc1)
[[-0. 0. 1. 0.35]
[ 0. -1. 0. 0. ]
[ 1. 0. 0. 0. ]
[ 0. 0. 0. 1. ]]
"""
assert Hg.ishomogeneousmatrix(H_g0)
p_01 = Hg.pdot(Hg.inv(H_g0), p_g1)
if (abs(p_01) <= half_extents0).all():
# p_01 is inside the box, we need to find the nearest face
i = argmin(hstack((half_extents0 - p_01, half_extents0 + p_01)))
f_0 = p_01.copy()
normal = zeros(3)
if i < 3:
f_0[i] = half_extents0[i]
normal[i] = 1
else:
f_0[i - 3] = -half_extents0[i - 3]
normal[i - 3] = -1 #TODO check this line is correct
f_g = Hg.pdot(H_g0, f_0)
sdist = -norm(f_g - p_g1) - radius1
else:
# find the point x inside the box that is the nearest to
# the sphere center:
f_0 = zeros(3)
for i in range(3):
f_0[i] = max(min(half_extents0[i], p_01[i]), -half_extents0[i])
f_g = Hg.pdot(H_g0, f_0)
vec = p_g1 - f_g
normal = vec / norm(vec)
sdist = norm(vec) - radius1
H_gc0 = Hg.zaligned(normal)
H_gc1 = H_gc0.copy()
H_gc0[0:3, 3] = f_g
H_gc1[0:3, 3] = p_g1 - radius1 * normal
return (sdist, H_gc0, H_gc1)
