private void addPolygonToArtifacts(String name, ConvexPolygon2D polygon, Color color) { YoFrameConvexPolygon2D yoPlanePolygon = new YoFrameConvexPolygon2D(name + "Polygon", worldFrame, 10, registry); artifacts.add(new YoArtifactPolygon(name, yoPlanePolygon , color, false)); yoPlanePolygon.set(polygon); }
private YoGraphicPolygon(String name, YoFrameConvexPolygon2D yoFrameConvexPolygon2d, YoFramePoint3D framePoint, YoFrameYawPitchRoll yawPitchRoll, YoFrameQuaternion quaternion, double scale, double height, AppearanceDefinition appearance) { super(name, framePoint, yawPitchRoll, quaternion, scale); if (yoFrameConvexPolygon2d.getNumberOfVertices() <= 0) yoFrameConvexPolygon2d.set(Vertex2DSupplier.emptyVertex2DSupplier()); this.yoFrameConvexPolygon2d = yoFrameConvexPolygon2d; this.appearance = appearance; this.height = height; verticesToDisplay = new ArrayList<>(yoFrameConvexPolygon2d.getMaxNumberOfVertices()); graphics3dObject = new Graphics3DObject(); graphics3dObject.setChangeable(true); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(yoFrameConvexPolygon2d, height); instruction = new Graphics3DAddMeshDataInstruction(meshDataHolder, appearance); graphics3dObject.addInstruction(instruction); }
/** {@inheritDoc} */ @Override public void clearAndUpdate() { clear(); isUpToDate = true; }
/** {@inheritDoc} */ @Override public void addVertexMatchingFrame(ReferenceFrame referenceFrame, Point2DReadOnly vertex, boolean checkIfTransformInXYPlane) { // Check for the trivial case: the geometry is already expressed in the desired frame. if (getReferenceFrame() == referenceFrame) { addVertex(vertex); } else { referenceFrame.getTransformToDesiredFrame(transformToDesiredFrame, getReferenceFrame()); addVertex(vertex); getVertexUnsafe(getNumberOfVertices() - 1).applyTransform(transformToDesiredFrame, checkIfTransformInXYPlane); } }
@Override public YoVariable<?>[] getVariables() { YoVariable<?>[] vars = new YoVariable[1 + 2 * convexPolygon.getMaxNumberOfVertices()]; int i = 0; vars[i++] = convexPolygon.getYoNumberOfVertices(); for (YoFramePoint2D p : convexPolygon.getVertexBuffer()) { vars[i++] = p.getYoX(); vars[i++] = p.getYoY(); } return vars; }
for (int i = 0; i < maxPolygonsToVisualize; i++) YoFrameConvexPolygon2D polygon = new YoFrameConvexPolygon2D("Polygon" + i, worldFrame, maxPolygonsVertices, registry); YoFramePoseUsingYawPitchRoll pose = new YoFramePoseUsingYawPitchRoll("PolygonPose" + i, worldFrame, registry); pose.setToNaN(); if (planarRegion.getConvexHull().getNumberOfVertices() > visiblePolygons.get(polygonIdx).getMaxNumberOfVertices()) pose.set(transformToWorld); visiblePolygonPoses.get(polygonIdx).set(pose); visiblePolygons.get(polygonIdx).set(planarRegion.getConvexHull());
public YoGraphicPolygon(String name, YoFramePose3D framePose, int maxNumberOfVertices, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, new YoFrameConvexPolygon2D(name + "ConvexPolygon2d", worldFrame, maxNumberOfVertices, registry), framePose, scale, appearance); }
public void updateConvexPolygon2d(ConvexPolygon2DReadOnly convexPolygon2d) { yoFrameConvexPolygon2d.set(convexPolygon2d); update(); }
YoFrameConvexPolygon2D yoFrameConvexPolygon2d = new YoFrameConvexPolygon2D("boo", "yaw", ReferenceFrame.getWorldFrame(), 4, new YoVariableRegistry("bah")); yoFrameConvexPolygon2d.set(poly); FramePoint2DReadOnly convexPoint = yoFrameConvexPolygon2d.getVertex(i); polyPoint.checkReferenceFrameMatch(convexPoint); assertTrue("not equal expected: " + polyPoint + " actual: " + convexPoint, MathTools.epsilonEquals(polyPoint.getX(), convexPoint.getX(), 1e-7)); yoFrameConvexPolygon2d.set(poly); FramePoint2DReadOnly convexPoint = yoFrameConvexPolygon2d.getVertex(i); FramePoint2D polyPoint = new FramePoint2D(expected.getVertex(i)); assertTrue("not equal expected: " + polyPoint + " actual: " + convexPoint, polyPoint.epsilonEquals(convexPoint, 1e-7));
private YoGraphicPolygon(String name, YoVariable<?>[] yoVariables, double[] constants, AppearanceDefinition appearance) { /* * 2 * (int) constants[1] + 1 => constants[1] is the size of the 2D vertex * buffer so we have 2 * YoVariables, the "+ 1" is for the YoInteger that * keeps track of the actual polygon size. */ super(name, Arrays.copyOfRange(yoVariables, 2 * (int) constants[1] + 1, yoVariables.length), constants[0]); int constantIndex = 1; // 0 corresponds to the scale factor int vertexBufferSize = (int) constants[constantIndex++]; // Ensuring backward compatibility this.height = constants.length == 3 ? constants[constantIndex++] : DEFAULT_HEIGHT; int yoIndex = 0; YoInteger numberOfVertices = (YoInteger) yoVariables[yoIndex++]; List<YoFramePoint2D> yoFrameVertices = new ArrayList<>(); while (yoFrameVertices.size() < vertexBufferSize) { YoDouble xVariable = (YoDouble) yoVariables[yoIndex++]; YoDouble yVariable = (YoDouble) yoVariables[yoIndex++]; yoFrameVertices.add(new YoFramePoint2D(xVariable, yVariable, worldFrame)); } yoFrameConvexPolygon2d = new YoFrameConvexPolygon2D(yoFrameVertices, numberOfVertices, worldFrame); verticesToDisplay = new ArrayList<>(yoFrameConvexPolygon2d.getMaxNumberOfVertices()); this.appearance = appearance; graphics3dObject = new Graphics3DObject(); graphics3dObject.setChangeable(true); MeshDataHolder meshDataHolder = MeshDataGenerator.ExtrudedPolygon(yoFrameConvexPolygon2d, height); instruction = new Graphics3DAddMeshDataInstruction(meshDataHolder, appearance); graphics3dObject.addInstruction(instruction); }
@Override public void notifyOfVariableChange(YoVariable<?> v) { FramePoint2D icp = new FramePoint2D(yoICP); RobotSide supportSide = yoSupportSide.getEnumValue(); yoFootPolygons.get(supportSide.getOppositeSide()).clear(); footPolygons.get(supportSide).changeFrame(worldFrame); yoFootPolygons.get(supportSide).set(footPolygons.get(supportSide)); footPolygons.get(supportSide).changeFrame(ankleZUpFrames.get(supportSide)); oneStepCaptureRegionCalculator.calculateCaptureRegion(supportSide.getOppositeSide(), swingTimeRemaining.getDoubleValue(), icp, omega0, footPolygons.get(supportSide)); FrameConvexPolygon2D frameConvexPolygon2d = new FrameConvexPolygon2D(); frameConvexPolygon2d.setIncludingFrame(oneStepCaptureRegionCalculator.getCaptureRegion()); frameConvexPolygon2d.changeFrame(worldFrame); yoCaptureRegion.set(frameConvexPolygon2d); simulationOverheadPlotter.update(); } };
@Override public void update() { verticesToDisplay.clear(); for (int i = yoFrameConvexPolygon2d.getNumberOfVertices() - 1; i >= 0; i--) { /* * Using the unsafe getter as this instance of the polygon might not be * recognized as up-to-date. This can occur when displaying a remote * YoGraphic. In such scenario, the coordinates of the vertices are * updated under the hood and this polygon only serves as a data * holder. */ verticesToDisplay.add(yoFrameConvexPolygon2d.getVertexUnsafe(i)); } instruction.setMesh(MeshDataGenerator.ExtrudedPolygon(verticesToDisplay, height)); }
@Override public void draw(Graphics2DAdapter graphics) { graphics.setColor(color); graphics.setStroke(stroke); tempConvexPolygon.clear(); for (int i = 0; i < convexPolygon.getNumberOfVertices(); i++) { // Accessing directly the vertex buffer to avoid internal checks. Due asynchronous read/write, these check can throw exceptions. tempConvexPolygon.addVertex(convexPolygon.getVertexBuffer().get(i)); } tempConvexPolygon.update(); if (fill) { graphics.drawPolygonFilled(tempConvexPolygon); } else { graphics.drawPolygon(tempConvexPolygon); } }
YoFrameConvexPolygon2D defaultPolygon = new YoFrameConvexPolygon2D("DefaultFootPolygon", worldFrame, 4, registry); defaultPolygon.set(PlannerTools.createDefaultFootPolygon()); for (int i = 0; i < maxPolygonsToVisualize; i++) YoFrameConvexPolygon2D polygon = new YoFrameConvexPolygon2D("Polygon" + i, worldFrame, maxPolygonsVertices, registry); YoFramePoseUsingYawPitchRoll pose = new YoFramePoseUsingYawPitchRoll("PolygonPose" + i, worldFrame, registry); pose.setToNaN(); if (planarRegion.getConvexHull().getNumberOfVertices() > visiblePolygons.get(polygonIdx).getMaxNumberOfVertices()) pose.set(transformToWorld); visiblePolygonPoses.get(polygonIdx).set(pose); visiblePolygons.get(polygonIdx).set(planarRegion.getConvexHull());
public YoGraphicPolygon(String name, YoFramePoseUsingYawPitchRoll framePose, int maxNumberOfVertices, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, new YoFrameConvexPolygon2D(name + "ConvexPolygon2d", worldFrame, maxNumberOfVertices, registry), framePose, scale, appearance); }
public void updateConvexPolygon2d(FrameConvexPolygon2DReadOnly frameConvexPolygon2d) { yoFrameConvexPolygon2d.set(frameConvexPolygon2d); update(); }
@Override public YoVariable<?>[] getVariables() { //poly + framePoint + frameOrientation YoVariable<?>[] superYoVariables = super.getVariables(); YoVariable<?>[] yoVariables = new YoVariable[1 + 2 * yoFrameConvexPolygon2d.getMaxNumberOfVertices() + superYoVariables.length]; int i = 0; yoVariables[i++] = yoFrameConvexPolygon2d.getYoNumberOfVertices(); for (YoFramePoint2D p : yoFrameConvexPolygon2d.getVertexBuffer()) { yoVariables[i++] = p.getYoX(); yoVariables[i++] = p.getYoY(); } for (YoVariable<?> superYoVariable : superYoVariables) { yoVariables[i++] = superYoVariable; } return yoVariables; }
public PolygonSnapperVisualizer(ConvexPolygon2D snappingPolygonShape) { Robot robot = new Robot("Robot"); scs = new SimulationConstructionSet(robot); scs.setDT(0.1, 1); polygonToSnap = new YoFrameConvexPolygon2D("polygonToSnap", ReferenceFrame.getWorldFrame(), 4, registry); snappedPolygon = new YoFrameConvexPolygon2D("snappedPolygon", ReferenceFrame.getWorldFrame(), 4, registry); polygonToSnap.set(snappingPolygonShape); snappedPolygon.set(snappingPolygonShape); polygonToSnapPose = new YoFramePoseUsingYawPitchRoll("polygonToSnapPose", ReferenceFrame.getWorldFrame(), registry); snappedPolygonPose = new YoFramePoseUsingYawPitchRoll("snappedPolygonPose", ReferenceFrame.getWorldFrame(), registry); polygonToSnapPose.setToNaN(); snappedPolygonPose.setToNaN(); polygonToSnapViz = new YoGraphicPolygon("polygonToSnapViz", polygonToSnap, polygonToSnapPose, 1.0, YoAppearance.Green()); snappedPolygonViz = new YoGraphicPolygon("snappedPolygonViz", polygonToSnap, snappedPolygonPose, 1.0, YoAppearance.Red()); polygonToSnapViz.update(); snappedPolygonViz.update(); scs.addYoGraphic(polygonToSnapViz); scs.addYoGraphic(snappedPolygonViz); scs.addYoVariableRegistry(registry); scs.setGroundVisible(false); scs.startOnAThread(); }
public YoGraphicPolygon(String name, int maxNumberOfVertices, YoVariableRegistry registry, boolean useYawPitchRoll, double scale, AppearanceDefinition appearance) { this(name, new YoFrameConvexPolygon2D(name + "ConvexPolygon2d", worldFrame, maxNumberOfVertices, registry), registry, useYawPitchRoll, scale, appearance); }
@Override public void run() { int i = 0; while (i++ < 20) { quickPause(); ConvexPolygon2D newPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(secondPointList)); yoPolygon.set(newPolygon); ConvexPolygon2D newYoPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(pointList)); yoFramePolygon.set(newYoPolygon); yoGraphicYoFramePolygon.update(); Vector3D eulerAngles = new Vector3D(); yoFramePolygonOrientation.getEulerAngles(eulerAngles); eulerAngles.setY(eulerAngles.getY() + 0.1); yoFramePolygonOrientation.setEulerAngles(eulerAngles); yoGraphicText.setText("Hello"); yoGraphicText.update(); scs.tickAndUpdate(); quickPause(); newPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(pointList)); yoPolygon.set(newPolygon); yoGraphicYoFramePolygon.update(); newYoPolygon = new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(secondPointList)); yoFramePolygon.set(newYoPolygon); yoGraphicText.setText("GoodBye"); yoGraphicText.update(); scs.tickAndUpdate(); } }