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 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)