/** * Returns all of the intersections when the convexPolygon is snapped onto this PlanarRegion with the snappingTransform. * @param convexPolygon2d Polygon to snap. * @param snappingTransform RigidBodyTransform that snaps the polygon onto this region. Must have same surface normal as this region. * @param intersectionsToPack ArrayList of ConvexPolygon2d to pack with the intersections. * @return intersectionArea Total area of intersections */ public double getPolygonIntersectionAreaWhenSnapped(ConvexPolygon2d convexPolygon2d, RigidBodyTransform snappingTransform) { return getPolygonIntersectionAreaWhenSnapped(convexPolygon2d, snappingTransform, null); }
/** * Returns all of the intersections when the convexPolygon is snapped onto this PlanarRegion with * the snappingTransform. * * @param convexPolygon2d Polygon to snap. * @param snappingTransform RigidBodyTransform that snaps the polygon onto this region. Must have * same surface normal as this region. * @param intersectionsToPack ArrayList of ConvexPolygon2d to pack with the intersections. * @return intersectionArea Total area of intersections */ public double getPolygonIntersectionAreaWhenSnapped(ConvexPolygon2D convexPolygon2d, RigidBodyTransform snappingTransform) { return getPolygonIntersectionAreaWhenSnapped(convexPolygon2d, snappingTransform, null); }
private boolean checkIfEnoughArea(BipedalFootstepPlannerNode nodeToExpand, PlanarRegion planarRegion) { RigidBodyTransform nodeToExpandTransform = new RigidBodyTransform(); nodeToExpand.getSoleTransform(nodeToExpandTransform); ConvexPolygon2d snappedPolygon = controllerPolygonsInSoleFrame.get(nodeToExpand.getRobotSide()); snappedPolygon.update(); footArea.set(snappedPolygon.getArea()); ConvexPolygon2d footholdPolygon = new ConvexPolygon2d(); totalArea.set(planarRegion.getPolygonIntersectionAreaWhenSnapped(snappedPolygon, nodeToExpandTransform, footholdPolygon)); nodeToExpand.setPercentageOfFoothold(totalArea.getDoubleValue() / footArea.getDoubleValue()); if (nodeToExpand.isPartialFoothold()) { nodeToExpand.setPartialFootholdPolygon(footholdPolygon); } if (totalArea.getDoubleValue() < parameters.getMinimumFootholdPercent() * footArea.getDoubleValue()) { notifyListenerNodeUnderConsiderationWasRejected(nodeToExpand, BipedalFootstepPlannerNodeRejectionReason.NOT_ENOUGH_AREA); return false; } return true; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetPolygonIntersectionsWhenSnapped() { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationEulerAndZeroTranslation(0.1, 0.2, 0.3); transform.setTranslation(1.2, 3.4, 5.6); ConvexPolygon2D convexPolygon = new ConvexPolygon2D(); convexPolygon.addVertex(0.2, 0.2); convexPolygon.addVertex(0.2, -0.2); convexPolygon.addVertex(-0.2, -0.2); convexPolygon.addVertex(-0.2, 0.2); convexPolygon.update(); PlanarRegion planarRegion = new PlanarRegion(transform, convexPolygon); ConvexPolygon2D polygonToSnap = new ConvexPolygon2D(); polygonToSnap.addVertex(0.1, 0.1); polygonToSnap.addVertex(0.1, -0.1); polygonToSnap.addVertex(-0.1, -0.1); polygonToSnap.addVertex(-0.1, 0.1); polygonToSnap.update(); RigidBodyTransform snappingTransform = new RigidBodyTransform(); snappingTransform.setRotationEulerAndZeroTranslation(0.1, 0.2, 0.3); snappingTransform.setTranslation(1.2, 3.4, 5.6); double intersectionArea = planarRegion.getPolygonIntersectionAreaWhenSnapped(polygonToSnap, snappingTransform); assertEquals(0.04, intersectionArea, 1e-7); }