public double dot(FrameVector2d frameVector) { checkReferenceFrameMatch(frameVector); return this.tuple.dot(frameVector.tuple); }
public double angle(FrameVector2d frameVector) { checkReferenceFrameMatch(frameVector); return this.tuple.angle(frameVector.tuple); }
public static void computeCapturePointVelocity(FrameVector2d capturePointVelocityToPack, FrameVector2d centerOfMassVelocityInWorld, FrameVector2d centerOfMassAccelerationInWorld, double omega0) { centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassAccelerationInWorld.checkReferenceFrameMatch(worldFrame); capturePointVelocityToPack.setToZero(worldFrame); capturePointVelocityToPack.set(centerOfMassAccelerationInWorld); capturePointVelocityToPack.scale(1.0 / omega0); capturePointVelocityToPack.add(centerOfMassVelocityInWorld); }
@Override public void setFinalHeadingTarget(FrameVector2d finalHeadingTarget) { finalHeadingTarget.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setFinalHeadingTargetAngle(Math.atan2(finalHeadingTarget.getY(), finalHeadingTarget.getX())); }
@Override public void setFinalHeadingTarget(FrameVector2d finalHeadingTarget) { finalHeadingTarget.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setFinalHeadingTargetAngle(Math.atan2(finalHeadingTarget.getY(), finalHeadingTarget.getX())); }
public double cross(FrameVector2d frameVector) { checkReferenceFrameMatch(frameVector); return this.tuple.getX() * frameVector.tuple.getY() - tuple.getY() * frameVector.tuple.getX(); }
public void setFallingDirection(FrameVector2d fallingDirection) { fallingDirection.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); if (this.fallingDirection == null) this.fallingDirection = new Vector2f(); this.fallingDirection.set(fallingDirection.getVector()); }
public void setTranslationAndUpdate(FrameVector2d translation) { translation.checkReferenceFrameMatch(parentFrame); this.translation.setX(translation.getX()); this.translation.setY(translation.getY()); this.update(); }
public static void computeCapturePoint(FramePoint2d capturePointToPack, FramePoint2d centerOfMassInWorld, FrameVector2d centerOfMassVelocityInWorld, double omega0) { centerOfMassInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); capturePointToPack.setToZero(worldFrame); capturePointToPack.set(centerOfMassVelocityInWorld); capturePointToPack.scale(1.0 / omega0); capturePointToPack.add(centerOfMassInWorld); }
/** * Compute the distance along the capture point guide line from the * current capture point position to the desired capture point position. * * @param currentCapturePointPosition * @param desiredCapturePointPosition * @param desiredCapturePointVelocity * @return */ public static double computeDistanceToCapturePointFreezeLineIn2d(FramePoint2d currentCapturePointPosition, FramePoint2d desiredCapturePointPosition, FrameVector2d desiredCapturePointVelocity) { currentCapturePointPosition.checkReferenceFrameMatch(desiredCapturePointPosition); desiredCapturePointVelocity.checkReferenceFrameMatch(desiredCapturePointPosition); double desiredCapturePointVelocityMagnitude = Math.sqrt(MathTools.square(desiredCapturePointVelocity.getX()) + MathTools.square(desiredCapturePointVelocity.getY())); if (desiredCapturePointVelocityMagnitude == 0.0) { return Double.NaN; } else { double normalizedCapturePointVelocityX = desiredCapturePointVelocity.getX() / desiredCapturePointVelocityMagnitude; double normalizedCapturePointVelocityY = desiredCapturePointVelocity.getY() / desiredCapturePointVelocityMagnitude; double capturePointErrorX = currentCapturePointPosition.getX() - desiredCapturePointPosition.getX(); double capturePointErrorY = currentCapturePointPosition.getY() - desiredCapturePointPosition.getY(); return -(normalizedCapturePointVelocityX * capturePointErrorX + normalizedCapturePointVelocityY * capturePointErrorY); } }
/** * 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); } }