private boolean isBetweenEndpoints(double x, double y, double epsilon) { double alpha = percentageAlongLineSegment(x, y); if (alpha < epsilon) return false; if (alpha > 1.0 - epsilon) return false; return true; }
public double percentageAlongLineSegment(FramePoint2d point2d) { checkReferenceFrameMatch(point2d); return lineSegment.percentageAlongLineSegment(point2d.getPoint()); }
/** * Computes a percentage along this line segment representing the location of the given point once projected onto this line segment. * The returned percentage is in ] -∞; ∞ [, {@code 0.0} representing {@code lineSegmentStart}, and {@code 1.0} representing {@code lineSegmentEnd}. * <p> * For example, if the returned percentage is {@code 0.5}, it means that the projection of the given point is located at the middle of the line segment. * The coordinates of the projection of the point can be computed from the {@code percentage} as follows: * <code> * Point2d projection = new Point2d(); </br> * projection.interpolate(lineSegmentStart, lineSegmentEnd, percentage); </br> * </code> * </p> * <p> * Edge cases: * <ul> * <li> if the length of the given line segment is too small, i.e. {@code lineSegmentStart.distanceSquared(lineSegmentEnd) < Epsilons.ONE_TRILLIONTH}, this method fails and returns {@link Double#NaN}. * </ul> * </p> * * @param pointX the x-coordinate of the query point. * @param pointY the y-coordinate of the query point. * @return the computed percentage along this line segment representing where the point projection is located. */ public double percentageAlongLineSegment(Point2d point2d) { return percentageAlongLineSegment(point2d.getX(), point2d.getY()); }
/** * Compute the smallest distance from the point to this line segment. * If the projection of the given point on this line segment results in a point that is outside the line segment, the distance is computed between the given point and the closest line segment end point. */ @Override public double distance(Point2d point) { double alpha = percentageAlongLineSegment(point); if (alpha <= 0.0) { return point.distance(endpoints[0]); } else if (alpha >= 1.0) { return point.distance(endpoints[1]); } else { // Here we know the projection of the point belongs to the line segment. // In this case computing the distance from the point to the line segment is the same as computing the distance from the point the equivalent line. return GeometryTools.distanceFromPointToLine(point, endpoints[0], endpoints[1]); } }
private void solve(CoMHeightPartialDerivativesData coMHeightPartialDerivativesDataToPack, Point2d queryPoint, boolean isInDoubleSupport) double splineQuery = projectionSegment.percentageAlongLineSegment(queryPoint) * projectionSegment.length();
Point2d nextPoint2d = new Point2d(nextContactFramePosition.getX(), nextContactFramePosition.getY()); line2d.orthogonalProjectionCopy(nextPoint2d); xSNext = projectionSegment.percentageAlongLineSegment(nextPoint2d) * projectionSegment.length();