public boolean eplilonEquals(FrameVector2d frameVector, double epsilon) { boolean referenceFrameMatches = referenceFrame.equals(frameVector.getReferenceFrame()); boolean pointEquals = tuple.epsilonEquals(frameVector.tuple, epsilon); return referenceFrameMatches && pointEquals; }
@Override public boolean equals(Object frameVector) { if (frameVector instanceof FrameVector2d) { boolean referenceFrameMatches = referenceFrame.equals(((FrameVector2d) frameVector).getReferenceFrame()); boolean pointEquals = tuple.equals(((FrameVector2d) frameVector).tuple); return referenceFrameMatches && pointEquals; } else { return super.equals(frameVector); } }
public void set(FramePoint2d firstEndpoint, FrameVector2d vectorToSecondEndpoint) { referenceFrame.checkReferenceFrameMatch(firstEndpoint.getReferenceFrame()); referenceFrame.checkReferenceFrameMatch(vectorToSecondEndpoint.getReferenceFrame()); frameLineSegment.set(firstEndpoint, vectorToSecondEndpoint); getYoValuesFromFrameLineSegment(); }
/** * Will return a point on a circle around the origin. The point will be in between the given directions and at * a position specified by the alpha value. E.g. an alpha value of 0.5 will result in the point being in the * middle of the given directions. */ public void getPointBetweenVectorsAtDistanceFromOriginCircular(FrameVector2d directionA, FrameVector2d directionB, double alpha, double radius, FramePoint2d midpoint, FramePoint2d pointToPack) { directionA.checkReferenceFrameMatch(directionB.getReferenceFrame()); directionA.checkReferenceFrameMatch(midpoint.getReferenceFrame()); alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); double angleBetweenDirections = directionA.angle(directionB); double angleBetweenDirectionsToSetLine = angleBetweenDirections * alpha; rotatedFromA.setToZero(directionA.getReferenceFrame()); rotatedFromA.set(directionA.getX(), directionA.getY(), 0.0); axisAngle.set(negZRotationAxis, angleBetweenDirectionsToSetLine); rotation.setRotation(axisAngle); rotatedFromA.applyTransform(rotation); rotatedFromA.normalize(); rotatedFromA.scale(radius); pointToPack.changeFrame(rotatedFromA.getReferenceFrame()); pointToPack.set(rotatedFromA.getX(), rotatedFromA.getY()); pointToPack.add(midpoint); } }
desiredHeadingControlModule.getDesiredHeading(desiredHeading, timeFromNow); toLeftOfDesiredHeading.setIncludingFrame(desiredHeading.getReferenceFrame(), -desiredHeading.getY(), desiredHeading.getX()); desiredVelocity.changeFrame(desiredHeading.getReferenceFrame()); velocityMagnitudeInHeading.set(desiredVelocity.dot(desiredHeading)); velocityMagnitudeToLeftOfHeading.set(desiredVelocity.dot(toLeftOfDesiredHeading));
desiredICPToModify.changeFrame(icpOffsetForFreezing.getReferenceFrame()); desiredICPToModify.add(icpOffsetForFreezing);
comXYAcceleration.setToZero(desiredICPVelocity.getReferenceFrame());