@Override public void setPosition(Point3DReadOnly position) { this.position.set(position); }
@Override public void setPositionToZero() { position.set(0.0, 0.0, 0.0); }
/** * Changes the point through which this plane has to go. * * @param pointX the new x-coordinate of the point on this plane. * @param pointY the new y-coordinate of the point on this plane. * @param pointZ the new z-coordinate of the point on this plane. */ public void setPoint(double pointX, double pointY, double pointZ) { point.set(pointX, pointY, pointZ); hasPointBeenSet = true; }
/** * Sets the initial fix point that the SCS camera looks at. * @param fixX * @param fixY * @param fixZ */ public void setSCSCameraFix(double fixX, double fixY, double fixZ) { scsCameraFix.set(fixX, fixY, fixZ); }
@Override public void setPositionToNaN() { position.set(Double.NaN, Double.NaN, Double.NaN); }
private void setAveragePointDirty() { if (averagePoint != null) this.averagePoint.set(Double.NaN, Double.NaN, Double.NaN); }
public void setPlane(Point3D pointOnPlane, Vector3D planeNormal) { this.pointOnPlane.set(pointOnPlane); this.planeNormal.set(planeNormal); }
/** * Clear the current data. * If the mean of the next dataset is somewhat known, it is preferable to use {@link #clearAndSetPredictedMean(Tuple3DBasics)}. */ public void clear() { sampleSize = 0; mean.set(0.0, 0., 0.0); secondMoment.zero(); }
private PlanarRegionsList createFlatGround(Point3D startPoseToPack, Point3D goalPoseToPack) { PlanarRegionsListGenerator generator = new PlanarRegionsListGenerator(); generator.addRectangle(50.0, 5.0); startPoseToPack.set(-18.005, -2.001, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), startPoseToPack, startPoseToPack); goalPoseToPack.set(18.005, 2.001, 0.0); // RotationMatrixTools.applyRollRotation(Math.toRadians(10.0), goalPoseToPack, goalPoseToPack); return generator.getPlanarRegionsList(); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { point.set(this); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); framePoint.setIncludingFrame(currentReferenceFrame, point); framePoint.changeFrame(desiredFrame); point.set(framePoint); set(point); }
private static void pointInsideRampSide(Point3D pointToPack, Ramp3D ramp, double epsilon) { Random random = new Random(97932L); double xVal = random.nextDouble() * ramp.getLength(); double yVal = (2 * random.nextInt(1) - 1) * 0.5 * ramp.getWidth() + random.nextDouble() * epsilon; double zVal = random.nextDouble() * ramp.getHeight(); if (zVal > xVal * (ramp.getHeight() / ramp.getLength())) zVal = xVal * (ramp.getHeight() / ramp.getLength()); pointToPack.set(xVal, yVal, zVal); ramp.transformToWorld(pointToPack); }
public static VehiclePosePacket createVehiclePosePacket(Point3D position, Quaternion orientation) { VehiclePosePacket message = new VehiclePosePacket(); message.getPosition().set(position); message.getOrientation().set(orientation); return message; }
public static WallPosePacket createWallPosePacket(double cuttingRadius, Tuple3DReadOnly centerPosition, QuaternionReadOnly centerOrientation) { WallPosePacket message = new WallPosePacket(); message.setCuttingRadius(cuttingRadius); message.getCenterPosition().set(centerPosition); message.getCenterOrientation().set(centerOrientation); return message; }
public static WallPosePacket createWallPosePacket(double cuttingRadius, Tuple3DReadOnly centerPosition, RotationMatrixReadOnly rotationMatrix) { WallPosePacket message = new WallPosePacket(); message.setCuttingRadius(cuttingRadius); message.getCenterPosition().set(centerPosition); message.getCenterOrientation().set(new Quaternion(rotationMatrix)); return message; }
public static EuclideanTrajectoryPointMessage createEuclideanTrajectoryPointMessage(double time, Point3DReadOnly position, Vector3DReadOnly linearVelocity) { EuclideanTrajectoryPointMessage message = new EuclideanTrajectoryPointMessage(); message.setTime(time); message.getPosition().set(new Point3D(position)); message.getLinearVelocity().set(new Vector3D(linearVelocity)); return message; }
private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message) { FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.getLocation().set(stepLocation); footstepData.getOrientation().set(orient); footstepData.setRobotSide(robotSide.toByte()); message.getFootstepDataList().add().set(footstepData); }
private void addFootstep(Point3D stepLocation, Quaternion orient, RobotSide robotSide, FootstepDataListMessage message) { FootstepDataMessage footstepData = new FootstepDataMessage(); footstepData.getLocation().set(stepLocation); footstepData.getOrientation().set(orient); footstepData.setRobotSide(robotSide.toByte()); message.getFootstepDataList().add().set(footstepData); }
public static VideoPacket createVideoPacket(VideoSource videoSource, long timeStamp, byte[] data, Point3DReadOnly position, QuaternionReadOnly orientation, IntrinsicParameters intrinsicParameters) { VideoPacket message = new VideoPacket(); message.setVideoSource(videoSource.toByte()); message.setTimestamp(timeStamp); message.getData().add(data); message.getPosition().set(position); message.getOrientation().set(orientation); message.getIntrinsicParameters().set(toIntrinsicParametersMessage(intrinsicParameters)); return message; }