private boolean pointInRange(Point3D point, Point3D sensorOrigin) { double dist = sensorOrigin.distance(point); return (dist > parameters.minRange) && (dist < parameters.maxRange); }
protected boolean includePoint(Point3D point, float intensity) { return point.distance(origin) < 1.0; }
public double positionDistance(EuclideanWaypoint euclideanWaypoint) { return position.distance(euclideanWaypoint.position); }
protected boolean includePoint(Point3D point, Color color) { Color.RGBtoHSB(color.getRed(), color.getGreen(), color.getBlue(), hsbvals); return point.distance(origin) < 1.0; // return ((point.distance(origin) < 1.0) && (Math.abs(hsbvals[0] - pinkHue) < 0.01)); }
public double distance(FootstepPlanState other) { return footstepData.getLocation().distance(other.footstepData.getLocation()); }
/** * Finds and returns the closest point the the provided point on the planar regions. */ public static Point3D projectPointToPlanes(Point3DReadOnly point, PlanarRegionsList regions) { double smallestDistance = Double.POSITIVE_INFINITY; Point3D closestPoint = null; for (PlanarRegion region : regions.getPlanarRegionsAsList()) { Point3D intersection = closestPointOnPlane(point, region); double distance = intersection.distance(point); if (closestPoint == null || distance < smallestDistance) { smallestDistance = distance; closestPoint = intersection; } } return closestPoint; }
public Point3D[] getTheTwoPointsFurthestApart() { double maxDist = 0.0; Point3D pointA = new Point3D(); Point3D pointB = new Point3D(); for (Point3D observer : points) { for (Point3D target : points) { double distance = observer.distance(target); if (distance > maxDist) { maxDist = distance; pointA = observer; pointB = target; } } } return new Point3D[] {pointA, pointB}; } }
public static Point3D intersectRegionsWithRay(PlanarRegionsList regions, Point3D rayStart, Vector3D rayDirection) { double smallestDistance = Double.POSITIVE_INFINITY; Point3D closestIntersection = null; for (PlanarRegion region : regions.getPlanarRegionsAsList()) { Point3D intersection = intersectRegionWithRay(region, rayStart, rayDirection); if (intersection == null) { continue; } double distance = intersection.distance(rayStart); if (distance < smallestDistance) { smallestDistance = distance; closestIntersection = intersection; } } return closestIntersection; }
public void checkRepInvarients(Box bounds) { if ((points == null) || (points.isEmpty())) { throw new RuntimeException("All leaves should have at least one point"); } if ((points != null) && (!points.isEmpty())) { for (Point3D point : points) { if (!bounds.containsOrEquals(point.getX(), point.getY())) throw new RuntimeException(); } } if ((averagePoint != null) && (Double.isNaN(averagePoint.getX()))) { Point3D averageCheck = new Point3D(averagePoint); averagePoint.set(Double.NaN, Double.NaN, Double.NaN); if (averageCheck.distance(this.getAveragePoint()) > 1e-7) throw new RuntimeException(); } }
@Override public boolean isPossibleGround(Point3D point, Point3D lidarOrigin) { Point3D footAvg = getMidFootPoint(); double footZ = footAvg.getZ(); footAvg.setZ(point.getZ()); double maxHeight = parameters.quadTreeZAboveFeet + point.distance(footAvg) * parameters.quadTreeZSlope; if (maxHeight > parameters.quadTreeZMax) { maxHeight = parameters.quadTreeZMax; } return (point.getZ() - footZ) < maxHeight; }
public CollisionModelCapsule(SimpleCollisionShapeFactory shapeFactory, OneDoFJointBasics currentJoint, OneDoFJointBasics nextJoint, double radius) { this.currentJoint = currentJoint; this.nextJoint = nextJoint; this.shapeFactory = shapeFactory; this.transform = new RigidBodyTransform(); this.graphicObject = new Graphics3DObject(); updateRighdBodyTransform(); this.radius = radius; this.height = currentLocation.distance(nextLocation) + radius * 2; create(); updateCollisionShape(); }
protected boolean isValidNearScan(Point3D point, Point3D lidarOrigin) { boolean valid = true; valid &= point.getZ() < lidarOrigin.getZ() + parameters.nearScanZMaxAboveHead; Point3D center = new Point3D(lidarOrigin.getX(), lidarOrigin.getY(), point.getZ()); valid &= point.distance(center) < parameters.nearScanRadius; return valid; }
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); }
public void changeCameraPosition(double x, double y, double z) { Point3D desiredCameraPosition = new Point3D(x, y, z); Point3D desiredFocusPoint = new Point3D(focusPointTranslation.getX(), focusPointTranslation.getY(), focusPointTranslation.getZ()); double distanceFromFocusPoint = desiredCameraPosition.distance(desiredFocusPoint); offsetFromFocusPoint.setZ(-distanceFromFocusPoint); rotationCalculator.setRotationFromCameraAndFocusPositions(desiredCameraPosition, desiredFocusPoint, 0.0); }
private Point3D computeFinalPosition(Point3D initialPosition, double ballVelocityMagnitude) { tempPoint.set(ballTarget.getX(), ballTarget.getY(), ballTarget.getZ()); double distance = initialPosition.distance(tempPoint); double estimatedCollisionTime = distance / ballVelocityMagnitude; Point3D ret = new Point3D(ballTargetVelocity); ret.scale(estimatedCollisionTime); ret.add(tempPoint); return ret; }
public static double getDistance(RigidBodyTransform from, RigidBodyTransform to, double positionWeight, double orientationWeight) { Point3D pointFrom = new Point3D(from.getTranslationVector()); Quaternion orientationFrom = new Quaternion(from.getRotationMatrix()); Point3D pointTo = new Point3D(to.getTranslationVector()); Quaternion orientationTo = new Quaternion(to.getRotationMatrix()); double positionDistance = positionWeight * pointFrom.distance(pointTo); double orientationDistance = orientationWeight * orientationFrom.distance(orientationTo); double distance = positionDistance + orientationDistance; return distance; }
private static void doBrakeDown3D(ArrayList<Point3D> points, int index, Point3D point1, Point3D point2) { if (point2.distance(point1) > 0.2) { Point3D newPointToAdd = new Point3D(point1.getX() + ((point2.getX() - point1.getX()) / 2.0), point1.getY() + ((point2.getY() - point1.getY()) / 2.0), point1.getZ() + ((point2.getZ() - point1.getZ()) / 2.0)); points.add(index, new Point3D(newPointToAdd.getX(), newPointToAdd.getY(), 0)); doBrakeDown3D(points, index, point1, new Point3D(newPointToAdd.getX(), newPointToAdd.getY(), 0)); } }
@Test public void testDistanceBetweenPoint3Ds() throws Exception { Random random = new Random(23423L); for (int i = 0; i < ITERATIONS; i++) { Point3D firstPoint = EuclidCoreRandomTools.nextPoint3D(random); Point3D secondPoint = EuclidCoreRandomTools.nextPoint3D(random); firstPoint.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); secondPoint.scale(EuclidCoreRandomTools.nextDouble(random, 10.0)); double expectedDistance = firstPoint.distance(secondPoint); double actualDistance = EuclidGeometryTools.distanceBetweenPoint3Ds(firstPoint.getX(), firstPoint.getY(), firstPoint.getZ(), secondPoint.getX(), secondPoint.getY(), secondPoint.getZ()); assertEquals(expectedDistance, actualDistance, EPSILON); actualDistance = EuclidGeometryTools.distanceBetweenPoint3Ds(firstPoint.getX(), firstPoint.getY(), firstPoint.getZ(), secondPoint); assertEquals(expectedDistance, actualDistance, EPSILON); } }
@Test public void testOrthogonalProjection() { Point3D center = new Point3D(1.0, 2.1, 3.2); double radius = 0.7634; Sphere3D sphere3d = new Sphere3D(center, radius); Point3D randomPoint = new Point3D(17.3, 19.2, 11.4); Point3D orthogonalProjection = new Point3D(randomPoint); sphere3d.orthogonalProjection(orthogonalProjection); assertEquals(radius, orthogonalProjection.distance(center), 1e-7); Vector3D vector1 = new Vector3D(randomPoint); vector1.sub(center); Vector3D vector2 = new Vector3D(orthogonalProjection); vector2.sub(center); vector1.normalize(); vector2.normalize(); EuclidCoreTestTools.assertTuple3DEquals(vector1, vector2, 1e-7); }
private Node createBorderEdgesGraphics(PlanarRegionSegmentationRawData rawData, ConcaveHullFactoryResult concaveHullFactoryResult) { int regionId = rawData.getRegionId(); JavaFXMultiColorMeshBuilder meshBuilder = new JavaFXMultiColorMeshBuilder(new TextureColorAdaptivePalette(16)); Point3D planeOrigin = rawData.getOrigin(); Quaternion planeOrientation = rawData.getOrientation(); Color regionColor = OcTreeMeshBuilder.getRegionColor(regionId); for (ConcaveHullVariables intermediateVariables : concaveHullFactoryResult.getIntermediateVariables()) { Set<QuadEdge> borderEdges = intermediateVariables.getBorderEdges(); for (QuadEdge edge : borderEdges) { Point3D dest = PolygonizerTools.toPointInWorld(edge.dest().getX(), edge.dest().getY(), planeOrigin, planeOrientation); Point3D orig = PolygonizerTools.toPointInWorld(edge.orig().getX(), edge.orig().getY(), planeOrigin, planeOrientation); boolean isEdgeTooLong = dest.distance(orig) > parameters.getEdgeLengthThreshold(); Color lineColor = Color.hsb(regionColor.getHue(), regionColor.getSaturation(), isEdgeTooLong ? 0.25 : regionColor.getBrightness()); meshBuilder.addLine(dest, orig, 0.0015, lineColor); } } MeshView meshView = new MeshView(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); meshView.setMouseTransparent(true); return meshView; }