private void corruptLocationVector(Point3D location) { Vector3D randomVector = RandomGeometry.nextVector3D(random, minLocationCorruption, maxLocationCorruption); location.add(randomVector); }
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; } }
public void setFootStepCheckPoints(ArrayList<Point3D> locations, double xRange, double yRange) { footCheckPointFlag = new ArrayList<>(locations.size()); footCheckPoint = new ArrayList<>(locations.size()); for (int i = 0; i < locations.size(); i++) { Point3D minBound = new Point3D(locations.get(i)); minBound.add(-xRange / 2.0, -yRange / 2.0, -10.0); Point3D maxBound = new Point3D(locations.get(i)); maxBound.add(xRange / 2.0, yRange / 2.0, 10.0); footCheckPoint.add(new BoundingBox3D(minBound, maxBound)); YoBoolean newFlag = new YoBoolean("FootstepCheckPointFlag" + Integer.toString(i), circleWalkRegistry); footCheckPointFlag.add(newFlag); } }
public void setFootStepCheckPoints(ArrayList<Point3D> locations, double xRange, double yRange) { footCheckPointFlag = new ArrayList<>(locations.size()); footCheckPoint = new ArrayList<>(locations.size()); for (int i = 0; i < locations.size(); i++) { Point3D minBound = new Point3D(locations.get(i)); minBound.add(-xRange / 2.0, -yRange / 2.0, -10.0); Point3D maxBound = new Point3D(locations.get(i)); maxBound.add(xRange / 2.0, yRange / 2.0, 10.0); footCheckPoint.add(new BoundingBox3D(minBound, maxBound)); YoBoolean newFlag = new YoBoolean("FootstepCheckPointFlag" + Integer.toString(i), circleWalkRegistry); footCheckPointFlag.add(newFlag); } }
private static Pose3D computeCircleTrajectory(double time, double trajectoryTime, double circleRadius, Point3DReadOnly circleCenter, Quaternion circleRotation, QuaternionReadOnly constantOrientation, boolean ccw, double phase) { double theta = (ccw ? -time : time) / trajectoryTime * 2.0 * Math.PI + phase; double z = circleRadius * Math.sin(theta); double y = circleRadius * Math.cos(theta); Point3D point = new Point3D(0.0, y, z); circleRotation.transform(point); point.add(circleCenter); return new Pose3D(point, constantOrientation); }
private void assertPointIsOnRay(Point3D query, Point3D rayOrigin, Vector3D rayDirection, double epsilon) { Point3D pointOnRay = new Point3D(); pointOnRay.add(rayOrigin, rayDirection); assertPointIsOnRay(query, rayOrigin, pointOnRay, epsilon); }
/** * 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(); }
/** * circular trajectory */ public static Pose3D computeCircleTrajectory(double time, double trajectoryTime, double circleRadius, Point3DReadOnly circleCenter, Quaternion circleRotation, QuaternionReadOnly constantOrientation, boolean ccw, double phase) { double theta = (ccw ? -time : time) / trajectoryTime * 2.0 * Math.PI + phase; double z = circleRadius * Math.sin(theta); double y = circleRadius * Math.cos(theta); Point3D point = new Point3D(0.0, y, z); circleRotation.transform(point); point.add(circleCenter); return new Pose3D(point, constantOrientation); } }
/** * 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; }
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; }
private void addPointsContribution(Point3D pointToContribute, Point3D pointOnAToPack, Point3D pointOnBToPack) { double lambda = lambdas.get(pointToContribute); tempVector1.set(simplexPointToPolytopePointA.get(pointToContribute)); tempVector1.scale(lambda); pointOnAToPack.add(tempVector1); tempVector1.set(simplexPointToPolytopePointB.get(pointToContribute)); tempVector1.scale(lambda); pointOnBToPack.add(tempVector1); }
private void applyTranslationalSlip(double percentOfDelta) { FrameVector3D slipDelta = new FrameVector3D(slipAmount); slipDelta.scale(percentOfDelta); slipAmount.sub(slipDelta); 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.add(slipDelta); groundContactPointToSlip.setTouchdownLocation(touchdownLocation); } } }
public static ConvexPolytope constructRandomSphereOutlinedPolytope(Random random, int numberOfPoints, double radius, double xyzBoundary) { ConvexPolytope polytope = new ConvexPolytope(); Point3D sphereCenter = EuclidCoreRandomTools.nextPoint3D(random, xyzBoundary, xyzBoundary, xyzBoundary); for (int i = 0; i < numberOfPoints; i++) { Vector3D randomVector = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, radius); Point3D point = new Point3D(sphereCenter); point.add(randomVector); //TODO: Need to connect the edges later once they are used in the algorithm!! polytope.addVertex(point); } return polytope; }
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); }
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; }
public void computeBestEffortPlan(double horizonLength) { Vector2D goalDirection = new Vector2D(bodyGoalPose.getPosition()); goalDirection.sub(bodyStartPose.getX(), bodyStartPose.getY()); goalDirection.scale(horizonLength / goalDirection.length()); Point3D waypoint = new Point3D(bodyStartPose.getPosition()); waypoint.add(goalDirection.getX(), goalDirection.getY(), 0.0); waypoints.add(waypoint); }
private void projectOriginOntoEdge(Point3D vertexOne, Point3D vertexTwo, Point3D projectionToPack) { tempVector1.set(vertexOne); tempVector1.scale(-1.0); tempVector2.sub(vertexTwo, vertexOne); double percentFromVertexOneToVertexTwo = tempVector1.dot(tempVector2) / (tempVector2.dot(tempVector2)); tempVector2.scale(percentFromVertexOneToVertexTwo); projectionToPack.set(vertexOne); projectionToPack.add(tempVector2); double oneMinusPercentFromVertexOneToVertexTwo = 1.0 - percentFromVertexOneToVertexTwo; lambdas.clear(); setLambda(vertexOne, oneMinusPercentFromVertexOneToVertexTwo); setLambda(vertexTwo, percentFromVertexOneToVertexTwo); }
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; }
@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()); } }