public ConnectionPoint3D getPointGivenPercentage(double percentage, int regionId) { Point3D result = new Point3D(); result.interpolate(source, target, percentage); return new ConnectionPoint3D(result, regionId); }
public static Point3D getPointAlongPathGivenDistanceFromStart(List<? extends Point3DReadOnly> path, double distance) { if (path == null || path.isEmpty()) return null; if (path.size() == 1 || distance <= 0.0) return new Point3D(path.get(0)); for (int segmentIndex = 0; segmentIndex < path.size() - 1; segmentIndex++) { Point3DReadOnly segmentStart = path.get(segmentIndex); Point3DReadOnly segmentEnd = path.get(segmentIndex + 1); double segmentLength = segmentStart.distance(segmentEnd); if (distance <= segmentLength) { Point3D result = new Point3D(); result.interpolate(segmentStart, segmentEnd, distance / segmentLength); return result; } else { distance -= segmentLength; } } // Got here because of numerical error, we are at the path. return new Point3D(path.get(path.size() - 1)); }
protected void validateTest(FootstepDataListMessage footsteps, boolean simulate) throws SimulationExceededMaximumTimeException { List<FootstepDataMessage> footstepList = footsteps.getFootstepDataList(); int size = footstepList.size(); if (simulate) { double duration = size * (footsteps.getDefaultSwingDuration() + footsteps.getDefaultTransferDuration()); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(duration + 3.0)); } Point3D lastStep = footstepList.get(size - 1).getLocation(); Point3D secondToLastStep = footstepList.get(size - 2).getLocation(); Point3D goalPoint = new Point3D(); goalPoint.interpolate(lastStep, secondToLastStep, 0.5); goalPoint.addZ(getNominalHeight()); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(goalPoint, new Vector3D(0.2, 0.2, 0.4)); drcSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox); }
@Test public void testPointOnLineGivenPercentage() throws Exception { Random random = new Random(342L); for (int i = 0; i < ITERATIONS; i++) { LineSegment3D lineSegment3D = EuclidGeometryRandomTools.nextLineSegment3D(random, 10.0); double percentage = EuclidCoreRandomTools.nextDouble(random, 10.0); Point3D expected = new Point3D(); expected.interpolate(lineSegment3D.getFirstEndpoint(), lineSegment3D.getSecondEndpoint(), percentage); Point3DBasics actual = new Point3D(); actual = lineSegment3D.pointOnLineGivenPercentage(percentage); EuclidCoreTestTools.assertTuple3DEquals(expected, actual, EPSILON); } }
LineSegment3D randomSegment = new LineSegment3D(randomOriginStart, randomOriginEnd); Point3D midpoint = new Point3D(); midpoint.interpolate(randomOriginStart, randomOriginEnd, 0.5);
private Point3D getMidPoint(FootstepNode node, FootstepNode previousNode) { List<PlanarRegion> nodePlanes = snapper.getOrCreateSteppableRegions(node.getRoundedX(), node.getRoundedY()); if (nodePlanes.isEmpty()) return null; tempPoint.set(node.getRoundedX(), node.getRoundedY(), 0.0); Point3DReadOnly projectedPoint = PlanarRegionTools.projectPointToPlanesVertically(tempPoint, nodePlanes); if (projectedPoint == null) return null; List<PlanarRegion> previousNodePlanes = snapper.getOrCreateSteppableRegions(previousNode.getRoundedX(), previousNode.getRoundedY()); if (previousNodePlanes.isEmpty()) return null; tempPoint.set(previousNode.getRoundedX(), previousNode.getRoundedY(), 0.0); Point3DReadOnly previousProjectedPoint = PlanarRegionTools.projectPointToPlanesVertically(tempPoint, previousNodePlanes); if (previousProjectedPoint == null) return null; Point3D midPoint = new Point3D(); midPoint.interpolate(projectedPoint, previousProjectedPoint, 0.5); return midPoint; } }
pointOnLineSegment.interpolate(lineSegmentStart, lineSegmentEnd, expectedPercentage); double actualPercentage = EuclidGeometryTools.percentageAlongLineSegment3D(pointOnLineSegment, lineSegmentStart, lineSegmentEnd); assertEquals(expectedPercentage, actualPercentage, EPSILON); pointOnLineSegment.interpolate(lineSegmentStart, lineSegmentEnd, expectedPercentage); actualPercentage = EuclidGeometryTools.percentageAlongLineSegment3D(pointOnLineSegment, lineSegmentStart, lineSegmentEnd); assertEquals(expectedPercentage, actualPercentage, EPSILON); pointOnLineSegment.interpolate(lineSegmentStart, lineSegmentEnd, expectedPercentage); actualPercentage = EuclidGeometryTools.percentageAlongLineSegment3D(pointOnLineSegment, lineSegmentStart, lineSegmentEnd); assertEquals(expectedPercentage, actualPercentage, EPSILON); pointOffLineSegment.interpolate(lineSegmentStart, lineSegmentEnd, expectedPercentage); pointOffLineSegment.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, pointOffLineSegment); double actualPercentage = EuclidGeometryTools.percentageAlongLineSegment3D(pointOffLineSegment, lineSegmentStart, lineSegmentEnd); pointOffLineSegment.interpolate(lineSegmentStart, lineSegmentEnd, expectedPercentage); pointOffLineSegment.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, pointOffLineSegment); actualPercentage = EuclidGeometryTools.percentageAlongLineSegment3D(pointOffLineSegment, lineSegmentStart, lineSegmentEnd); pointOffLineSegment.interpolate(lineSegmentStart, lineSegmentEnd, expectedPercentage); pointOffLineSegment.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, pointOffLineSegment); actualPercentage = EuclidGeometryTools.percentageAlongLineSegment3D(pointOffLineSegment, lineSegmentStart, lineSegmentEnd);
/** * trajectory for current transform to goal transform */ public static Pose3D computeLinearTrajectory(double time, double trajectoryTime, RigidBodyTransform from, RigidBodyTransform to) { double progress = time / trajectoryTime; Point3D fromPoint = new Point3D(from.getTranslationVector()); Point3D toPoint = new Point3D(to.getTranslationVector()); Quaternion fromOrienation = new Quaternion(from.getRotationMatrix()); Quaternion toOrienation = new Quaternion(to.getRotationMatrix()); Point3D point = new Point3D(); Quaternion orientation = new Quaternion(); point.interpolate(fromPoint, toPoint, progress); orientation.interpolate(fromOrienation, toOrienation, progress); return new Pose3D(point, orientation); }
expected.interpolate(lineSegment3D.getFirstEndpoint(), lineSegment3D.getSecondEndpoint(), percentage); Point3DBasics actual = new Point3D(); actual = lineSegment3D.pointBetweenEndpointsGivenPercentage(percentage); expected.interpolate(lineSegment3D.getFirstEndpoint(), lineSegment3D.getSecondEndpoint(), percentage); Point3D actual = new Point3D(); lineSegment3D.pointBetweenEndpointsGivenPercentage(percentage, actual);
projection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); expectedDistance = projection.distance(testPoint); projection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); projection.set(lineSegmentStart); projection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); projection.set(lineSegmentEnd);
private void assertReachedGoal(FootstepDataListMessage footsteps) { int numberOfSteps = footsteps.getFootstepDataList().size(); Point3D lastStep = footsteps.getFootstepDataList().get(numberOfSteps - 1).getLocation(); Point3D nextToLastStep = footsteps.getFootstepDataList().get(numberOfSteps - 2).getLocation(); Point3D midStance = new Point3D(); midStance.interpolate(lastStep, nextToLastStep, 0.5); Point3D midpoint = new Point3D(midStance); midpoint.addZ(1.0); Point3D bounds = new Point3D(0.25, 0.25, 0.3); BoundingBox3D boundingBox3D = BoundingBox3D.createUsingCenterAndPlusMinusVector(midpoint, bounds); drcSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox3D); } }
projection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); expectedSquaredDistance = projection.distanceSquared(testPoint); projection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); projection.set(lineSegmentStart); projection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); projection.set(lineSegmentEnd);
lineSegmentStart.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); lineSegmentEnd.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0)); lineSegmentStart.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); lineSegmentEnd.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); lineSegmentStart.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); lineSegmentEnd.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0)); lineSegmentStart.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); lineSegmentEnd.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); lineSegmentStart.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0)); lineSegmentEnd.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0)); lineSegmentStart.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); lineSegmentEnd.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0));
expectedProjection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, expectedProjection); success = EuclidGeometryTools.orthogonalProjectionOnLineSegment3D(testPoint, lineSegmentStart, lineSegmentEnd, actualProjection); expectedProjection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, expectedProjection); expectedProjection.set(lineSegmentStart); expectedProjection.interpolate(lineSegmentStart, lineSegmentEnd, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0)); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, expectedProjection); expectedProjection.set(lineSegmentEnd);
rayOrigin.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, -10.0, 0.0)); rayOrigin.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 0.0, 1.0)); rayOrigin.interpolate(pointOnEllipsoid1, pointOnEllipsoid2, EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0));
@ContinuousIntegrationTest(estimatedDuration = 45.0) @Test(timeout = 70000) public void testWideStep() throws SimulationExceededMaximumTimeException { setupTest(); Point3D step1 = new Point3D(0.0, -stepWidth, 0.0); Point3D step2 = new Point3D(0.0, stanceWidth - stepWidth, 0.0); FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); footstepDataListMessage.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, step1, new FrameQuaternion())); footstepDataListMessage.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, step2, new FrameQuaternion())); drcSimulationTestHelper.publishToController(footstepDataListMessage); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(6.0)); Point3D center = new Point3D(); center.interpolate(step1, step2, 0.5); center.addZ(1.0); Vector3D plusMinusVector = new Vector3D(0.1, stanceWidth / 2.0, 0.1); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(center, plusMinusVector); drcSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox); }
double alpha = EuclidCoreRandomTools.nextDouble(random, 10.0); expectedPoint.interpolate(boundingBox3D.getMinPoint(), boundingBox3D.getMaxPoint(), alpha); boundingBox3D.getPointGivenParameters(alpha, alpha, alpha, actualPoint); EuclidCoreTestTools.assertTuple3DEquals(expectedPoint, actualPoint, EPSILON);
query.interpolate(firstPointOnLine, secondPointOnLine, EuclidCoreRandomTools.nextDouble(random, 10.0));
position.interpolate(aboveInitialPose.getPosition(), initialPose.getPosition(), alpha); scsRootJoint.setPosition(position); success = success && drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(0.05);
center.interpolate(step6Location, step7Location, 0.5); center.addZ(0.9); Vector3D plusMinusVector = new Vector3D(0.2, 0.2, 0.5);