private void drawPoint(FramePoint2DReadOnly framePoint2d, Graphics graphics) { drawPoint(framePoint2d.getX(), framePoint2d.getY(), graphics); }
/** * yawAboutPoint * * @param pointToYawAbout FramePoint2d * @param yaw double * @return CartesianPositionFootstep */ public static void yawAboutPoint(FramePoint2DReadOnly pointToTransform, FramePoint2DReadOnly pointToYawAbout, double yaw, FramePoint2D resultToPack) { pointToTransform.checkReferenceFrameMatch(pointToYawAbout); double tempX = pointToTransform.getX() - pointToYawAbout.getX(); double tempY = pointToTransform.getY() - pointToYawAbout.getY(); double cosAngle = Math.cos(yaw); double sinAngle = Math.sin(yaw); double x = cosAngle * tempX + -sinAngle * tempY; tempY = sinAngle * tempX + cosAngle * tempY; tempX = x; resultToPack.setIncludingFrame(pointToYawAbout); resultToPack.add(tempX, tempY); }
public void setPositionChangeOnlyXY(FramePoint2DReadOnly position2d) { position2d.checkReferenceFrameMatch(footstepPose); setX(position2d.getX()); setY(position2d.getY()); }
private static void assertEquals(FramePoint2DReadOnly pointA, CoPTrajectoryPoint pointB, double epsilon) { assertEquals(new FramePoint3D(pointA.getReferenceFrame(), pointA.getX(), pointA.getY(), 0.0), pointB, epsilon); }
public static boolean isLineBetweenVertices(FrameLine2DReadOnly frameLine2d, FramePoint2DReadOnly leftVertex, FramePoint2DReadOnly rightVertex, boolean mustBeStrictlyBetween) { FramePoint2D lineStart = new FramePoint2D(frameLine2d.getPoint()); FrameVector2D lineDirection = new FrameVector2D(frameLine2d.getDirection()); double startToLeftVertexX = leftVertex.getX() - lineStart.getX(); double startToLeftVertexY = leftVertex.getY() - lineStart.getY(); double startToRightVertexX = rightVertex.getX() - lineStart.getX(); double startToRightVertexY = rightVertex.getY() - lineStart.getY(); // double crossProduct = vectorToEdge1X * vectorToEdge2Y - vectorToEdge1Y * vectorToEdge2X; double leftCrossProduct = lineDirection.getX() * startToLeftVertexY - lineDirection.getY() * startToLeftVertexX; double rightCrossProduct = lineDirection.getX() * startToRightVertexY - lineDirection.getY() * startToRightVertexX; if (mustBeStrictlyBetween) { if ((leftCrossProduct > 0.0) && (rightCrossProduct < 0.0)) return true; } else if ((leftCrossProduct >= 0.0) && (rightCrossProduct <= 0.0)) return true; return false; }
polyPoint.checkReferenceFrameMatch(convexPoint); assertTrue("not equal expected: " + polyPoint + " actual: " + convexPoint, MathTools.epsilonEquals(polyPoint.getX(), convexPoint.getX(), 1e-7)); assertTrue("not equal expected: " + polyPoint + " actual: " + convexPoint, MathTools.epsilonEquals(polyPoint.getY(), convexPoint.getY(), 1e-7));