private double getAverageHeightOfLeaf() { Point3d averagePoint = leaf.getAveragePoint(); return averagePoint.getZ(); }
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(); }
@Override public boolean isValidNearScan(Point3d point, Point3d lidarOrigin) { boolean valid = super.isValidNearScan(point, lidarOrigin); valid &= point.getZ() > getMidFootPoint().getZ() + parameters.nearScanZMinAboveFeet; // valid &= parameters.nearScanCollisions || robotBoundingBoxes.isValidPoint(lidarOrigin, point); valid &= Math.abs(getAngleToPelvis(point, lidarOrigin)) < parameters.nearScanRadians; return valid; }
private void checkIsTransformationInPlane(RigidBodyTransform transformToNewFrame, Point3d transformedPoint) { transformToNewFrame.getTranslation(temporaryTranslation); if (Math.abs(temporaryTranslation.getZ() - transformedPoint.getZ()) > epsilon) throw new RuntimeException("Cannot transform FramePoint2d to a plane with a different surface normal"); }
@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 boolean intersectionWith(Line2d secondLine, Point3d intersectionToPack) { boolean success = GeometryTools.getIntersectionBetweenTwoLines(point, normalizedVector, secondLine.point, secondLine.normalizedVector, tempPoint2d); if (success) intersectionToPack.set(tempPoint2d.getX(), tempPoint2d.getY(), intersectionToPack.getZ()); return success; }
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 void update(Point3d pointUnfiltered) { x.update(pointUnfiltered.getX()); y.update(pointUnfiltered.getY()); z.update(pointUnfiltered.getZ()); }
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); }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3d positionInWorld = new Vector3d(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
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); }