public Point3D getMeanPoint() { Point3D pointMean = new Point3D(); for(int i=0;i<points.size();i++) { pointMean.add(points.get(i)); } pointMean.scale(1.0/points.size()); return pointMean; } }
@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); } }
/** * Computes the average 3D point from a given collection of 3D points. * <p> * WARNING: This method generates garbage. * </p> * * @param points the collection of 3D points to compute the average from. Not modified. * @return the computed average. */ public static Point3D averagePoint3Ds(Collection<? extends Point3DReadOnly> points) { if (points.isEmpty()) return null; Point3D totalPoint = new Point3D(); for (Point3DReadOnly point : points) { totalPoint.add(point); } totalPoint.scale(1.0 / points.size()); return totalPoint; }
@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)); } }
/** * Returns the average of two 3D points. * <p> * WARNING: This method generates garbage. * </p> * * @param a the first 3D point. Not modified. * @param b the second 3D point. Not modified. * @return the computed average. */ public static Point3D averagePoint3Ds(Point3DReadOnly a, Point3DReadOnly b) { Point3D average = new Point3D(a); average.add(b); average.scale(0.5); return average; }
@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); } }
public Point3D getAveragePoint() { if (averagePoint == null) { averagePoint = new Point3D(Double.NaN, Double.NaN, Double.NaN); } if (Double.isNaN(averagePoint.getX())) { averagePoint.set(0.0, 0.0, 0.0); int numberOfPoints = points.size(); for (int i = 0; i < numberOfPoints; i++) { averagePoint.add(points.get(i)); } averagePoint.scale(1.0 / ((double) numberOfPoints)); } return averagePoint; }
private static Point3D getRandomConvexCombination(Random random, Point3D[] vertices) { double[] weightings = new double[vertices.length]; double sum = 0.0; for (int j = 0; j < weightings.length; j++) { sum += weightings[j] = random.nextDouble(); } for (int j = 0; j < weightings.length; j++) { weightings[j] /= sum; } Point3D pointToTest = new Point3D(); for (int j = 0; j < weightings.length; j++) { Point3D tempPoint = new Point3D(vertices[j]); tempPoint.scale(weightings[j]); pointToTest.add(tempPoint); } return pointToTest; } }
private Point3D computeTouchdownCoM() { int touchdownCount = 0; Point3D touchdownCoM = new Point3D(); Point3D touchdownLocation = new Point3D(); for (int i = 0; i < groundContactPointsToSlip.size(); i++) { GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i); boolean touchedDown = (groundContactPointToSlip.isInContact()); if (touchedDown) { groundContactPointToSlip.getTouchdownLocation(touchdownLocation); touchdownCoM.add(touchdownLocation); touchdownCount++; } } touchdownCoM.scale(1.0/touchdownCount); return touchdownCoM; }
public SearchNodeNeighborsVisualizer() { double nodeSize = OcTreeKeyConversionTools.computeNodeSize(depth, resolution, treeDepth); Point3D nodeCenter = new Point3D(nodeSize, nodeSize, nodeSize); nodeCenter.scale(0.5); OcTreeKey key = OcTreeKeyConversionTools.coordinateToKey(nodeCenter, depth, resolution, treeDepth); neighbors = OcTreeKeyTools.computeNeighborKeys(key, depth, resolution, treeDepth, searchRadius); }
private static Point3D getRandomConvexCombination(Random random, Point3D[] vertices) { double[] weightings = new double[vertices.length]; for (int j = 0; j < weightings.length; j++) { weightings[j] = random.nextDouble(); } double sum = MathTools.sum(weightings); for (int j = 0; j < weightings.length; j++) { weightings[j] /= sum; } Point3D pointToTest = new Point3D(); for (int j = 0; j < weightings.length; j++) { Point3D tempPoint = new Point3D(vertices[j]); tempPoint.scale(weightings[j]); pointToTest.add(tempPoint); } return pointToTest; }
private void computeClosestPointsOnAAndB(ExpandingPolytopeEntry closestTriangleToOrigin, Point3D closestPointOnA, Point3D closestPointOnB) { closestPointOnA.set(0.0, 0.0, 0.0); closestPointOnB.set(0.0, 0.0, 0.0); for (int i = 0; i < 3; i++) { Point3D vertex = closestTriangleToOrigin.getVertex(i); double lambda = closestTriangleToOrigin.getLambda(i); Point3D pointOnA = correspondingPointsOnA.get(vertex); Point3D pointOnB = correspondingPointsOnB.get(vertex); tempPoint.set(pointOnA); tempPoint.scale(lambda); closestPointOnA.add(tempPoint); tempPoint.set(pointOnB); tempPoint.scale(lambda); closestPointOnB.add(tempPoint); } }
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; }
@Test public void testDistanceSquaredBetweenPoint3Ds() 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.distanceSquared(secondPoint); double actualDistance = EuclidGeometryTools.distanceSquaredBetweenPoint3Ds(firstPoint.getX(), firstPoint.getY(), firstPoint.getZ(), secondPoint.getX(), secondPoint.getY(), secondPoint.getZ()); assertEquals(expectedDistance, actualDistance, EPSILON); actualDistance = EuclidGeometryTools.distanceSquaredBetweenPoint3Ds(firstPoint.getX(), firstPoint.getY(), firstPoint.getZ(), secondPoint); assertEquals(expectedDistance, actualDistance, EPSILON); } }
@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); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testGeneralCase() { 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); 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()); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testDegenerateCase() { 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 = new Point3D(point1); 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()); } }
@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()); } }
@ContinuousIntegrationTest(estimatedDuration = 0.1) @Test(timeout = 30000) public void testXCase() { 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.setX(point1.getX()); 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()); } }
private Point3D getMidFootPoint() { RigidBodyTransform temp = new RigidBodyTransform(); Point3D left = new Point3D(); Point3D avg = new Point3D(); fullRobotModel.getFoot(RobotSide.LEFT).getBodyFixedFrame().getTransformToDesiredFrame(temp, ReferenceFrame.getWorldFrame()); temp.transform(left); fullRobotModel.getFoot(RobotSide.RIGHT).getBodyFixedFrame().getTransformToDesiredFrame(temp, ReferenceFrame.getWorldFrame()); temp.transform(avg); avg.add(left); avg.scale(0.5); return avg; }