@Override protected void setY(Point3D data, double y) { data.setY(y); }
public void setMinY(double yMin) { minCoordinate.setY(yMin); minCoordinateDirtyBit = false; minKeyDirtyBit = true; }
public void setMaxY(double yMax) { maxCoordinate.setY(yMax); maxCoordinateDirtyBit = false; maxKeyDirtyBit = true; }
/** * Sets the y-coordinate of this point. * * @param y the y-coordinate. */ @Override public void setY(double y) { point.setY(y); }
public void setTrackingY(double cameraTrackingY) { this.trackingPosition.setY(cameraTrackingY); }
public void setDollyY(double cameraDollyY) { this.dollyPosition.setY(cameraDollyY); }
private ArrayList<FootstepDataMessage> createFootstepList(RobotSide robotSide, boolean isDirectionForward) { ArrayList<FootstepDataMessage> listOfSteps = new ArrayList<>(8); for (int i = 0; i < listOfSteps.size(); i++) { double y = robotSide == RobotSide.LEFT ? 0.15 : -0.15; Point3D position = new Point3D(); position.setY(y); if (isDirectionForward) { position.setX(i * 0.2 + 0.2); } else { double startingPoint = 8 * 0.2; position.setX(startingPoint -i * 0.2 - 0.2); } FootstepDataMessage footStep = HumanoidMessageTools.createFootstepDataMessage(robotSide, position, new Quaternion(0.0, 0.0, 0.0, 1.0)); listOfSteps.set(i, footStep); robotSide = robotSide.getOppositeSide(); } return listOfSteps; }
@Test public void testHashCode() throws Exception { Random random = new Random(621541L); Point3D tuple1 = createEmptyTuple(); tuple1.setX(random.nextDouble()); tuple1.setY(random.nextDouble()); tuple1.setZ(random.nextDouble()); int newHashCode, previousHashCode; newHashCode = tuple1.hashCode(); assertEquals(newHashCode, tuple1.hashCode()); previousHashCode = tuple1.hashCode(); for (int i = 0; i < ITERATIONS; i++) { tuple1.setElement(i % 3, random.nextDouble()); newHashCode = tuple1.hashCode(); assertNotEquals(newHashCode, previousHashCode); previousHashCode = newHashCode; } }
@Override public boolean checkIfInside(double x, double y, double z, Point3D intersectionToPack, Vector3D normalToPack) { tempPoint3dForCheckInside.setX(x); tempPoint3dForCheckInside.setY(y); tempPoint3dForCheckInside.setZ(z); boolean isPointInside = planarRegion.isPointOnOrSlightlyBelow(tempPoint3dForCheckInside, allowablePenetrationThickness); if (isPointInside) { if (intersectionToPack != null) { intersectionToPack.set(tempPoint3dForCheckInside); } if (normalToPack != null) { planarRegion.getNormal(normalToPack); } } return isPointInside; }
public static <NODE extends AbstractOcTreeNode<NODE>> Point3D computeMinCoordinate(NODE root, double resolution, int treeDepth) { // empty tree if (root == null) return new Point3D(); Point3D min = new Point3D(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); for (NODE node : OcTreeIteratorFactory.createLeafIterable(root)) { double size = node.getSize(); double halfSize = size / 2.0; double x = node.getX() - halfSize; double y = node.getY() - halfSize; double z = node.getZ() - halfSize; min.setX(Math.min(x, min.getX())); min.setY(Math.min(y, min.getY())); min.setZ(Math.min(z, min.getZ())); } return min; }
public static <NODE extends AbstractOcTreeNode<NODE>> Point3D computeMaxCoordinate(NODE root, double resolution, int treeDepth) { // empty tree if (root == null) return new Point3D(); Point3D max = new Point3D(Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY); for (NODE node : OcTreeIteratorFactory.createLeafIterable(root)) { double size = node.getSize(); double halfSize = size / 2.0; double x = node.getX() + halfSize; double y = node.getY() + halfSize; double z = node.getZ() + halfSize; max.setX(Math.max(x, max.getX())); max.setY(Math.max(y, max.getY())); max.setZ(Math.max(z, max.getZ())); } return max; }
/** * Computes the distance of the point to the region projected onto the world xy-plane. * * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return distance to this region. If 0.0, point is in the region. */ public double distanceToPointByProjectionOntoXYPlane(double x, double y) { localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); localPoint2D.set(localPoint); return distanceToPoint(localPoint2D); }
private void updateBoundingBoxWithNewNode(NormalOcTreeNode newNode) { double nodeX = newNode.getX(); if (nodeX < min.getX()) min.setX(nodeX); else if (nodeX > max.getX()) max.setX(nodeX); double nodeY = newNode.getY(); if (nodeY < min.getY()) min.setY(nodeY); else if (nodeY > max.getY()) max.setY(nodeY); double nodeZ = newNode.getZ(); if (nodeZ < min.getZ()) min.setZ(nodeZ); else if (nodeZ > max.getZ()) max.setZ(nodeZ); }
@Override public void handle(MouseEvent event) { PickResult pickResult = event.getPickResult(); Node intersectedNode = pickResult.getIntersectedNode(); if (intersectedNode == null || intersectedNode instanceof SubScene) return; javafx.geometry.Point3D localPoint = pickResult.getIntersectedPoint(); javafx.geometry.Point3D scenePoint = intersectedNode.getLocalToSceneTransform().transform(localPoint); Point3D interception = new Point3D(); interception.setX(scenePoint.getX()); interception.setY(scenePoint.getY()); interception.setZ(scenePoint.getZ()); latestInterception.set(interception); } };
@Override public void handle(MouseEvent event) { PickResult pickResult = event.getPickResult(); Node intersectedNode = pickResult.getIntersectedNode(); if (intersectedNode == null || intersectedNode instanceof SubScene) return; javafx.geometry.Point3D localPoint = pickResult.getIntersectedPoint(); javafx.geometry.Point3D scenePoint = intersectedNode.getLocalToSceneTransform().transform(localPoint); Point3D interception = new Point3D(); interception.setX(scenePoint.getX()); interception.setY(scenePoint.getY()); interception.setZ(scenePoint.getZ()); latestInterception.set(interception); } };
/** * Computes if the point is in the region projected onto the world xy-plane. * * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInsideByProjectionOntoXYPlane(double x, double y) { Point3D localPoint = new Point3D(); localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); return isPointInside(localPoint.getX(), localPoint.getY()); }
private void updateBoundingBoxAfterRemovingNode(NormalOcTreeNode removedNode) { if (nodes.isEmpty()) { min.set(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); max.set(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY); } double nodeX = removedNode.getX(); double nodeY = removedNode.getY(); double nodeZ = removedNode.getZ(); double epsilon = 1.0e-3; if (Math.abs(nodeX - min.getX()) < epsilon) min.setX(findMin((node1, node2) -> node1.getX() < node2.getX() ? -1 : 1).getX()); else if (Math.abs(nodeX - max.getX()) < epsilon) max.setX(findMax((node1, node2) -> node1.getX() < node2.getX() ? -1 : 1).getX()); if (Math.abs(nodeY - min.getY()) < epsilon) min.setY(findMin((node1, node2) -> node1.getY() < node2.getY() ? -1 : 1).getY()); else if (Math.abs(nodeY - max.getY()) < epsilon) max.setY(findMax((node1, node2) -> node1.getY() < node2.getY() ? -1 : 1).getY()); if (Math.abs(nodeZ - min.getZ()) < epsilon) min.setZ(findMin((node1, node2) -> node1.getZ() < node2.getZ() ? -1 : 1).getZ()); else if (Math.abs(nodeZ - max.getZ()) < epsilon) max.setZ(findMax((node1, node2) -> node1.getZ() < node2.getZ() ? -1 : 1).getZ()); }
private static void packExtrapolatedPoint(Vector3DReadOnly from, Vector3DReadOnly to, double ratio, Point3D toPack) { toPack.setX(ratio * (to.getX() - from.getX()) + from.getX()); toPack.setY(ratio * (to.getY() - from.getY()) + from.getY()); toPack.setZ(ratio * (to.getZ() - from.getZ()) + from.getZ()); }
public static Point3D toPointInWorld(double xToTransform, double yToTransform, Point3D planeOrigin, Quaternion planeOrientation) { Point3D pointInWorld = new Point3D(); double qx = planeOrientation.getX(); double qy = planeOrientation.getY(); double qz = planeOrientation.getZ(); double qs = planeOrientation.getS(); // t = 2.0 * cross(q.xyz, v); // v' = v + q.s * t + cross(q.xyz, t); double x = xToTransform; double y = yToTransform; double z = 0.0; double crossX = 2.0 * (qy * z - qz * y); double crossY = 2.0 * (qz * x - qx * z); double crossZ = 2.0 * (qx * y - qy * x); double crossCrossX = qy * crossZ - qz * crossY; double crossCrossY = qz * crossX - qx * crossZ; double crossCrossZ = qx * crossY - qy * crossX; pointInWorld.setX(x + qs * crossX + crossCrossX); pointInWorld.setY(y + qs * crossY + crossCrossY); pointInWorld.setZ(z + qs * crossZ + crossCrossZ); pointInWorld.add(planeOrigin); return pointInWorld; }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testYCase() { Random gen = new Random(124L); for (int i = 0; i < 100000; i++) { Point3D point1 = RandomGeometry.nextPoint3D(gen, -1000, -1000, -1000, 1000, 1000, 1000); Point3D point2 = RandomGeometry.nextPoint3D(gen, -1000, -1000, -1000, 1000, 1000, 1000); point2.setY(point1.getY()); double s = RandomNumbers.nextDouble(gen, 0, 1); Point3D temp = new Point3D(point1); temp.scale(1 - s); Point3D point3 = new Point3D(temp); temp.set(point2); temp.scale(s); point3.add(temp); runTest(point1.getX(), point1.getY(), point1.getZ(), point2.getX(), point2.getY(), point2.getZ(), point3.getX(), point3.getY(), point3.getZ()); } }