public static double getRadius(FramePoint position) { double x = position.getX(); double y = position.getY(); double radius = Math.hypot(x, y); return radius; }
public static double getAngle(FramePoint position) { double x = position.getX(); double y = position.getY(); double angle = Math.atan2(y, x); return angle; }
/** * Creates a new FramePoint2d based on the x and y components of this FramePoint */ public FramePoint2d toFramePoint2d() { return new FramePoint2d(this.getReferenceFrame(), this.getX(), this.getY()); }
public static void packFramePointInJMEVector(FramePoint original, Vector3f target) { target.set((float) original.getX(), (float) original.getY(), (float) original.getZ()); }
public void getPoint2d(Point2d pointToPack) { pointToPack.setX(this.getX()); pointToPack.setY(this.getY()); }
@Override public void getPosition2d(FramePoint2d framePoint2dToPack) { framePoint2dToPack.setIncludingFrame(getReferenceFrame(), position.getX(), position.getY()); }
public double getXYPlaneDistance(FramePoint2d framePoint2d) { checkReferenceFrameMatch(framePoint2d); double dx, dy; dx = this.getX() - framePoint2d.getX(); dy = this.getY() - framePoint2d.getY(); return Math.sqrt(dx * dx + dy * dy); }
/** * Makes the pointToPack a FramePoint2d version of this FramePoint * * @param pointToPack */ public void getFramePoint2d(FramePoint2d pointToPack) { pointToPack.setIncludingFrame(this.getReferenceFrame(), this.getX(), this.getY()); }
/** * Makes the tupleToPack a FrameTuple2d version of this FramePoint * * @param tupleToPack */ public void getFrameTuple2d(FrameTuple2d<?, ?> tupleToPack) { tupleToPack.setIncludingFrame(this.getReferenceFrame(), this.getX(), this.getY()); }
public void setPosition(FramePoint position) { this.x.set(position.getX()); this.y.set(position.getY()); if (this.z != null) this.z.set(position.getZ()); }
public static double getRadialVelocity(FramePoint position, FrameVector velocity) { position.checkReferenceFrameMatch(velocity); double x = position.getX(); double y = position.getY(); double xd = velocity.getX(); double yd = velocity.getY(); double radius = getRadius(position); return (x * xd + y * yd) / radius; }
public void update(FramePoint pointUnfiltered) { checkReferenceFrameMatch(pointUnfiltered); x.update(pointUnfiltered.getX()); y.update(pointUnfiltered.getY()); z.update(pointUnfiltered.getZ()); }
public ReferenceFrame copyAndAimAxisAtPoint(Axis axisToAlign, FramePoint targetToAimAt) { ReferenceFrame initialFrame = targetToAimAt.getReferenceFrame(); targetToAimAt.changeFrame(this); FrameVector targetRelativeToCurrentFrame = new FrameVector(this, targetToAimAt.getX(), targetToAimAt.getY(), targetToAimAt.getZ()); targetToAimAt.changeFrame(initialFrame); return copyAndAlignAxisWithVector(axisToAlign, targetRelativeToCurrentFrame); }
private void drawSupportPolygon(QuadrupedSupportPolygon supportPolygon, YoFrameConvexPolygon2d yoFramePolygon) { ConvexPolygon2d polygon = new ConvexPolygon2d(); for(RobotQuadrant quadrant : supportPolygon.getSupportingQuadrantsInOrder()) { FramePoint footstep = supportPolygon.getFootstep(quadrant); polygon.addVertex(footstep.getX(), footstep.getY()); } polygon.update(); yoFramePolygon.setConvexPolygon2d(polygon); }
private void updateSensorPosition() { sensorFrame.update(); sensorPose.setPose(sensorFrame.getTransformToDesiredFrame(world)); sensorPose.getPositionIncludingFrame(temp); yoSensorPositionInWorld.set(temp.getReferenceFrame(), temp.getX(), temp.getY(), temp.getZ()); }
public void getClosestVoxel(FramePoint voxelLocationToPack, FramePoint inputPoint) { checkReferenceFrameMatch(inputPoint); if (!boundingBox.isInside(inputPoint.getPoint())) throw new RuntimeException("The given point is outside the grid"); voxelLocationToPack.setToZero(getReferenceFrame()); voxelLocationToPack.setX(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getX()))); voxelLocationToPack.setY(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getY()))); voxelLocationToPack.setZ(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getZ()))); }
public Graphics3DObject createVisualization(FramePoint voxelLocation, double scale, double reachabilityValue) { ReferenceFrame originalFrame = voxelLocation.getReferenceFrame(); voxelLocation.changeFrame(ReferenceFrame.getWorldFrame()); Graphics3DObject voxelViz = new Graphics3DObject(); AppearanceDefinition appearance = YoAppearance.RGBColorFromHex(Color.HSBtoRGB((float) (0.7 * reachabilityValue), 1.0f, 1.0f)); voxelViz.translate(voxelLocation.getX(), voxelLocation.getY(), voxelLocation.getZ()); voxelViz.addSphere(scale * voxelSize / 2.0, appearance); voxelLocation.changeFrame(originalFrame); return voxelViz; } }