/** * returns true if this boxes minimum point y value >= referenceY */ public boolean isBoxAtOrLeftOf(double referenceY) { return minPoint.getY() >= referenceY; }
private void initTransition() { camXSpeed = -(camX - storedCameraPositions.get(storedPositionIndex).getX()) / transitionTime; camYSpeed = -(camY - storedCameraPositions.get(storedPositionIndex).getY()) / transitionTime; camZSpeed = -(camZ - storedCameraPositions.get(storedPositionIndex).getZ()) / transitionTime; fixXSpeed = -(fixX - storedFixPositions.get(storedPositionIndex).getX()) / transitionTime; fixYSpeed = -(fixY - storedFixPositions.get(storedPositionIndex).getY()) / transitionTime; fixZSpeed = -(fixZ - storedFixPositions.get(storedPositionIndex).getZ()) / transitionTime; transitioning = true; lastTransitionTime = System.currentTimeMillis(); }
private boolean approximatelyEqual(FootstepDataMessage currentLocation, FootstepDataMessage checkAgainst){ if (currentLocation == null) return false; double xDiff = currentLocation.location.getX() - checkAgainst.location.getX(); double yDiff = currentLocation.location.getY() - checkAgainst.location.getY(); if (currentLocation.robotSide != checkAgainst.robotSide) return false; if (Math.sqrt(Math.pow(xDiff, 2) + Math.pow(yDiff,2)) > 0.05) return false; if (Math.abs(RotationTools.computeYaw(currentLocation.orientation) - RotationTools.computeYaw(checkAgainst.orientation)) > Math.PI/16) return false; return true; }
public void getAllPointsWithinBounds(Box bounds, ArrayList<Point3d> pointsToPack) { for (Point3d point : points) { if (bounds.containsOrEquals(point.getX(), point.getY())) { pointsToPack.add(point); } } }
@Override public boolean containsNaN() { if (Double.isNaN(position.getX()) || Double.isNaN(position.getY()) || Double.isNaN(position.getZ())) return true; if (Double.isNaN(linearVelocity.getX()) || Double.isNaN(linearVelocity.getY()) || Double.isNaN(linearVelocity.getZ())) return true; return false; }
public static BoundingBox3d union(BoundingBox3d boundingBoxOne, BoundingBox3d boundingBoxTwo) { double minX = Math.min(boundingBoxOne.minPoint.getX() - boundingBoxOne.epsilon, boundingBoxTwo.minPoint.getX() - boundingBoxTwo.epsilon); double minY = Math.min(boundingBoxOne.minPoint.getY() - boundingBoxOne.epsilon, boundingBoxTwo.minPoint.getY() - boundingBoxTwo.epsilon); double minZ = Math.min(boundingBoxOne.minPoint.getZ() - boundingBoxOne.epsilon, boundingBoxTwo.minPoint.getZ() - boundingBoxTwo.epsilon); double maxX = Math.max(boundingBoxOne.maxPoint.getX() + boundingBoxOne.epsilon, boundingBoxTwo.maxPoint.getX() + boundingBoxTwo.epsilon); double maxY = Math.max(boundingBoxOne.maxPoint.getY() + boundingBoxOne.epsilon, boundingBoxTwo.maxPoint.getY() + boundingBoxTwo.epsilon); double maxZ = Math.max(boundingBoxOne.maxPoint.getZ() + boundingBoxOne.epsilon, boundingBoxTwo.maxPoint.getZ() + boundingBoxTwo.epsilon); Point3d unionMin = new Point3d(minX, minY, minZ); Point3d unionMax = new Point3d(maxX, maxY, maxZ); return new BoundingBox3d(unionMin, unionMax, 0.0); }
public void setStartAndEnd(Point3d startPoint, Point3d endPoint) { this.startX.set(startPoint.getX()); this.startY.set(startPoint.getY()); this.startZ.set(startPoint.getZ()); this.endX.set(endPoint.getX()); this.endY.set(endPoint.getY()); this.endZ.set(endPoint.getZ()); this.vectorX.set(endPoint.getX() - startPoint.getX()); this.vectorY.set(endPoint.getY() - startPoint.getY()); this.vectorZ.set(endPoint.getZ() - startPoint.getZ()); }
@Override protected void setBehaviorInput() { FramePoint point = new FramePoint(ReferenceFrame.getWorldFrame(), grabLocation.getX(), grabLocation.getY(), grabLocation.getZ() + objectRadius); atlasPrimitiveActions.wholeBodyBehavior.setSolutionQualityThreshold(2.01); atlasPrimitiveActions.wholeBodyBehavior.setTrajectoryTime(3); FrameOrientation tmpOr = new FrameOrientation(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); atlasPrimitiveActions.wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
public QuadTreeForGroundHeightMap readQuadTreeForGroundHeightMap(BufferedReader bufferedReader, int skipPoints, int maxNumberOfPoints, Box bounds, double maxZ, QuadTreeForGroundParameters quadTreeParameters) throws IOException { QuadTreeForGroundHeightMap quadTreeToReturn = new QuadTreeForGroundHeightMap(bounds, quadTreeParameters); ArrayList<Point3d> points = readPointsFromBufferedReader(bufferedReader, skipPoints, maxNumberOfPoints, bounds, maxZ); for (Point3d point : points) { quadTreeToReturn.addToQuadtree(point.getX(), point.getY(), point.getZ()); } return quadTreeToReturn; }
public boolean isPointConservitivelyNearWheel(double x, double y, double z, RigidBodyTransform inverseTransformFromSomeframeToWheel, double fudgeFactor) { tempPoint.set(x, y, z); inverseTransformFromSomeframeToWheel.transform(tempPoint); x = tempPoint.getX(); y = tempPoint.getY(); z = tempPoint.getZ(); return isPointNearWheel(x, y, z, fudgeFactor); }
public void update(Point3d pointUnfiltered) { x.update(pointUnfiltered.getX()); y.update(pointUnfiltered.getY()); z.update(pointUnfiltered.getZ()); }
private double getAngleToPelvis(Point3d point, Point3d lidarOrigin) { RigidBodyTransform tf = new RigidBodyTransform(); ReferenceFrame.getWorldFrame().getTransformToDesiredFrame(tf, fullRobotModel.getPelvis().getBodyFixedFrame()); Point3d tfPoint = new Point3d(point); tf.transform(tfPoint); return Math.atan2(tfPoint.getY(), tfPoint.getX()); }
public static void applyTranform(Transform transform, Point3d pointToTransform) { Point3D temporaryVector = transform.transform(pointToTransform.getX(), pointToTransform.getY(), pointToTransform.getZ()); pointToTransform.set(temporaryVector.getX(), temporaryVector.getY(), temporaryVector.getZ()); }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d pointToCheck, double epsilon) { return MathTools.isPreciselyBoundedByInclusive(0.0, size.getX(), pointToCheck.getX(), epsilon * 2.0) && MathTools.isPreciselyBoundedByInclusive(-size.getY() / 2.0, size.getY() / 2.0, pointToCheck.getY(), epsilon * 2.0) && MathTools.isPreciselyBoundedByInclusive(0.0, size.getZ(), pointToCheck.getZ(), epsilon * 2.0) && rampPlane.isOnOrBelow(pointToCheck, epsilon); }