How to use

isPointInside

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

public boolean isPointInside(FramePoint2d framePoint)
{
 return isPointInside(framePoint, 0.0);
}

private void drawTestPoints(ArrayList<FramePoint2d> testPoints, FrameConvexPolygon2d polygon, Graphics graphics)
{
 for (FramePoint2d testPoint : testPoints)
 {
   boolean isInside = polygon.isPointInside(testPoint);
   if (isInside)
   {
    drawInsidePoint(testPoint, graphics);
   }
   else
   {
    drawOutsidePoint(testPoint, graphics);
   }
 }
}

@Override
public boolean isDone()
{
 if (!balanceManager.isICPPlanDone())
   return false;
 balanceManager.getCapturePoint(capturePoint2d);
 FrameConvexPolygon2d supportPolygonInWorld = momentumBasedController.getBipedSupportPolygons().getSupportPolygonInWorld();
 boolean isICPInsideSupportPolygon = supportPolygonInWorld.isPointInside(capturePoint2d);
 if (!isICPInsideSupportPolygon)
   return true;
 else
   return balanceManager.isTransitionToSingleSupportSafe(transferToSide);
}

public void update()
{
 momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP);
 if (desiredCoP.containsNaN())
   isDesiredCoPOnEdge.set(false);
 else
 {
   double epsilon = isDesiredCoPOnEdge.getBooleanValue() ? EPSILON_POINT_ON_EDGE_WITH_HYSTERESIS : EPSILON_POINT_ON_EDGE;
   FrameConvexPolygon2d footSupportPolygon = bipedSupportPolygons.getFootPolygonInSoleFrame(robotSide);
   isDesiredCoPOnEdge.set(!footSupportPolygon.isPointInside(desiredCoP, -epsilon)); // Minus means that the check is done with a smaller polygon
 }
}

@Override
public void computeCMPInternal(FramePoint2d desiredCMPPreviousValue)
{
 icpOptimizationController.compute(yoTime.getDoubleValue(), desiredCapturePoint, desiredCapturePointVelocity, capturePoint, omega0);
 icpOptimizationController.getDesiredCMP(desiredCMP);
 yoUnprojectedDesiredCMP.set(desiredCMP);
 // do projection here:
 if (!areaToProjectInto.isEmpty())
 {
   desiredCMPinSafeArea.set(safeArea.isPointInside(desiredCMP));
   if (safeArea.isPointInside(desiredCMP))
   {
    supportPolygon.setIncludingFrameAndUpdate(bipedSupportPolygons.getSupportPolygonInMidFeetZUp());
    areaToProjectInto.setIncludingFrameAndUpdate(supportPolygon);
   }
   cmpProjector.projectCMPIntoSupportPolygonIfOutside(capturePoint, areaToProjectInto, finalDesiredCapturePoint, desiredCMP);
 }
}

boolean isCoPInsideFoot = footPolygon.isPointInside(tempCoP2d);
if (!isCoPInsideFoot)

public boolean isIncluded(Point3d testInput)
{
 FramePoint test = new FramePoint(ReferenceFrame.getWorldFrame(),testInput);
 if (DEBUG)
   System.out.printf("SimpleFootstepMask: testing point input is <%.2f,%.2f> in frame %s", test.getX(), test.getY(), test.getReferenceFrame());
 test.changeFrame(yawFrame2d);
 boolean isPointInside = footPolygon.isPointInside(test.toFramePoint2d());
 if (DEBUG)
   System.out.printf("SimpleFootstepMask: testing point <%.2f,%.2f> in frame %s. Point is in is %s.  ", test.getX(), test.getY(), test.getReferenceFrame(),isPointInside ? "true":"false");
 return isPointInside;
}

@Override
public void doAction()
{
 super.doAction();
 if (walkingMessageHandler.hasFootTrajectoryForFlamingoStance(swingSide))
 {
   while(walkingMessageHandler.hasFootTrajectoryForFlamingoStance(swingSide))
    feetManager.handleFootTrajectoryCommand(walkingMessageHandler.pollFootTrajectoryForFlamingoStance(swingSide));
 }
 balanceManager.getCapturePoint(capturePoint2d);
 boolean icpErrorIsTooLarge = balanceManager.getICPErrorMagnitude() > 0.05;
 if (icpErrorIsTooLarge)
 {
   FrameConvexPolygon2d supportPolygonInWorld = bipedSupportPolygons.getSupportPolygonInWorld();
   FrameConvexPolygon2d combinedFootPolygon = failureDetectionControlModule.getCombinedFootPolygon();
   if (!supportPolygonInWorld.isPointInside(capturePoint2d, 2.0e-2) && combinedFootPolygon.isPointInside(capturePoint2d))
   {
    feetManager.requestMoveStraightTouchdownForDisturbanceRecovery(swingSide);
    initiateFootLoadingProcedure(swingSide);
    balanceManager.requestICPPlannerToHoldCurrentCoMInNextDoubleSupport();
   }
 }
 walkingMessageHandler.clearFootTrajectory(supportSide);
}

public void computeCMPInternal(FramePoint2d desiredCMPPreviousValue)
{
 if (supportSide != supportLegPreviousTick.getEnumValue())
 {
   icpProportionalController.reset();
 }
 desiredCMP.set(icpProportionalController.doProportionalControl(desiredCMPPreviousValue, capturePoint, desiredCapturePoint,
    finalDesiredCapturePoint, desiredCapturePointVelocity, perfectCMP, omega0));
 yoUnprojectedDesiredCMP.set(desiredCMP);
 // do projection here:
 if (!areaToProjectInto.isEmpty())
 {
   desiredCMPinSafeArea.set(safeArea.isPointInside(desiredCMP));
   if (safeArea.isPointInside(desiredCMP))
   {
    supportPolygon.setIncludingFrameAndUpdate(bipedSupportPolygons.getSupportPolygonInMidFeetZUp());
    areaToProjectInto.setIncludingFrameAndUpdate(supportPolygon);
   }
   cmpProjector.projectCMPIntoSupportPolygonIfOutside(capturePoint, areaToProjectInto, finalDesiredCapturePoint, desiredCMP);
   if (cmpProjector.getWasCMPProjected())
    icpProportionalController.bleedOffIntegralTerm();
 }
}

private void projectCMPIntoSupportPolygonIfOutsideLocal(FrameConvexPolygon2d supportPolygon, FramePoint2d desiredCMPToPack)
{
 cmpProjected.set(false);
 if (supportPolygon.getArea() < 1.0e-3)
 {
   supportPolygon.getCentroid(desiredCMPToPack);
   return;
 }
 if (supportPolygon.isPointInside(projectedCMP))
 {
   return;
 }
 else
 {
   supportPolygon.getClosestVertex(projectedCMP, projectedCMP);
   cmpProjected.set(true);
 }
 desiredCMPToPack.setX(projectedCMP.getX());
}

  isDesiredECMPOKForToeOff.set(onToesSupportPolygon.isPointInside(desiredECMP));
else
  isDesiredECMPOKForToeOff.set(true);
     onToesSupportPolygon.isPointInside(desiredICP) && leadingFootSupportPolygon.distance(desiredICP) < (icpProximityToLeadingFootForDSToeOff.getDoubleValue());
  isCurrentICPOKForToeOff =
     onToesSupportPolygon.isPointInside(currentICP) && leadingFootSupportPolygon.distance(currentICP) < (icpProximityToLeadingFootForDSToeOff.getDoubleValue());
  isDesiredICPOKForToeOff = leadingFootSupportPolygon.isPointInside(desiredICP);
  isCurrentICPOKForToeOff = leadingFootSupportPolygon.isPointInside(currentICP);

  isDesiredECMPOKForToeOff.set(isOnExitCMP && onToesSupportPolygon.isPointInside(desiredECMP));
else
  isDesiredECMPOKForToeOff.set(isOnExitCMP);
     onToesSupportPolygon.isPointInside(desiredICP) && nextFootstepPolygon.distance(desiredICP) < (icpProximityToLeadingFootForSSToeOff.getDoubleValue());
  isCurrentICPOKForToeOff =
     onToesSupportPolygon.isPointInside(currentICP) && nextFootstepPolygon.distance(currentICP) < (icpProximityToLeadingFootForSSToeOff.getDoubleValue());
  isDesiredICPOKForToeOff = nextFootstepPolygon.isPointInside(desiredICP);
  isCurrentICPOKForToeOff = nextFootstepPolygon.isPointInside(currentICP);

@Deprecated
public static void movePointInsidePolygonAlongLine(FramePoint2d point, FrameConvexPolygon2d polygon, FrameLine2d line, double amountToBeInside)
 if (!polygon.isPointInside(point))

@Override
public void doTransitionIntoAction()
{
 super.doTransitionIntoAction();
 footPolygon.clear(soleFrame);
 for (int i = 0; i < contactPoints.size(); i++)
 {
   contactPoints.get(i).getPosition2d(toeOffContactPoint2d);
   footPolygon.addVertex(toeOffContactPoint2d);
 }
 footPolygon.update();
 FramePoint2d rayOrigin;
 if (!exitCMP2d.containsNaN() && footPolygon.isPointInside(exitCMP2d))
   rayOrigin = exitCMP2d;
 else
   rayOrigin = footPolygon.getCentroid();
 rayThroughExitCMP.set(rayOrigin, exitCMPRayDirection2d);
 frameConvexPolygonWithRayIntersector2d.intersectWithRay(footPolygon, rayThroughExitCMP);
 toeOffContactPoint2d.set(frameConvexPolygonWithRayIntersector2d.getIntersectionPointOne());
 contactPointPosition.setXYIncludingFrame(toeOffContactPoint2d);
 contactPointPosition.changeFrame(contactableFoot.getRigidBody().getBodyFixedFrame());
 pointFeedbackControlCommand.setBodyFixedPointToControl(contactPointPosition);
 desiredContactPointPosition.setXYIncludingFrame(toeOffContactPoint2d);
 desiredContactPointPosition.changeFrame(worldFrame);
 desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint());
 desiredOrientation.changeFrame(worldFrame);
 desiredYawToHold = desiredOrientation.getYaw();
 desiredRollToHold = desiredOrientation.getRoll();
}

private void checkIfUseUpperBodyMomentumDoubleSupport()
{
 safeArea.setIncludingFrameAndUpdate(supportPolygon);
 safeCMPArea.setIncludingFrameAndUpdate(safeArea);
 // hysteresis:
 // shrink the safe area if we are already using upper body momentum
 if (usingUpperBodyMomentum.getBooleanValue())
 {
   polygonShrinker.shrinkConstantDistanceInto(safeArea, distanceToShrinkSafeAreaIfRecoveringDS.getDoubleValue(), tempPolygon1);
   safeArea.setIncludingFrameAndUpdate(tempPolygon1);
 }
 else
 {
   polygonShrinker.shrinkConstantDistanceInto(safeArea, distanceToShrinkSafeAreaDS.getDoubleValue(), tempPolygon1);
   safeArea.setIncludingFrameAndUpdate(tempPolygon1);
 }
 capturePoint2d.changeFrameAndProjectToXYPlane(safeArea.getReferenceFrame());
 boolean icpInSafeArea = safeArea.isPointInside(capturePoint2d);
 if (!icpInSafeArea)
 {
   usingUpperBodyMomentum.set(true);
 }
 else
 {
   usingUpperBodyMomentum.set(false);
 }
}

private void computeShrunkFoothold(FramePoint2d desiredCenterOfPressure)
{
 boolean wasCoPInThatRegion = false;
 if (useCoPOccupancyGrid.getBooleanValue()) {
   numberOfCellsOccupiedOnSideOfLine.set(footCoPOccupancyGrid.computeNumberOfCellsOccupiedOnSideOfLine(lineOfRotation, RobotSide.RIGHT,
      distanceFromLineOfRotationToComputeCoPOccupancy.getDoubleValue()));
   wasCoPInThatRegion = numberOfCellsOccupiedOnSideOfLine.getIntegerValue() >= thresholdForCoPRegionOccupancy.getIntegerValue();
 }
 unsafeArea.set(unsafePolygon.getArea());
 boolean areaBigEnough = unsafeArea.getDoubleValue() >= minAreaToConsider.getDoubleValue();
 unsafeAreaAboveThreshold.set(areaBigEnough);
 boolean desiredCopInPolygon = unsafePolygon.isPointInside(desiredCenterOfPressure, 0.0e-3);
 if (desiredCopInPolygon && !wasCoPInThatRegion && areaBigEnough)
 {
   backupFootPolygon.set(shrunkFootPolygon);
   ConvexPolygonTools.cutPolygonWithLine(lineOfRotation, shrunkFootPolygon, RobotSide.RIGHT);
   unsafePolygon.changeFrameAndProjectToXYPlane(worldFrame);
   yoUnsafePolygon.setFrameConvexPolygon2d(unsafePolygon);
   shrunkFootPolygonInWorld.setIncludingFrame(shrunkFootPolygon);
   shrunkFootPolygonInWorld.changeFrameAndProjectToXYPlane(worldFrame);
   yoShrunkFootPolygon.setFrameConvexPolygon2d(shrunkFootPolygonInWorld);
 }
 else
 {
   doNothing();
 }
}

boolean icpInSafeArea = safeArea.isPointInside(capturePoint2d);

if (supportPolygon.isPointInside(desiredCMP))
  return;

  return;
boolean isCoPOnEdge = !footPolygon.isPointInside(cop, -EPSILON);

isICPOutside.set(!supportPolygonInMidFeetZUp.isPointInside(this.capturePoint2d));