diff --git a/polytope/polytope.py b/polytope/polytope.py index 5d0deac..11d0759 100755 --- a/polytope/polytope.py +++ b/polytope/polytope.py @@ -290,6 +290,36 @@ def intersect(self, other, abs_tol=ABS_TOL): return reduce(Polytope(iA, ib), abs_tol=abs_tol) + def translation(self, d): + """Returns a copy of C{self} translated by the vector C{d}. + + Consult L{polytope.polytope._translate} for implementation details. + + @type d: 1d array + """ + newpoly = self.copy() + _translate(newpoly, d) + return newpoly + + def rotation(self, i=None, j=None, theta=None): + """Returns a rotated copy of C{self}. + + Describe the plane of rotation and the angle of rotation (in radians) + with i, j, and theta. + + i and j are the indices 0..N-1 of two of the identity basis + vectors, and theta is the angle of rotation. + + Consult L{polytope.polytope._rotate} for more detail. + + @type i: int + @type j: int + @type theta: number + """ + newpoly = self.copy() + _rotate(newpoly, i=i, j=j, theta=theta) + return newpoly + def copy(self): """Return copy of this Polytope. """ @@ -442,6 +472,137 @@ def text(self, txt, ax=None, color='black'): _plot_text(self, txt, ax, color) +def _translate(polyreg, d): + """Translate C{polyreg} by the vector C{d}. Modifies C{polyreg} in-place. + + @type d: 1d array + """ + + 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, i=None, j=None, u=None, v=None, theta=None, R=None): + """Rotate C{polyreg} in-place. Return the rotation matrix. + + There are two types of rotation: simple and compound. For simple rotations, + by definition, all motion can be projected as circles in a single plane; + the other N-2 dimensions are invariant. Therefore any simple rotation can + be parameterized by its plane of rotation. Compound rotations are the + combination of multiple simple rotations; they have more than one plane of + rotation. For N > 3 dimensions, a compound rotation may be necessary to map + one orientation to another (Euler's rotation theorem no longer applies). + + Use one of the following three methods to specify rotation. The first two + can only express simple rotation, but simple rotations may be applied in a + sequence to acheive a compound rotation. + + (1) Provide the indices 0..N-1 of the identity basis vectors, i and j, + which define the plane of rotation and a radian angle of rotation, theta, + between them. This method contructs the Givens rotation matrix. The right + hand rule defines the positive rotation direction. + + (2) Provide two vectors, the two vectors define the plane of rotation + and angle of rotation is TWICE the angle from the first vector, u, to + the second vector, v. NOTE: This method is not implemented at this time. + + (3) Provide 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 + + @param polyreg: The polytope or region to be rotated. + @type polyreg: L{Polytope} or L{Region} + @param i: The first index describing the plane of rotation. + @type i: int + @param j: The second index describing the plane of rotation. + @type j: int + @param u: The first vector describing the plane of rotation. + @type u: 1d array + @param u: The second vector describing the plane of rotation. + @type v: 1d array. + @param theta: The radian angle to rotate the polyreg in the plane defined + by i and j. + @type theta: number + @param R: A predefined rotation matrix. + @type R: 2d array + """ + # determine the rotation matrix based on inputs + if R is not None: + logger.info("rotate via predefined matrix.") + + elif i is not None and j is not None and theta is not None: + logger.info("rotate via indices and angle.") + if i == j: + raise ValueError("Must provide two unique basis vectors.") + R = np.identity(polyreg.dim) + c = np.cos(theta) + s = np.sin(theta) + R[i, i] = c + R[j, j] = c + R[i, j] = -s + R[j, i] = s + + elif u is not None and v is not None: # 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 (i and j and theta) 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 +781,36 @@ def intersect(self, other, abs_tol=ABS_TOL): P = union(P, isect, check_convex=True) return P + def rotation(self, i=None, j=None, theta=None): + """Returns a rotated copy of C{self}. + + Describe the plane of rotation and the angle of rotation (in radians) + with i, j, and theta. + + i and j are the indices 0..N-1 of two of the identity basis + vectors, and theta is the angle of rotation. + + Consult L{polytope.polytope._rotate} for more detail. + + @type i: int + @type j: int + @type theta: number + """ + newreg = self.copy() + _rotate(newreg, i=i, j=j, theta=theta) + return newreg + + def translation(self, d): + """Returns a copy of C{self} translated by the vector C{d}. + + Consult L{polytope.polytope._translate} for implementation details. + + @type d: 1d array + """ + 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 +2418,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( diff --git a/tests/polytope_test.py b/tests/polytope_test.py index d5d8885..337f118 100644 --- a/tests/polytope_test.py +++ b/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()