How to use

orthogonalProjection

method

in

us.ihmc.robotics.geometry.FrameConvexPolygon2d

public void projectOntoShrunkenPolygon(FramePoint2d pointToProject)
{
 shrunkFootPolygon.orthogonalProjection(pointToProject);
}

public static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon)
{
 ReferenceFrame originalReferenceFrame = point.getReferenceFrame();
 point.changeFrame(polygon.getReferenceFrame());
 FramePoint2d originalPoint = new FramePoint2d(point);
 polygon.orthogonalProjection(point);
 double distance = originalPoint.distance(point);
 if (distance > epsilon)
   throw new RuntimeException("point outside polygon by " + distance);
 point.changeFrame(originalReferenceFrame);
}

private void putExitCMPOnToes(FrameConvexPolygon2d footSupportPolygon, FramePoint2d exitCMPToPack)
{
 // Set x to have the CMP slightly inside the support polygon
 exitCMPToPack.setToZero(footSupportPolygon.getReferenceFrame());
 exitCMPToPack.setX(footSupportPolygon.getMaxX() - 1.6e-2);
 exitCMPToPack.setY(footSupportPolygon.getCentroid().getY());
 // Then constrain the computed CMP to be inside a safe support region
 tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon);
 convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdgesWhenSteppingDown.getDoubleValue(),
    footSupportPolygon);
 footSupportPolygon.orthogonalProjection(exitCMPToPack);
}

public void requestICPPlannerToHoldCurrentCoM()
{
 centerOfMassPosition.setToZero(centerOfMassFrame);
 FrameConvexPolygon2d supportPolygonInMidFeetZUp = bipedSupportPolygons.getSupportPolygonInMidFeetZUp();
 convexPolygonShrinker.shrinkConstantDistanceInto(supportPolygonInMidFeetZUp, distanceToShrinkSupportPolygonWhenHoldingCurrent.getDoubleValue(), shrunkSupportPolygon);
 centerOfMassPosition.changeFrame(shrunkSupportPolygon.getReferenceFrame());
 centerOfMassPosition2d.setByProjectionOntoXYPlaneIncludingFrame(centerOfMassPosition);
 shrunkSupportPolygon.orthogonalProjection(centerOfMassPosition2d);
 centerOfMassPosition.setXY(centerOfMassPosition2d);
 centerOfMassPosition.changeFrame(worldFrame);
 icpPlanner.holdCurrentICP(yoTime.getDoubleValue(), centerOfMassPosition);
}

private void constrainCMPAccordingToSupportPolygonAndUserOffsets(FramePoint2d cmpToPack, FrameConvexPolygon2d footSupportPolygon,
   FramePoint2d centroidOfFootstepToConsider, YoFrameVector2d cmpOffset, double minForwardCMPOffset, double maxForwardCMPOffset)
{
 // First constrain the computed CMP to the given min/max along the x-axis.
 FramePoint2d footSupportCentroid = footSupportPolygon.getCentroid();
 double cmpXOffsetFromCentroid = stepLengthToCMPOffsetFactor.getDoubleValue() * (centroidOfFootstepToConsider.getX() - footSupportCentroid.getX()) + cmpOffset.getX();
 cmpXOffsetFromCentroid = MathTools.clipToMinMax(cmpXOffsetFromCentroid, minForwardCMPOffset, maxForwardCMPOffset);
 cmpToPack.setIncludingFrame(footSupportCentroid);
 cmpToPack.add(cmpXOffsetFromCentroid, cmpOffset.getY());
 // Then constrain the computed CMP to be inside a safe support region
 tempSupportPolygonForShrinking.setIncludingFrameAndUpdate(footSupportPolygon);
 convexPolygonShrinker.shrinkConstantDistanceInto(tempSupportPolygonForShrinking, safeDistanceFromCMPToSupportEdges.getDoubleValue(), footSupportPolygon);
 footSupportPolygon.orthogonalProjection(cmpToPack);
}

/** 
* This function projects the footstep midpoint in the capture region.
* Might be a bit conservative it should be sufficient to slightly overlap the capture region
* and the touch-down polygon.
*/
private void projectFootstepInCaptureRegion(Footstep footstep, FramePoint2d projectionPoint, FrameConvexPolygon2d captureRegion)
{
 projection.setIncludingFrame(projectionPoint);
 projection.changeFrame(footstep.getParentFrame());
 // move the position of the footstep to the capture region centroid
 nextStep2d.setIncludingFrame(captureRegion.getCentroid());
 nextStep2d.changeFrame(footstep.getParentFrame());
 // move the position as far away from the projectionPoint as possible
 direction.setIncludingFrame(nextStep2d);
 direction.sub(projection);
 direction.normalize();
 direction.scale(10.0);
 nextStep2d.add(direction);
 nextStep2d.changeFrame(captureRegion.getReferenceFrame());
 captureRegion.orthogonalProjection(nextStep2d);
 nextStep2d.changeFrame(footstep.getParentFrame());
 footstep.setPositionChangeOnlyXY(nextStep2d);
 calculateTouchdownFootPolygon(footstep, captureRegion.getReferenceFrame(), touchdownFootPolygon);
}

supportPolygon.orthogonalProjection(desiredCMP);

safeSupportPolygonToConstrainICPOffset.orthogonalProjection(desiredICPToModify);

projectionArea.orthogonalProjection(projectedCMP);
activeProjection.set(ProjectionMethod.ORTHOGONAL_PROJECTION);