/** * isPointInside * Determines whether a point is inside the convex polygon (point in polygon test). * Uses the orientation method (Nordbeck, Rystedt, 1967) * Test is only valid for convex polygons, and only if the vertices are ordered clockwise. * * @param framePoint FramePoint2d the point to be tested * @return boolean true if the point is inside the polygon */ public boolean isPointInside(FramePoint2d framePoint, double epsilon) { framePoint.checkReferenceFrameMatch(referenceFrame); return ConvexPolygon2dCalculator.isPointInside(framePoint.tuple, epsilon, this.convexPolygon); }
public void setCenterOfPressure(FramePoint2d centerOfPressure, RigidBody foot) { if (centerOfPressure != null) { centerOfPressure.checkReferenceFrameMatch(soleFrames.get(foot)); centerOfPressure.get(centerOfPressures.get(foot)); } else { centerOfPressures.get(foot).set(Double.NaN, Double.NaN); } }
public void setXYFromPosition2dAndUpdate(FramePoint2d position2d) { position2d.checkReferenceFrameMatch(parentFrame); originPose.setXYFromPosition2d(position2d); this.update(); }
public void setPositionAndUpdate(FramePoint2d framePoint) { framePoint.checkReferenceFrameMatch(parentFrame); originPose.setPosition(framePoint); this.update(); }
public void setFirstEndpoint(FramePoint2d firstEndpointToPack) { firstEndpointToPack.checkReferenceFrameMatch(referenceFrame); lineSegment.set(firstEndpointToPack.getPoint(), lineSegment.endpoints[1]); }
public void setSecondEndpoint(FramePoint2d secondEndpointToPack) { secondEndpointToPack.checkReferenceFrameMatch(referenceFrame); lineSegment.set(lineSegment.endpoints[0], secondEndpointToPack.getPoint()); }
public boolean containsEpsilon(FramePoint2d framePoint, double epsilon) { framePoint.checkReferenceFrameMatch(referenceFrame); return line.containsEpsilon(framePoint.getPointCopy(), epsilon); }
/** * Add a vertex to this polygon. * Note that this method recycles memory. * @param newVertex {@code FramePoint2d} the new vertex (it is not modified). * @throws ReferenceFrameMismatchException */ public void addVertex(FramePoint2d vertex) { vertex.checkReferenceFrameMatch(referenceFrame); addVertex(vertex.getPoint()); }
public void setIncludingFrame(FramePoint2d endpoint0, FramePoint2d endpoint1) { endpoint0.checkReferenceFrameMatch(endpoint1); this.referenceFrame = endpoint0.referenceFrame; this.line.set(endpoint0.getPoint(), endpoint1.getPoint()); }
public FramePoint2d getClosestVertexCopy(FramePoint2d point) { point.checkReferenceFrameMatch(referenceFrame); return new FramePoint2d(referenceFrame, ConvexPolygon2dCalculator.getClosestVertexCopy(point.getPoint(), convexPolygon)); }
/** FrameVector2d <p/> A normal vector2d associated with a specific reference frame. */ public FrameVector2d(FramePoint2d startFramePoint, FramePoint2d endFramePoint) { this(endFramePoint.referenceFrame, endFramePoint.tuple.getX(), endFramePoint.tuple.getY(), endFramePoint.name); startFramePoint.checkReferenceFrameMatch(endFramePoint); sub(startFramePoint); }
public void getClosestVertex(FramePoint2d closestVertexToPack, FramePoint2d point) { point.checkReferenceFrameMatch(referenceFrame); closestVertexToPack.setIncludingFrame(referenceFrame, ConvexPolygon2dCalculator.getClosestVertexCopy(point.getPoint(), convexPolygon)); }
public FrameLineSegment2d(FramePoint2d[] endpoints) { this(endpoints[0].getReferenceFrame(), new LineSegment2d(endpoints[0].getPointCopy(), endpoints[1].getPointCopy())); endpoints[0].checkReferenceFrameMatch(endpoints[1]); }
public FrameLineSegment2d(FramePoint2d firstEndpoint, FramePoint2d secondEndpoint) { this(firstEndpoint.getReferenceFrame(), new LineSegment2d(firstEndpoint.getPointCopy(), secondEndpoint.getPointCopy())); firstEndpoint.checkReferenceFrameMatch(secondEndpoint); }
public FrameLine2d(FramePoint2d firstPointOnLine, FramePoint2d secondPointOnLine) { this(firstPointOnLine.getReferenceFrame(), new Line2d(firstPointOnLine.getPointCopy(), secondPointOnLine.getPointCopy())); firstPointOnLine.checkReferenceFrameMatch(secondPointOnLine); }
public FrameLine2d(FramePoint2d framePoint2d, FrameVector2d frameVector2d) { this(framePoint2d.getReferenceFrame(), new Line2d(framePoint2d.getPointCopy(), frameVector2d.getVectorCopy())); framePoint2d.checkReferenceFrameMatch(frameVector2d); }
public void setIncludingFrame(FramePoint2d startPoint, FrameVector2d vector) { startPoint.checkReferenceFrameMatch(vector); this.referenceFrame = startPoint.getReferenceFrame(); this.line.set(startPoint.getPoint(), vector.getVector()); }
public static void computeCapturePoint(FramePoint2d capturePointToPack, FramePoint2d centerOfMassInWorld, FrameVector2d centerOfMassVelocityInWorld, double omega0) { centerOfMassInWorld.checkReferenceFrameMatch(worldFrame); centerOfMassVelocityInWorld.checkReferenceFrameMatch(worldFrame); capturePointToPack.setToZero(worldFrame); capturePointToPack.set(centerOfMassVelocityInWorld); capturePointToPack.scale(1.0 / omega0); capturePointToPack.add(centerOfMassInWorld); }
/** * Packs the linear velocity of a point2d that is fixed in bodyFrame but is expressed in baseFrame, * with respect to baseFrame, expressed in expressedInFrame */ public void getLineaVelocityOfPoint2dFixedInBodyFrame(FrameVector linearVelocityToPack, FramePoint2d point2dFixedInBodyFrame) { baseFrame.checkReferenceFrameMatch(expressedInFrame); point2dFixedInBodyFrame.checkReferenceFrameMatch(baseFrame); point2dFixedInBodyFrame.get(freeVector); linearVelocityToPack.setToZero(expressedInFrame); linearVelocityToPack.cross(angularPart, freeVector); linearVelocityToPack.add(linearPart); }
public boolean isCellAtLocationOccupied(FramePoint2d location) { location.checkReferenceFrameMatch(soleFrame); location.get(tempPoint); tempPoint.setX(tempPoint.getX() - gridOrigin.getX()); tempPoint.setY(tempPoint.getY() - gridOrigin.getY()); int xIndex = findXIndex(tempPoint.getX()); int yIndex = findYIndex(tempPoint.getY()); if (checkIfIndicesAreValid(xIndex, yIndex)) return occupancyGrid.get(xIndex, yIndex) > 0.9; else return false; }