/** * Gets the maximum x-coordinate of this bounding box. * * @return the maximum x-coordinate. */ public double getMaxX() { return maxPoint.getX(); }
public double getMaxX() { if (maxCoordinateDirtyBit) throw new RuntimeException("The bounding box max coordinate is not up to date."); return maxCoordinate.getX(); }
public static float[] toPointCloudFloatArrayInYUp(List<Point3D> points) { float[] ret = new float[points.size() * 3]; for (int i = 0; i < points.size(); i++) { Point3D point3d = points.get(i); ret[i * 3 + 0] = (float) point3d.getY(); ret[i * 3 + 1] = (float) point3d.getZ(); ret[i * 3 + 2] = (float) point3d.getX(); } return ret; }
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)); } }
private void addRandomPointsToQuadTree(Random random, int numberOfPoints, Box bounds, double minZ, double maxZ, QuadTreeForGround quadTree) { for (int i = 0; i < numberOfPoints; i++) { Point3D point = RandomGeometry.nextPoint3D(random, bounds.minX, bounds.minY, minZ, bounds.maxX, bounds.maxY, maxZ); quadTree.put(point.getX(), point.getY(), point.getZ()); } }
public void assertPointsLieOnHeightMap(ArrayList<Point3D> points) { if (DO_ASSERTS) { for (Point3D point : points) { double heightMapZ = heightMap.getHeightAtPoint(point.getX(), point.getY()); assertEquals(point.getZ(), heightMapZ, 1e-7); } } }
private Vector3D generateRandomVector3d(Random random, Point3D minValues, Point3D maxValues) { double x = generateRandomDoubleBetween(random, minValues.getX(), maxValues.getX()); double y = generateRandomDoubleBetween(random, minValues.getY(), maxValues.getY()); double z = generateRandomDoubleBetween(random, minValues.getZ(), maxValues.getZ()); return new Vector3D(x, y, z); }
public void assertPointsLieOnHeightMap(ArrayList<Point3D> points) { if (DO_ASSERTS) { for (Point3D point : points) { double heightMapZ = heightMap.getHeightAtPoint(point.getX(), point.getY()); assertEquals(point.getZ(), heightMapZ, 1e-7); } } }
private void setGoalPosition(Point3D position) { goalSphere.setTranslateX(position.getX()); goalSphere.setTranslateY(position.getY()); goalSphere.setTranslateZ(position.getZ()); }
private void setStartPosition(Point3D position) { startSphere.setTranslateX(position.getX()); startSphere.setTranslateY(position.getY()); startSphere.setTranslateZ(position.getZ()); }
private void setGoalPosition(Point3D position) { goalSphere.setTranslateX(position.getX()); goalSphere.setTranslateY(position.getY()); goalSphere.setTranslateZ(position.getZ()); }
public void startSimulation() { SimulationConstructionSetParameters simulationParameters = new SimulationConstructionSetParameters(true, 16000); SimulationConstructionSet scs = new SimulationConstructionSet(simulatedRobot, simulationParameters); scs.setDT(robotModel.getSimulateDT(), 10); scs.setCameraPosition(scsCameraPosition.getX(), scsCameraPosition.getY(), scsCameraPosition.getZ()); scs.setCameraFix(scsCameraFix.getX(), scsCameraFix.getY(), scsCameraFix.getZ()); scs.startOnAThread(); }
@Override protected void setBehaviorInput() { FramePoint3D point = new FramePoint3D(ReferenceFrame.getWorldFrame(), initialSphereDetectionBehavior.getBallLocation().getX(), initialSphereDetectionBehavior.getBallLocation().getY(), initialSphereDetectionBehavior.getBallLocation().getZ() + initialSphereDetectionBehavior.getSpehereRadius()); wholeBodyBehavior.setSolutionQualityThreshold(2.01); wholeBodyBehavior.setTrajectoryTime(3); FrameQuaternion tmpOr = new FrameQuaternion(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
@Test public void testOrthogonalProjectionBottom() { double height = 2.0; double radius = 1.0; Cylinder3D cylinder3d = new Cylinder3D(height, radius); Point3D pointToCheckAndPack = new Point3D(0.5, 0.25, -height); Point3D expectedProjection = new Point3D(pointToCheckAndPack.getX(), pointToCheckAndPack.getY(), -1.0); cylinder3d.orthogonalProjection(pointToCheckAndPack); assertPointEquals(expectedProjection, pointToCheckAndPack); }
private void addBox(double size, Point3D center, Group root, Color color) { Box box = new Box(size, size, size); box.setTranslateX(center.getX()); box.setTranslateY(center.getY()); box.setTranslateZ(center.getZ()); PhongMaterial material = new PhongMaterial(); material.setDiffuseColor(color); box.setMaterial(material); root.getChildren().add(box); }
public boolean hasMoved(Point3D position) { xyPoint.setIncludingFrame(WORLD_FRAME, position.getX(), position.getY()); double prevZ = previousPlane.getZOnPlane(xyPoint); p1.setIncludingFrame(WORLD_FRAME, position); p1.changeFrame(planeFrame); xyPoint.setToNaN(planeFrame); xyPoint.setIncludingFrame(p1); double currentZ = plane.getZOnPlane(xyPoint); return currentZ - prevZ != 0; }
@Test public void testSetSecondEndpoint() throws Exception { Random random = new Random(324324L); Point3D expectedSecondEndpoint = EuclidCoreRandomTools.nextPoint3D(random, 10.0); LineSegment3D lineSegment3D = new LineSegment3D(); lineSegment3D.setSecondEndpoint(expectedSecondEndpoint); EuclidCoreTestTools.assertTuple3DEquals(expectedSecondEndpoint, lineSegment3D.getSecondEndpoint(), EPSILON); expectedSecondEndpoint = EuclidCoreRandomTools.nextPoint3D(random, 10.0); lineSegment3D.setSecondEndpoint(expectedSecondEndpoint.getX(), expectedSecondEndpoint.getY(), expectedSecondEndpoint.getZ()); EuclidCoreTestTools.assertTuple3DEquals(expectedSecondEndpoint, lineSegment3D.getSecondEndpoint(), EPSILON); }
public String getID() { String id = ""; id = RobotSide.fromByte(footstepData.getRobotSide()).getSideNameFirstLetter() + "_" + discretize(footstepData.getLocation().getX(), distanceResolution) + "_" + discretize(footstepData.getLocation().getY(), distanceResolution) + "_" + discretize(theta, thetaResolution); return id; } }
@Override public double heightAt(double x, double y, double z) { if (convexPolygon.isPointInside(x, y, EPSILON)) { topPlane.getPoint(tempPlaneCentroid); topPlane.getNormal(tempPlaneNormal); return tempPlaneCentroid.getZ() - (tempPlaneNormal.getX() * (x - tempPlaneCentroid.getX()) + tempPlaneNormal.getY() * (y - tempPlaneCentroid.getY())) / tempPlaneNormal.getZ(); } return boundingBox.getMinZ(); }