@Override public FrameLine initialValue() { return new FrameLine(); } };
@Override public boolean epsilonEquals(FrameLine otherLine, double epsilon) { checkReferenceFrameMatch(otherLine); return line.epsilonEquals(otherLine.getGeometryObject(), epsilon); }
public FrameLine(FrameLine frameLine) { this(frameLine.getReferenceFrame(), new Line3d(frameLine.getPoint(), frameLine.getNormalizedVector())); }
intersectionToPack.setToNaN(); intersectionToPack.setToZero(planeOne.getReferenceFrame()); Vector3d intersectPlaneNormal = intersectionToPack.getNormalizedVector(); intersectPlaneNormal.cross(planeOne.getNormal(), planeTwo.getNormal()); intersectionToPack.setVectorWithoutChecks(intersectPlaneNormal); RotationTools.computeQuaternionFromYawAndZNormal(0.0, intersectionToPack.getNormalizedVector(), tempIntersectPlaneReferenceFrame.get().getOrientationUnsafe()); tempIntersectPlaneReferenceFrame.get().setPositionWithoutChecksAndUpdate(planeOne.getPoint()); tempIntersectFrameLineOne.get().setPointWithoutChecks(planeOne.getPoint()); tempIntersectFrameLineTwo.get().setPointWithoutChecks(planeTwo.getPoint()); Vector3d intersectVectorOne = tempIntersectFrameLineOne.get().getNormalizedVector(); Vector3d intersectVectorTwo = tempIntersectFrameLineTwo.get().getNormalizedVector(); intersectVectorOne.cross(planeOne.getNormal(), intersectionToPack.getNormalizedVector()); intersectVectorTwo.cross(planeTwo.getNormal(), intersectionToPack.getNormalizedVector()); tempIntersectFrameLineOne.get().setVectorWithoutChecks(intersectVectorOne); tempIntersectFrameLineTwo.get().setVectorWithoutChecks(intersectVectorTwo); tempIntersectFrameLineOne.get().changeFrame(tempIntersectPlaneReferenceFrame.get()); tempIntersectFrameLineTwo.get().changeFrame(tempIntersectPlaneReferenceFrame.get()); tempIntersectFrameLineOne.get().projectOntoXYPlane(tempIntersectLine2dOne.get()); tempIntersectFrameLineTwo.get().projectOntoXYPlane(tempIntersectLine2dTwo.get()); tempIntersectLine2dOne.get().intersectionWith(tempIntersectLine2dTwo.get(), intersectionToPack.getPoint());
private void intersectASinglePolygon(FrameConvexPolygon2d polygon, Pair<FramePoint, FramePoint> intersectionWithPolygon) { intersectionOfPlanes.changeFrame(polygon.getReferenceFrame()); intersectionOfPlanes.projectOntoXYPlane(planeIntersectionOnPolygonPlane); polygon.intersectionWith(planeIntersectionOnPolygonPlane, lineIntersectionOnPolygonPlane); if (lineIntersectionOnPolygonPlane.getFirst().containsNaN() && lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { noIntersection = true; } if (!lineIntersectionOnPolygonPlane.getFirst().containsNaN()) { intersectionWithPolygon.getFirst().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getFirst()); } if (!lineIntersectionOnPolygonPlane.getSecond().containsNaN()) { intersectionWithPolygon.getSecond().setXYIncludingFrame(lineIntersectionOnPolygonPlane.getSecond()); } }
public void setPoint(FramePoint point) { checkReferenceFrameMatch(point); line.setPoint(point.getPoint()); }
public FrameLine(ReferenceFrame referenceFrame, Line3d line) { super(referenceFrame, line); this.line = getGeometryObject(); }
public void projectOntoXYPlane(FrameLine2d lineToPack) { lineToPack.set(getReferenceFrame(), line.getPoint().getX(), line.getPoint().getY(), line.getNormalizedVector().getX(), line.getNormalizedVector().getY()); }
if (intersectionOfPlanes.containsNaN())
public void setVector(FrameVector vector) { checkReferenceFrameMatch(vector); line.setVector(vector.getVector()); }
public void getIntersectionWithLine(FramePoint pointToPack, FrameLine line) { checkReferenceFrameMatch(line.getReferenceFrame()); checkReferenceFrameMatch(pointToPack.getReferenceFrame()); Point3d intersectionToPack = new Point3d(); plane3d.getIntersectionWithLine(intersectionToPack, line.getPoint(), line.getNormalizedVector()); pointToPack.set(intersectionToPack); }
@Override public FrameLine initialValue() { return new FrameLine(); } };
public FrameConvexPolygon2dIntersector() { planeOne = new FramePlane3d(); planeTwo = new FramePlane3d(); intersectionOfPlanes = new FrameLine(); planeIntersectionOnPolygonPlane = new FrameLine2d(); lineIntersectionOnPolygonPlane = new Pair<FramePoint2d, FramePoint2d>(new FramePoint2d(), new FramePoint2d()); intersectionWithPolygonOne = new Pair<FramePoint, FramePoint>(new FramePoint(), new FramePoint()); intersectionWithPolygonTwo = new Pair<FramePoint, FramePoint>(new FramePoint(), new FramePoint()); point2Vector = new Vector3d(); point3Vector = new Vector3d(); point4Vector = new Vector3d(); noIntersection = false; }