/** * Redefines this bounding box given its {@code center} location and half its size along each axis * {@code halfSize}. * * @param center the new center location of this bounding box. Not modified. * @param halfSize half the size of this bounding box. Not modified. */ public void set(Point3DReadOnly center, Vector3DReadOnly halfSize) { minPoint.sub(center, halfSize); maxPoint.add(center, halfSize); checkBounds(); }
private static List<Point3D> createRandomDataset(Random random, Point3D average, int length, Vector3D maxAmplitude) { List<Point3D> dataset = new ArrayList<>(length); Point3D min = new Point3D(); Point3D max = new Point3D(); min.sub(average, maxAmplitude); max.add(average, maxAmplitude); for (int i = 0; i < length; i++) dataset.add(RandomGeometry.nextPoint3D(random, min, max)); return dataset; }
public void computeSupportPointOnMinkowskiDifference(ConvexPolytope cubeOne, ConvexPolytope cubeTwo, Vector3D supportDirection, Point3D supportPoint) { // Because everything is linear and convex, the support point on the Minkowski difference is s_{a minkowskidiff b}(d) = s_a(d) - s_b(-d) Point3D supportingVertexOne = cubeOne.getSupportingVertex(supportDirection); negativeSupportDirection.set(supportDirection); negativeSupportDirection.scale(-1.0); Point3D supportingVertexTwo = cubeTwo.getSupportingVertex(negativeSupportDirection); supportPoint.set(supportingVertexOne); supportPoint.sub(supportingVertexTwo); }
private Point3D getRandomPositionInSphere(Random random, RobotSide robotSide) { Point3D circleCenterFromAnkle = getCircleCenterFromAnkle(robotSide); Vector3D circleRadius = getCircleRadius(); Point3D min = new Point3D(); Point3D max = new Point3D(); min.sub(circleCenterFromAnkle, circleRadius); max.add(circleCenterFromAnkle, circleRadius); return RandomGeometry.nextPoint3D(random, min, max); }
for (Point3D point : projectedPoints) point.sub(averagePoint); rotationMatrix.transform(point); if (Math.abs(point.getZ()) > 1e-14)
private void applyRotationalSlip(double percentOfDelta) { FrameQuaternion identity = new FrameQuaternion(ReferenceFrame.getWorldFrame()); FrameQuaternion desired = slipRotation.getFrameOrientationCopy(); FrameQuaternion delta = new FrameQuaternion(); delta.interpolate(identity, desired, percentOfDelta); desired.interpolate(identity, desired, 1.0-percentOfDelta); slipRotation.set(desired); Point3D touchdownCoM = computeTouchdownCoM(); RotationMatrix deltaRotation = new RotationMatrix(delta); 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); touchdownLocation.sub(touchdownCoM); deltaRotation.transform(touchdownLocation); touchdownLocation.add(touchdownCoM); groundContactPointToSlip.setTouchdownLocation(touchdownLocation); } } }
tempPoint.sub(footstepData.getLocation()); rotationMatrix.transform(tempPoint);
tempPoint.sub(fromPoint); tempPoint.sub(exitFootCMP); tempPoint.scale(3*Math.pow((1.0 - tPh2), 2)); comVelo.set(tempPoint); tempPoint.sub(entryFootCMP); tempPoint.scale(6*(1.0 - tPh2)*tPh2); comVelo.add(tempPoint); tempPoint.sub(supportPoint); tempPoint.sub(exitFootCMP); tempPoint.scale(3*Math.pow(tPh2, 2)); comVelo.add(tempPoint);
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testIsPointOn() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointOnOrSlightlyAbove(pointAbove, 1e-5)); assertFalse(planarRegion.isPointOnOrSlightlyAbove(pointBelow, 1e-5)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testIsPointInWorld2DInside() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointInWorld2DInside(pointAbove)); assertTrue(planarRegion.isPointInWorld2DInside(pointBelow)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testIsPointOnOrSlightlyBelow() { Random random = new Random(1776L); RigidBodyTransform transformToWorld = new RigidBodyTransform(); Point3D regionTranslation = new Point3D(); Point3D pointAbove = new Point3D(); Point3D pointBelow = new Point3D(); Vector3D regionNormal = new Vector3D(); for (int i = 0; i < 10000; i++) { PlanarRegion planarRegion = PlanarRegion.generatePlanarRegionFromRandomPolygonsWithRandomTransform(random, 1, 10.0, 5); planarRegion.getTransformToWorld(transformToWorld); Point2DReadOnly centroid = planarRegion.getLastConvexPolygon().getCentroid(); regionTranslation.set(centroid.getX(), centroid.getY(), 0.0); transformToWorld.transform(regionTranslation); regionTranslation.setZ(planarRegion.getPlaneZGivenXY(regionTranslation.getX(), regionTranslation.getY())); planarRegion.getNormal(regionNormal); regionNormal.normalize(); regionNormal.scale(1e-6); pointAbove.add(regionTranslation, regionNormal); pointBelow.sub(regionTranslation, regionNormal); assertTrue(planarRegion.isPointOnOrSlightlyBelow(pointBelow, 1e-5)); assertFalse(planarRegion.isPointOnOrSlightlyBelow(pointAbove, 1e-5)); } }
pointOnRamp.sub(normalToPack);
minkowskiDifferenceVertex.sub(vertexOne, vertexTwo);
Point3D min = new Point3D(rootJointPosition); Point3D max = new Point3D(rootJointPosition); min.sub(epsilon); max.add(epsilon);
Point3D min = new Point3D(rootJointPosition); Point3D max = new Point3D(rootJointPosition); min.sub(epsilon); max.add(epsilon); drcSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(new BoundingBox3D(min, max));