/** * Compute the 3D equivalent of this line segment. The 3D equivalent of each end point is computed * as follows: {@code endPoint3d = endPoint1d * direction3d + zero3d}. * * @param zero3d position of the 3D equivalent of an endpoint equal to zero. * @param direction3d direction toward greater values of {@code endPoint1d}. * @return the 3D equivalent of this line segment. */ public LineSegment3D toLineSegment3d(Point3DReadOnly zero3d, Vector3DReadOnly direction3d) { Point3D firstEndpoint = new Point3D(); Point3D secondEndpoint = new Point3D(); firstEndpoint.scaleAdd(this.firstEndpoint, direction3d, zero3d); secondEndpoint.scaleAdd(this.secondEndpoint, direction3d, zero3d); return new LineSegment3D(firstEndpoint, secondEndpoint); }
private List<Pair<OcTreeKey, Float>> insertMissRay(Point3DReadOnly sensorOrigin, Point3DReadOnly scanPoint, Set<OcTreeKey> occupiedCells, Map<OcTreeKey, NormalOcTreeNode> keyToNodeMap) { Vector3D direction = new Vector3D(scanPoint); direction.sub(sensorOrigin); double length = direction.length(); if (minInsertRange >= 0.0 && length < minInsertRange) return null; Point3D rayEnd = new Point3D(scanPoint); if (maxInsertRange > 0.0 && length > maxInsertRange) { // user set a maxrange and length is above rayEnd.scaleAdd(maxInsertRange / length, direction, sensorOrigin); } // end if maxrange List<Pair<OcTreeKey, Float>> keysAndMissUpdates = new ArrayList<>(); RayActionRule integrateMissActionRule = new RayActionRule() { @Override public void doAction(Point3DReadOnly rayOrigin, Point3DReadOnly rayEnd, Vector3DReadOnly rayDirection, OcTreeKeyReadOnly key) { Pair<OcTreeKey, Float> keyAndMissUpdate = doRayActionOnFreeCell(rayOrigin, rayEnd, rayDirection, key, occupiedCells, keyToNodeMap); if (keyAndMissUpdate != null) keysAndMissUpdates.add(keyAndMissUpdate); } }; OcTreeRayTools.doActionOnRayKeys(sensorOrigin, rayEnd, boundingBox, integrateMissActionRule, resolution, treeDepth); return keysAndMissUpdates; }
private static Point3D travelAlongBodyPath(double distanceToTravel, Point3DReadOnly startingPosition, List<Point3DReadOnly> bodyPath) { Point3D newPosition = new Point3D(); for (int i = 0; i < bodyPath.size() - 1; i++) { LineSegment3D segment = new LineSegment3D(bodyPath.get(i), bodyPath.get(i + 1)); if (segment.distance(startingPosition) < 1.0e-4) { Vector3DBasics segmentDirection = segment.getDirection(true); newPosition.scaleAdd(distanceToTravel, segmentDirection, startingPosition); if (segment.distance(newPosition) < 1.0e-4) { return newPosition; } else { distanceToTravel -= startingPosition.distance(segment.getSecondEndpoint()); startingPosition = new Point3D(segment.getSecondEndpoint()); } } } return new Point3D(startingPosition); }
private static Point3D travelAlongBodyPath(double distanceToTravel, Point3D startingPosition, List<Point3DReadOnly> bodyPath) { Point3D newPosition = new Point3D(); for (int i = 0; i < bodyPath.size() - 1; i++) { LineSegment3D segment = new LineSegment3D(bodyPath.get(i), bodyPath.get(i + 1)); if (segment.distance(startingPosition) < 1.0e-4) { Vector3DBasics segmentDirection = segment.getDirection(true); newPosition.scaleAdd(distanceToTravel, segmentDirection, startingPosition); if (segment.distance(newPosition) < 1.0e-4) { return newPosition; } else { distanceToTravel -= startingPosition.distance(segment.getSecondEndpoint()); startingPosition = new Point3D(segment.getSecondEndpoint()); } } } return new Point3D(startingPosition); }
@Test public void testSignedDistanceFromPoint3DToPlane3D() throws Exception { Random random = new Random(1176L); for (int i = 0; i < ITERATIONS; i++) { Point3D pointOnPlane = EuclidCoreRandomTools.nextPoint3D(random); pointOnPlane.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Vector3D planeNormal = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); Vector3D parallelToPlane = EuclidCoreRandomTools.nextOrthogonalVector3D(random, planeNormal, true); Point3D secondPointOnPlane = new Point3D(); secondPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), parallelToPlane, pointOnPlane); double expectedDistance = EuclidCoreRandomTools.nextDouble(random, -10.0, 10.0); Point3D point = new Point3D(); point.scaleAdd(expectedDistance / planeNormal.length(), planeNormal, secondPointOnPlane); double actualDistance = EuclidGeometryTools.signedDistanceFromPoint3DToPlane3D(point, pointOnPlane, planeNormal); assertEquals(expectedDistance, actualDistance, EuclidGeometryTools.ONE_TRILLIONTH); } }
@Test public void testDistanceFromPoint3DToPlane3D() throws Exception { Random random = new Random(1176L); for (int i = 0; i < ITERATIONS; i++) { Point3D pointOnPlane = EuclidCoreRandomTools.nextPoint3D(random); pointOnPlane.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Vector3D planeNormal = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); Vector3D parallelToPlane = EuclidCoreRandomTools.nextOrthogonalVector3D(random, planeNormal, true); Point3D secondPointOnPlane = new Point3D(); secondPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), parallelToPlane, pointOnPlane); double expectedDistance = EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0); Point3D point = new Point3D(); double scalar = expectedDistance / planeNormal.length(); if (random.nextBoolean()) scalar = -scalar; point.scaleAdd(scalar, planeNormal, secondPointOnPlane); double actualDistance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point, pointOnPlane, planeNormal); assertEquals(expectedDistance, actualDistance, EuclidGeometryTools.ONE_TRILLIONTH); actualDistance = EuclidGeometryTools.distanceFromPoint3DToPlane3D(point.getX(), point.getY(), point.getZ(), pointOnPlane, planeNormal); assertEquals(expectedDistance, actualDistance, EuclidGeometryTools.ONE_TRILLIONTH); } }
@Test public void testNormal3DFromThreePoint3Ds() throws Exception { Random random = new Random(1176L); for (int i = 0; i < ITERATIONS; i++) { Vector3D expectedPlaneNormal = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); Point3D firstPointOnPlane = EuclidCoreRandomTools.nextPoint3D(random); firstPointOnPlane.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Point3D secondPointOnPlane = new Point3D(); Point3D thirdPointOnPlane = new Point3D(); Vector3D secondOrthogonalToNormal = EuclidCoreRandomTools.nextOrthogonalVector3D(random, expectedPlaneNormal, true); Vector3D thirdOrthogonalToNormal = EuclidCoreRandomTools.nextOrthogonalVector3D(random, expectedPlaneNormal, true); secondPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0), secondOrthogonalToNormal, firstPointOnPlane); thirdPointOnPlane.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 1.0, 10.0), thirdOrthogonalToNormal, firstPointOnPlane); Vector3D actualPlaneNormal = EuclidGeometryTools.normal3DFromThreePoint3Ds(firstPointOnPlane, secondPointOnPlane, thirdPointOnPlane); if (expectedPlaneNormal.dot(actualPlaneNormal) < 0.0) actualPlaneNormal.negate(); EuclidCoreTestTools.assertTuple3DEquals(expectedPlaneNormal, actualPlaneNormal, EPSILON); assertNull(EuclidGeometryTools.normal3DFromThreePoint3Ds(firstPointOnPlane, secondPointOnPlane, firstPointOnPlane)); } }
expectedIntersection.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), parallelToPlane, pointOnPlane); pointOnLine.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), lineDirection, expectedIntersection); pointOnLine.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), lineDirection, pointOnPlane); pointOnLine.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 1.0), planeNormal, pointOnLine); actualIntersection = EuclidGeometryTools.intersectionBetweenLine3DAndPlane3D(pointOnPlane, planeNormal, pointOnLine, lineDirection); assertNull(actualIntersection);
newEnd.scaleAdd(maxInsertRange, direction, origin); return integrateMissOnRay(origin, newEnd);
@Test public void testArePlane3DsCoincident() throws Exception { Random random = new Random(232L); for (int i = 0; i < ITERATIONS; i++) { Point3D pointOnPlane1 = EuclidCoreRandomTools.nextPoint3D(random); pointOnPlane1.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Vector3D planeNormal1 = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, 1.0); Point3D pointOnPlane2 = new Point3D(); Vector3D planeNormal2 = new Vector3D(); double distanceEpsilon = EuclidCoreRandomTools.nextDouble(random, 1.0); double distanceBetweenPlanes = EuclidCoreRandomTools.nextDouble(random, 1.0); pointOnPlane2.scaleAdd(distanceBetweenPlanes, planeNormal1, pointOnPlane1); Vector3D rotationAxis = EuclidCoreRandomTools.nextOrthogonalVector3D(random, planeNormal1, true); double angleEpsilon = EuclidCoreRandomTools.nextDouble(random, 0.0, Math.PI / 2.0); double rotationAngle = EuclidCoreRandomTools.nextDouble(random, 0.0, Math.PI / 2.0); AxisAngle rotationAxisAngle = new AxisAngle(rotationAxis, rotationAngle); RotationMatrix rotationMatrix = new RotationMatrix(); rotationMatrix.set(rotationAxisAngle); rotationMatrix.transform(planeNormal1, planeNormal2); boolean expectedCoincidentResult = Math.abs(distanceBetweenPlanes) < distanceEpsilon && rotationAngle < angleEpsilon; boolean actualCoincidentResult = EuclidGeometryTools.arePlane3DsCoincident(pointOnPlane1, planeNormal1, pointOnPlane2, planeNormal2, angleEpsilon, distanceEpsilon); assertEquals(expectedCoincidentResult, actualCoincidentResult); } }
@Test public void testOrthogonalProjectionOnLine3D() throws Exception { Random random = new Random(1176L); for (int i = 0; i < ITERATIONS; i++) { Point3D pointOnLine = EuclidCoreRandomTools.nextPoint3D(random); pointOnLine.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Vector3D lineDirection = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); Point3D expectedProjection = new Point3D(); expectedProjection.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), lineDirection, pointOnLine); Vector3D perpendicularToLineDirection = EuclidCoreRandomTools.nextOrthogonalVector3D(random, lineDirection, true); Point3D testPoint = new Point3D(); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), perpendicularToLineDirection, expectedProjection); Point3D actualProjection = new Point3D(); boolean success; success = EuclidGeometryTools.orthogonalProjectionOnLine3D(testPoint, pointOnLine, lineDirection, actualProjection); assertTrue(success); EuclidCoreTestTools.assertTuple3DEquals(expectedProjection, actualProjection, EPSILON); actualProjection = EuclidGeometryTools.orthogonalProjectionOnLine3D(testPoint, pointOnLine, lineDirection); EuclidCoreTestTools.assertTuple3DEquals(expectedProjection, actualProjection, EPSILON); lineDirection.normalize(); lineDirection.scale(0.9 * EuclidGeometryTools.ONE_TRILLIONTH); assertFalse(EuclidGeometryTools.orthogonalProjectionOnLine3D(testPoint, pointOnLine, lineDirection, actualProjection)); assertNull(EuclidGeometryTools.orthogonalProjectionOnLine3D(testPoint, pointOnLine, lineDirection)); } }
@Test public void testOrthogonalProjectionOnPlane3D() throws Exception { Random random = new Random(23423L); for (int i = 0; i < ITERATIONS; i++) { Point3D pointOnPlane = EuclidCoreRandomTools.nextPoint3D(random); pointOnPlane.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); Vector3D planeNormal = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0)); Vector3D parallelToPlane = EuclidCoreRandomTools.nextOrthogonalVector3D(random, planeNormal, true); Point3D expectedProjection = new Point3D(); expectedProjection.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), parallelToPlane, pointOnPlane); Point3D pointToProject = new Point3D(); double distanceOffPlane = EuclidCoreRandomTools.nextDouble(random, 10.0); pointToProject.scaleAdd(distanceOffPlane, planeNormal, expectedProjection); Point3D actualProjection = new Point3D(); boolean success; success = EuclidGeometryTools.orthogonalProjectionOnPlane3D(pointToProject, pointOnPlane, planeNormal, actualProjection); assertTrue(success); EuclidCoreTestTools.assertTuple3DEquals(expectedProjection, actualProjection, EPSILON); actualProjection = EuclidGeometryTools.orthogonalProjectionOnPlane3D(pointToProject, pointOnPlane, planeNormal); EuclidCoreTestTools.assertTuple3DEquals(expectedProjection, actualProjection, EPSILON); // Test failure case. planeNormal.normalize(); planeNormal.scale(0.9 * EuclidGeometryTools.ONE_TRILLIONTH); assertFalse(EuclidGeometryTools.orthogonalProjectionOnPlane3D(pointToProject, pointOnPlane, planeNormal, actualProjection)); assertNull(EuclidGeometryTools.orthogonalProjectionOnPlane3D(pointToProject, pointOnPlane, planeNormal)); } }
testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); expectedDistance = projection.distance(testPoint); actualDistance = EuclidGeometryTools.distanceFromPoint3DToLineSegment3D(testPoint, lineSegmentStart, lineSegmentEnd); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); projection.set(lineSegmentStart); expectedDistance = projection.distance(testPoint); testPoint.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 10.0), orthogonal, projection); projection.set(lineSegmentEnd); expectedDistance = projection.distance(testPoint);
public static ScanCollection createSingleSweepInPlane(Random random, double sensorDistanceFromPlane, Point3DReadOnly center, Vector3DBasics normal, double length, double width, int numberOfPoints) { ScanCollection sweepCollection = new ScanCollection(); normal.normalize(); RotationMatrix orientation = new RotationMatrix(EuclidGeometryTools.axisAngleFromZUpToVector3D(normal)); PointCloud pointCloud = new PointCloud(); for (int i = 0; i < numberOfPoints; i++) { Point3D pointInPlane = generateRandomPoint3D(random, length, width, 0.0); Point3D pointInWorld = new Point3D(); orientation.transform(pointInPlane, pointInWorld); pointInWorld.add(center); pointCloud.add(pointInWorld); } Point3D sensorOrigin = new Point3D(); sensorOrigin.scaleAdd(sensorDistanceFromPlane, normal, center); sweepCollection.addScan(pointCloud, sensorOrigin); return sweepCollection; } }
unfilteredPoint.scaleAdd(dt, EuclidCoreRandomTools.nextPoint3D(random), unfilteredPoint);
continue; intersection.scaleAdd(scaleFactor, edgeVector, edgeStart); intersection.setX(intersection.getX() * boxSize.getX()); intersection.setY(intersection.getY() * boxSize.getY());
finalPoint.scaleAdd(0.25, lastMessage0.getQuadrupedStepMessage().getGoalPosition(), finalPoint); finalPoint.scaleAdd(0.25, lastMessage1.getQuadrupedStepMessage().getGoalPosition(), finalPoint); finalPoint.scaleAdd(0.25, lastMessage2.getQuadrupedStepMessage().getGoalPosition(), finalPoint); finalPoint.scaleAdd(0.25, lastMessage3.getQuadrupedStepMessage().getGoalPosition(), finalPoint);
secondPointOnLine.scaleAdd(EuclidCoreRandomTools.nextDouble(random, 0.0, 10.0), expectedDirection, firstPointOnLine);