@Override public void getLineOfRotation(FrameLine2d lineOfRotationToPack) { lineOfRotationToPack.setIncludingFrame(lineOfRotationInSoleFrame); }
@Override public void getLineOfRotation(FrameLine2d lineOfRotationToPack) { lineOfRotationToPack.setIncludingFrame(lineOfRotationInSoleFrame); } }
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(); }
shiftedLine.setIncludingFrame(shiftedLinePoint, shiftedLineVector);
if (verified) lineOfRotation.setIncludingFrame(lineOfRotations.get(calculatorType));
return; icpToCMPLine.setIncludingFrame(capturePoint, desiredCMP); int intersections = supportPolygon.intersectionWithRay(icpToCMPLine, intersection1, intersection2); rayFromICPAwayFromFinalDesiredICP.setIncludingFrame(finalDesiredICPLocation, capturePoint); rayFromICPAwayFromFinalDesiredICP.setIncludingFrame(capturePoint, finalDesiredICPToICPDirection);
shiftedLine.setIncludingFrame(shiftedLinePoint, shiftedLineVector);
lineOfRotationInSoleFrame.setIncludingFrame(soleFrame, 0.0, 0.0, 1.0, 0.0);
lineOfRotationInWorldFrame.set(centerOfRotation2d, lineOfRotation2d); lineOfRotationInSoleFrame.setIncludingFrame(lineOfRotationInWorldFrame); lineOfRotationInSoleFrame.changeFrameAndProjectToXYPlane(soleFrame); lineOfRotationInSoleFrame.setPoint(centerOfPressure);
lineOfRotationInWorldFrame.setIncludingFrame(lineOfRotationInSoleFrame); lineOfRotationInWorldFrame.changeFrameAndProjectToXYPlane(worldFrame);
icpToCMPLine.setIncludingFrame(capturePoint, desiredCMP); if (projectionArea.intersectionWithRay(icpToCMPLine, intersection1, intersection2) > 0) finalICPToICPLine.setIncludingFrame(finalCapturePoint, capturePoint); if (projectionArea.intersectionWithRay(finalICPToICPLine, intersection1, intersection2) > 0)