@Test public void testGetMinimumDistanceTwoTrajRandom() { for (int i = 0; i < NUMBER_OF_TESTS; i++) { IVector2 initialPos1 = getRandomVector(POS_LIMIT); IVector2 finalPos1 = getRandomVector(POS_LIMIT); IVector2 initialVel1 = getRandomVector(2.0); IVector2 initialPos2 = getRandomVector(POS_LIMIT); IVector2 finalPos2 = getRandomVector(POS_LIMIT); IVector2 initialVel2 = getRandomVector(2.0); BangBangTrajectory2D traj1 = new BangBangTrajectory2D(initialPos1, finalPos1, initialVel1, 2, 3); BangBangTrajectory2D traj2 = new BangBangTrajectory2D(initialPos2, finalPos2, initialVel2, 2, 3); double dist = traj1.getPlanarCurve().getMinimumDistanceToCurve(traj2.getPlanarCurve()); double sampleDist = sampleDistMin(traj1, traj2, 1e-3); assertThat(dist).isCloseTo(sampleDist, within(2.0)); } }
public double getMinimumDistanceToCurve(final PlanarCurve curve) { List<Pair<PlanarCurveSegment, PlanarCurveSegment>> combined = combineCurves(segments, curve.getSegments()); double min = combined.get(0).getFirst().getPos().distanceTo(combined.get(0).getSecond().getPos()); for (Pair<PlanarCurveSegment, PlanarCurveSegment> part : combined) { double tStart = part.getFirst().getStartTime(); double tEnd = part.getFirst().getEndTime(); List<Double> roots = minDistanceRoots(part.getFirst(), part.getSecond()); roots.add(tEnd - tStart); double dist = roots.stream() .filter(r -> (r > 0) && (r <= (tEnd - tStart))) .mapToDouble(r -> part.getFirst().getPosition(r).distanceTo(part.getSecond().getPosition(r))) .min().orElse(Double.POSITIVE_INFINITY); if (dist < min) { min = dist; } } return min; }
PlanarCurve { public double getMinimumDistanceToCurve(final PlanarCurve curve) { List<Pair<PlanarCurveSegment, PlanarCurveSegment>> combined = combineCurves(segments, curve.getSegments()); double min = combined.get(0).getFirst().getPos().distanceTo(combined.get(0).getSecond().getPos()); for (Pair<PlanarCurveSegment, PlanarCurveSegment> part : combined) { double tStart = part.getFirst().getStartTime(); double tEnd = part.getFirst().getEndTime(); List<Double> roots = minDistanceRoots(part.getFirst(), part.getSecond()); roots.add(tEnd - tStart); double dist = roots.stream() .filter(r -> (r > 0) && (r <= (tEnd - tStart))) .mapToDouble(r -> part.getFirst().getPosition(r).distanceTo(part.getSecond().getPosition(r))) .min().orElse(Double.POSITIVE_INFINITY); if (dist < min) { min = dist; } } return min; } }
PlanarCurve { public double getMinimumDistanceToCurve(final PlanarCurve curve) { List<Pair<PlanarCurveSegment, PlanarCurveSegment>> combined = combineCurves(segments, curve.getSegments()); double min = combined.get(0).getFirst().getPos().distanceTo(combined.get(0).getSecond().getPos()); for (Pair<PlanarCurveSegment, PlanarCurveSegment> part : combined) { double tStart = part.getFirst().getStartTime(); double tEnd = part.getFirst().getEndTime(); List<Double> roots = minDistanceRoots(part.getFirst(), part.getSecond()); roots.add(tEnd - tStart); double dist = roots.stream() .filter(r -> (r > 0) && (r <= (tEnd - tStart))) .mapToDouble(r -> part.getFirst().getPosition(r).distanceTo(part.getSecond().getPosition(r))) .min().orElse(Double.POSITIVE_INFINITY); if (dist < min) { min = dist; } } return min; } PlanarCurve(final List<PlanarCurveSegment> segments); }
PlanarCurve { public double getMinimumDistanceToCurve(final PlanarCurve curve) { List<Pair<PlanarCurveSegment, PlanarCurveSegment>> combined = combineCurves(segments, curve.getSegments()); double min = combined.get(0).getFirst().getPos().distanceTo(combined.get(0).getSecond().getPos()); for (Pair<PlanarCurveSegment, PlanarCurveSegment> part : combined) { double tStart = part.getFirst().getStartTime(); double tEnd = part.getFirst().getEndTime(); List<Double> roots = minDistanceRoots(part.getFirst(), part.getSecond()); roots.add(tEnd - tStart); double dist = roots.stream() .filter(r -> (r > 0) && (r <= (tEnd - tStart))) .mapToDouble(r -> part.getFirst().getPosition(r).distanceTo(part.getSecond().getPosition(r))) .min().orElse(Double.POSITIVE_INFINITY); if (dist < min) { min = dist; } } return min; } PlanarCurve(final List<PlanarCurveSegment> segments); List<PlanarCurveSegment> getSegments(); double getTEnd(); static PlanarCurve fromPoint(final IVector2 point); static PlanarCurve fromPositionAndVelocity(final IVector2 pos, final IVector2 vel, final double tEnd); static PlanarCurve fromPositionVelocityAndAcceleration(final IVector2 pos, final IVector2 vel,
			final IVector2 acc, final double tEnd); PlanarCurve restrictToEnd(final double tEnd); PlanarCurve restrictToStart(final double tStart); double getMinimumDistanceToPoint(final IVector2 point); double getMinimumDistanceToCurve(final PlanarCurve curve); List<IVector2> getIntersectionsWithLineSegment(final ILine line); List<IVector2> getIntersectionsWithRectangle(final IRectangle rect); }
PlanarCurve { public double getMinimumDistanceToCurve(final PlanarCurve curve) { List<Pair<PlanarCurveSegment, PlanarCurveSegment>> combined = combineCurves(segments, curve.getSegments()); double min = combined.get(0).getFirst().getPos().distanceTo(combined.get(0).getSecond().getPos()); for (Pair<PlanarCurveSegment, PlanarCurveSegment> part : combined) { double tStart = part.getFirst().getStartTime(); double tEnd = part.getFirst().getEndTime(); List<Double> roots = minDistanceRoots(part.getFirst(), part.getSecond()); roots.add(tEnd - tStart); double dist = roots.stream() .filter(r -> (r > 0) && (r <= (tEnd - tStart))) .mapToDouble(r -> part.getFirst().getPosition(r).distanceTo(part.getSecond().getPosition(r))) .min().orElse(Double.POSITIVE_INFINITY); if (dist < min) { min = dist; } } return min; } PlanarCurve(final List<PlanarCurveSegment> segments); List<PlanarCurveSegment> getSegments(); double getTEnd(); static PlanarCurve fromPoint(final IVector2 point); static PlanarCurve fromPositionAndVelocity(final IVector2 pos, final IVector2 vel, final double tEnd); static PlanarCurve fromPositionVelocityAndAcceleration(final IVector2 pos, final IVector2 vel,
			final IVector2 acc, final double tEnd); PlanarCurve restrictToEnd(final double tEnd); PlanarCurve restrictToStart(final double tStart); double getMinimumDistanceToPoint(final IVector2 point); double getMinimumDistanceToCurve(final PlanarCurve curve); List<IVector2> getIntersectionsWithLineSegment(final ILine line); List<IVector2> getIntersectionsWithRectangle(final IRectangle rect);  }