Rigid Transformations

polytope/polytope.py

@@ -290,6 +290,32 @@ def intersect(self, other, abs_tol=ABS_TOL):
 
         return reduce(Polytope(iA, ib), abs_tol=abs_tol)
 
+    def translation(self, d):
+        """Translate the Polytope by a given vector.
+
+        Consult L{polytope.polytope._translate} for implementation details.
+
+        @type d: 1d array
+        @param d: The translation vector.
+
+        @rtype: L{Polytope}
+        @return: A translated copy of the Polytope.
+        """
+        newpoly = self.copy()
+        _translate(newpoly, d)
+        return newpoly
+
+    def rotation(self, u, v, theta=None):
+        """Rotate the Polytope. Only simple rotations are implemented at this
+        time.
+
+        @rtype: L{Polytope}
+        @return: A rotated copy of the Polytope.
+        """
+        newpoly = self.copy()
+        _rotate(newpoly, u, v, theta)
+        return newpoly
+
     def copy(self):
         """Return copy of this Polytope.
         """
@@ -442,6 +468,134 @@ def text(self, txt, ax=None, color='black'):
         _plot_text(self, txt, ax, color)
 
 
+def _translate(polyreg, d):
+    """Translate a polyreg by a vector in place. Does not return a copy.
+
+    @type d: 1d array
+    @param d: The translation vector.
+
+    @type polyreg: L{Polytope} or L{Region}
+    @param polyreg: The polytope or region to be translated.
+    """
+
+    if isinstance(polyreg, Polytope):
+        # Translate hyperplanes
+        polyreg.b = polyreg.b + np.dot(polyreg.A, d)
+    else:
+        # Translate subregions
+        for poly in polyreg.list_poly:
+            _translate(poly, d)
+
+    # Translate bbox and cheby
+    if polyreg.bbox is not None:
+        polyreg.bbox = (polyreg.bbox[0] + d,
+                        polyreg.bbox[1] + d)
+    if polyreg._chebXc is not None:
+        polyreg._chebXc = polyreg._chebXc + d
+
+
+def _rotate(polyreg, u=None, v=None, theta=None, R=None):
+    """Rotate this polyreg in place. Return the rotation matrix.
+
+    Simple rotations, by definition, occur only in 2 of the N dimensions; the
+    other N-2 dimensions are invariant with the rotation. Any rotations thus
+    require specification of the plane(s) of rotation. This function has three
+    different ways to specify this plane:
+
+    (1) Providing the indices [0, N) of the orthogonal basis vectors which
+    define the plane of rotation and an angle of rotation (theta) between them.
+    This allows easy construction of the Givens rotation matrix. The right hand
+    rule defines the positive rotation direction.
+
+    (2) Providing two unit vectors with no angle. The two vectors are contained
+    within a plane and the degree of rotation is the angle which moves the
+    first vector, u, into alignment with the second vector, v. NOTE: This
+    method is not implemented at this time.
+
+    (3) Providing an NxN rotation matrix, R. WARNING: No checks are made to
+    determine whether the provided transformation matrix is a valid rotation.
+
+    Further Reading
+    https://en.wikipedia.org/wiki/Plane_of_rotation
+
+    @type polyreg: L{Polytope} or L{Region}
+    @param polyreg: The polytope or region to be rotated.
+    @type u: number, 1d array
+    @param u: The first index or vector describing the plane of rotation.
+    @type v: number, 1d array
+    @param u: The second index or vector describing the plane of rotation.
+    @type theta: number
+    @param theta: The radian angle to rotate the polyreg in the plane defined
+                  by u and v.
+    @type R: 2d array
+    @param R: A predefined rotation matrix.
+
+    @rtype: 2d array
+    @return: The matrix used to rotate the polyreg.
+    """
+    # determine the rotation matrix based on inputs
+    if R is not None:
+        logger.info("rotate via predefined matrix.")
+
+    elif u is not None and v is not None:
+        if theta is not None:
+            logger.info("rotate via indices and angle.")
+            if u == v:
+                raise ValueError("Must provide two unique basis vectors.")
+            R = np.identity(polyreg.dim)
+            c = np.cos(theta)
+            s = np.sin(theta)
+            R[u, u] = c
+            R[v, v] = c
+            R[u, v] = -s
+            R[v, u] = s
+
+        else:  # theta is None
+            logger.info("rotate via 2 vectors.")
+            # TODO: Assert vectors are non-zero and non-parallel aka exterior
+            # product is non-zero; then autocalculate the complex rotation
+            # required to align the first vector with the second.
+            raise NotImplementedError("Rotation via 2 vectors is not currently"
+                                      " available. See source for TODO.")
+
+    else:
+        raise ValueError("R or (u and v) must be defined.")
+
+    if isinstance(polyreg, Polytope):
+        # Ensure that half space is normalized before rotation
+        n, p = _hessian_normal(polyreg.A, polyreg.b)
+
+        # Rotate the hyperplane normals
+        polyreg.A = np.inner(n, R)
+        polyreg.b = p
+    else:
+        # Rotate subregions
+        for poly in polyreg.list_poly:
+            _rotate(poly, None, None, R=R)
+
+    # transform bbox and cheby
+    if polyreg.bbox is not None:
+        polyreg.bbox = (np.inner(polyreg.bbox[0].T, R).T,
+                        np.inner(polyreg.bbox[1].T, R).T)
+    if polyreg._chebXc is not None:
+        polyreg._chebXc = np.inner(polyreg._chebXc, R)
+
+    return R
+
+
+def _hessian_normal(A, b):
+    """Normalizes a half space representation according to hessian normal form.
+    """
+    L2 = np.reshape(np.linalg.norm(A, axis=1), (-1, 1))  # needs to be column
+    if any(L2 == 0):
+        raise ValueError('One of the rows of A is a zero vector.')
+
+    n = A / L2  # hyperplane normals
+    p = b / L2.flatten()  # hyperplane distances from origin
+
+    return n, p
+
+
 class Region(object):
     """Class for lists of convex polytopes
 
@@ -620,6 +774,32 @@ def intersect(self, other, abs_tol=ABS_TOL):
                     P = union(P, isect, check_convex=True)
         return P
 
+    def rotation(self, u, v, theta=None):
+        """Rotate this Region. Only simple rotations are implemented at this
+        time.
+
+        @rtype: L{Region}
+        @return: A translated copy of the Region.
+        """
+        newreg = self.copy()
+        _rotate(newreg, u, v, theta)
+        return newreg
+
+    def translation(self, d):
+        """Translate this Region by a given vector.
+
+        Consult L{polytope.polytope._translate} for implementation details.
+
+        @type d: 1d array
+        @param d: The translation vector.
+
+        @rtype: L{Region}
+        @return: A translated copy of the Region.
+        """
+        newreg = self.copy()
+        _translate(newreg, d)
+        return newreg
+
     def __copy__(self):
         """Return copy of this Region."""
         return Region(list_poly=self.list_poly[:],
@@ -2227,7 +2407,7 @@ def lpsolve(c, G, h):
             result['status'] = 3
         else:
             result['status'] = 4
-        result['x'] = sol['x']
+        result['x'] = np.squeeze(sol['x'])
         result['fun'] = sol['primal objective']
     elif lp_solver == 'scipy':
         sol = optimize.linprog(

tests/polytope_test.py

@@ -3,6 +3,7 @@
 import logging
 
 import numpy as np
+from numpy.testing import assert_allclose
 import polytope as pc
 
 
@@ -11,12 +12,30 @@
 log.addHandler(logging.StreamHandler())
 
 
-# unit square
+# unit square in first quadrant
 Ab = np.array([[0.0, 1.0, 1.0],
                [0.0, -1.0, 0.0],
                [1.0, 0.0, 1.0],
                [-1.0, 0.0, 0.0]])
 
+# unit square in second quadrant
+Ab2 = np.array([[-1.0, 0.0, 1.0],
+               [1.0, 0.0, 0.0],
+               [0.0, 1.0, 1.0],
+               [0.0, -1.0, 0.0]])
+
+# unit square in third quadrant
+Ab3 = np.array([[0.0, 1.0, 0.0],
+               [0.0, -1.0, 1.0],
+               [1.0, 0.0, 0.0],
+               [-1.0, 0.0, 1.0]])
+
+# unit square in fourth quadrant
+Ab4 = np.array([[0.0, 1.0, 0.0],
+               [0.0, -1.0, 1.0],
+               [1.0, 0.0, 1.0],
+               [-1.0, 0.0, 0.0]])
+
 A = Ab[:, 0:2]
 b = Ab[:, 2]
 
@@ -48,5 +67,84 @@ def comparison_test():
         np.array([1, 0, 0])))
 
 
+def region_rotation_test():
+    p = pc.Region([pc.Polytope(A, b)])
+    p1 = pc.Region([pc.Polytope(A, b)])
+    p2 = pc.Region([pc.Polytope(Ab2[:, 0:2], Ab2[:, 2])])
+    p3 = pc.Region([pc.Polytope(Ab3[:, 0:2], Ab3[:, 2])])
+    p4 = pc.Region([pc.Polytope(Ab4[:, 0:2], Ab4[:, 2])])
+
+    p = p.rotation(0, 1, np.pi/2)
+    print(p.bounding_box)
+    assert(p == p2)
+    assert(not p == p3)
+    assert(not p == p4)
+    assert(not p == p1)
+    assert_allclose(p.chebXc, [-0.5, 0.5])
+
+    p = p.rotation(0, 1, np.pi/2)
+    assert(p == p3)
+    assert_allclose(p.chebXc, [-0.5, -0.5])
+
+    p = p.rotation(0, 1, np.pi/2)
+    assert(p == p4)
+    assert_allclose(p.chebXc, [0.5, -0.5])
+
+    p = p.rotation(0, 1, np.pi/2)
+    assert(p == p1)
+    assert_allclose(p.chebXc, [0.5, 0.5])
+
+
+def polytope_rotation_test():
+    p = pc.Polytope(A, b)
+    p1 = pc.Polytope(A, b)
+    p2 = pc.Polytope(Ab2[:, 0:2], Ab2[:, 2])
+    p3 = pc.Polytope(Ab3[:, 0:2], Ab3[:, 2])
+    p4 = pc.Polytope(Ab4[:, 0:2], Ab4[:, 2])
+
+    p = p.rotation(0, 1, np.pi/2)
+    print(p.bounding_box)
+    assert(p == p2)
+    assert(not p == p3)
+    assert(not p == p4)
+    assert(not p == p1)
+    assert_allclose(p.chebXc, [-0.5, 0.5])
+
+    p = p.rotation(0, 1, np.pi/2)
+    assert(p == p3)
+    assert_allclose(p.chebXc, [-0.5, -0.5])
+
+    p = p.rotation(0, 1, np.pi/2)
+    assert(p == p4)
+    assert_allclose(p.chebXc, [0.5, -0.5])
+
+    p = p.rotation(0, 1, np.pi/2)
+    assert(p == p1)
+    assert_allclose(p.chebXc, [0.5, 0.5])
+
+
+def region_translation_test():
+    p = pc.Region([pc.Polytope(A, b)])
+    p1 = pc.Region([pc.Polytope(A, b)])
+    p2 = pc.Region([pc.Polytope(Ab2[:, 0:2], Ab2[:, 2])])
+
+    p = p.translation([-1, 0])
+    assert(p == p2)
+    assert(not p == p1)
+    p = p.translation([1, 0])
+    assert(p == p1)
+
+
+def polytope_translation_test():
+    p = pc.Polytope(A, b)
+    p1 = pc.Polytope(A, b)
+    p2 = pc.Polytope(Ab2[:, 0:2], Ab2[:, 2])
+
+    p = p.translation([-1, 0])
+    assert(p == p2)
+    assert(not p == p1)
+    p = p.translation([1, 0])
+    assert(p == p1)
+
 if __name__ == '__main__':
     comparison_test()