private void fitLine() { if (!footCoPOccupancyGrid.fitLineToData(line)) return; lineL.setIncludingFrame(line); lineL.shiftToLeft(width/2.0); lineR.setIncludingFrame(line); lineR.shiftToRight(width/2.0); backupFootPolygon.set(shrunkFootPolygon); shrunkFootPolygon.clear(); shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineL)); shrunkFootPolygon.addVertices(defaultFootPolygon.intersectionWith(lineR)); shrunkFootPolygon.update(); }
rayFromICPAwayFromFinalDesiredICP.setIncludingFrame(capturePoint, finalDesiredICPToICPDirection); FramePoint2d[] finalDesiredICPToICPIntersections = supportPolygon.intersectionWith(rayFromICPAwayFromFinalDesiredICP);
footCenterCoPLineSegment.set(footFrame, 0.0, 0.0, tempCoP2d.getX(), tempCoP2d.getY()); FramePoint2d[] intersectionPoints = footPolygon.intersectionWith(footCenterCoPLineSegment);
boolean nextStepInside = desiredSteppingRegion.intersectionWith(touchdownFootPolygon, intersection);
FramePoint2d[] intersections = polygon.intersectionWith(line);
FramePoint2d[] intersections = polygon.intersectionWith(line); if (intersections != null)
FramePoint2d[] intersections = gridBoundaries.intersectionWith(shiftedLine);
private void intersectASinglePolygon(FrameConvexPolygon2d polygon, Pair<FramePoint, FramePoint> intersectionWithPolygon) { intersectionOfPlanes.changeFrame(polygon.getReferenceFrame()); intersectionOfPlanes.projectOntoXYPlane(planeIntersectionOnPolygonPlane); polygon.intersectionWith(planeIntersectionOnPolygonPlane, lineIntersectionOnPolygonPlane); if (lineIntersectionOnPolygonPlane.getFirst().containsNaN() && lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { noIntersection = true; } if (!lineIntersectionOnPolygonPlane.getFirst().containsNaN()) { intersectionWithPolygon.getFirst().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getFirst()); } if (!lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { intersectionWithPolygon.getSecond().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getSecond()); } }
rawCaptureRegion.intersectionWith(reachableRegions.get(swingSide.getOppositeSide()), captureRegionPolygon);