Refine search
/** * Returns the center point on a line. */ public static Point3d findCenter(Point3d min, Point3d max) { Point3d center = new Point3d(); center.x = (min.x + max.x) / 2.0; center.y = (min.y + max.y) / 2.0; center.z = (min.z + max.z) / 2.0; return center; }
protected boolean isValidNearScan(Point3d point, Point3d lidarOrigin) { boolean valid = true; valid &= point.getZ() < lidarOrigin.getZ() + parameters.nearScanZMaxAboveHead; Point3d center = new Point3d(lidarOrigin.getX(), lidarOrigin.getY(), point.getZ()); valid &= point.distance(center) < parameters.nearScanRadius; return valid; }
public Point3d get3DCoordinatesForSPLigands(IAtom refAtom, IAtomContainer withCoords, double length, double angle) { //logger.debug(" SP Ligands start "+refAtom.getPoint3d()+" "+(withCoords.getAtomAt(0)).getPoint3d()); Vector3d ca = new Vector3d(refAtom.getPoint3d()); ca.sub((withCoords.getAtom(0)).getPoint3d()); ca.normalize(); ca.scale(length); Point3d newPoint = new Point3d(refAtom.getPoint3d()); newPoint.add(ca); return newPoint; }
private RigidBodyTransform getTransformFromStepToWorld(RigidBodyTransform soleZUpTransform, Vector3d stepVectorInSoleFrame, double stepYaw) { Point3d stepLocationInWorld = new Point3d(stepVectorInSoleFrame); soleZUpTransform.transform(stepLocationInWorld); Vector3d stepRotationEulerInWorld = new Vector3d(); soleZUpTransform.getRotationEuler(stepRotationEulerInWorld); // stepRotationEulerInWorld.setZ((stepRotationEulerInWorld.getZ() + stepYaw + 2.0 * Math.PI) % Math.PI); stepRotationEulerInWorld.setZ(stepRotationEulerInWorld.getZ() + stepYaw); RigidBodyTransform nextTransform = new RigidBodyTransform(); nextTransform.setRotationEulerAndZeroTranslation(stepRotationEulerInWorld); nextTransform.setTranslation(stepLocationInWorld.getX(), stepLocationInWorld.getY(), stepLocationInWorld.getZ()); return nextTransform; }
@Test public void positivePointPlaneDistanceTest() { // the normal for the Y-Z plane is X Vector3d planeNormal = new Vector3d(XAXIS); planeNormal.normalize(); // an arbitrary point in the Y-Z plane Point3d pointInPlane = new Point3d(0, 1, 1); // make a positive point on the X axis = same direction as the normal Point3d pointToMeasurePos = new Point3d(2, 0, 0); double distancePos = StereoTool.signedDistanceToPlane(planeNormal, pointInPlane, pointToMeasurePos); Assert.assertEquals(2.0, distancePos, 0.1); }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d pointToCheck, double epsilon) { temporaryVector.set(pointToCheck.getX(), pointToCheck.getY(), 0.0); if (temporaryVector.length() < Epsilons.ONE_TRILLIONTH) { return tubeRadius >= radius; } temporaryVector.normalize(); temporaryVector.scale(radius); temporaryPoint.set(temporaryVector); return temporaryPoint.distance(pointToCheck) <= tubeRadius + epsilon; }
@Override protected boolean isInsideOrOnSurfaceShapeFrame(Point3d point, double epsilon) { tempPoint3d.set(point); double scaledX = tempPoint3d.getX() / radius.getX(); double scaledY = tempPoint3d.getY() / radius.getY(); double scaledZ = tempPoint3d.getZ() / radius.getZ(); double sumOfSquares = scaledX * scaledX + scaledY * scaledY + scaledZ * scaledZ; return sumOfSquares <= 1.0 + epsilon; }
/** * Move the viewfrustum to the right (according to the view direction). * * @param delta */ public void moveRight( double delta ) { Vector3d deltaVector = new Vector3d( right ); deltaVector.scale( delta ); eye.add( deltaVector ); lookingAt.add( deltaVector ); setCameraParams( eye, lookingAt, up ); }
private void calculateCenters() { localCenter = new Point3d(boxVector); localCenter.scale(0.5); rootCenter = new Point3d(localCenter); forwardTransform.transform(rootCenter); forwardCenterTransform = new Transform3D(forwardTransform); forwardCenterTransform.mul(toTransform(new Vector3d(localCenter))); reverseCenterTransform = new Transform3D(forwardCenterTransform); reverseCenterTransform.invert(); }
private static void addPolygonWithHolesInY(PolygonWithHolesList2d polygonWithHolesList2d, double height, MeshFactory meshFactory, TextureData textureData, double textureStartPointX, double textureStartPointY, Vector3d textureDirection, boolean top) { List<Triangle2d> topMP = Poly2TriSimpleUtil.triangulate(polygonWithHolesList2d); Vector3d yt = new Vector3d(0, 1, 0); if (!top) { yt.negate(); } Point3d textureStartPoint = new Point3d(textureStartPointX, height, -textureStartPointY); Plane3d planeTop = new Plane3d(textureStartPoint, yt); addPolygonToRoofMesh(meshFactory, topMP, planeTop, textureDirection, textureData, 0, 0); }
private static void setTranslationSettingZAndPreservingXAndY(Point3d highestVertex, RigidBodyTransform transformToReturn) { Vector3d newTranslation = new Vector3d(highestVertex.getX(), highestVertex.getY(), 0.0); transformToReturn.transform(newTranslation); newTranslation.scale(-1.0); newTranslation.add(highestVertex); transformToReturn.setTranslation(newTranslation); }
public static BoundingBox3d createUsingCenterAndPlusMinusVector(Point3d center, Tuple3d plusMinusVector, double epsilon) { Point3d minimumPoint = new Point3d(center); Point3d maximumPoint = new Point3d(center); minimumPoint.sub(plusMinusVector); maximumPoint.add(plusMinusVector); return new BoundingBox3d(minimumPoint, maximumPoint, epsilon); }
private Vector3d generateRandomVector3d(Random random, Point3d minValues, Point3d maxValues) { double x = generateRandomDoubleBetween(random, minValues.getX(), maxValues.getX()); double y = generateRandomDoubleBetween(random, minValues.getY(), maxValues.getY()); double z = generateRandomDoubleBetween(random, minValues.getZ(), maxValues.getZ()); return new Vector3d(x, y, z); }
public void changeCameraPosition(double x, double y, double z) { Point3d desiredCameraPosition = new Point3d(x, y, z); Point3d desiredFocusPoint = new Point3d(focusPointTranslation.getX(), focusPointTranslation.getY(), focusPointTranslation.getZ()); double distanceFromFocusPoint = desiredCameraPosition.distance(desiredFocusPoint); offsetFromFocusPoint.setZ(-distanceFromFocusPoint); rotationCalculator.setRotationFromCameraAndFocusPositions(desiredCameraPosition, desiredFocusPoint, 0.0); }
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()); }
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); }