public static MeshDataHolder Line(LineSegment3d lineSegment3d, double width) { return Line(lineSegment3d.getFirstEndpoint(), lineSegment3d.getSecondEndpoint(), width); }
/** * Compute the 3D equivalent of this line segment. * The 3D equivalent of each end point is computed as follows: * {@code endPoint3d = endPoint1d * direction3d + zero3d}. * @param zero3d position of the 3D equivalent of an endpoint equal to zero. * @param direction3d direction toward greater values of {@code endPoint1d}. * @return the 3D equivalent of this line segment. */ public LineSegment3d toLineSegment3d(Point3d zero3d, Vector3d direction3d) { LineSegment3d lineSegment3d = new LineSegment3d(); lineSegment3d.getFirstEndpoint().scaleAdd(endpoint1, direction3d, zero3d); lineSegment3d.getSecondEndpoint().scaleAdd(endpoint2, direction3d, zero3d); return lineSegment3d; }
public boolean firstEndpointContainsNaN() { return lineSegment3d.firstEndpointContainsNaN(); }
public Ray3d getRay(int index) { LineSegment3d unitSegment = getLineSegment(index, 1.0f); return new Ray3d(unitSegment.getFirstEndpoint(), unitSegment.getDirectionCopy(true)); }
public Point3d getFirstEndpoint() { return lineSegment3d.getFirstEndpoint(); }
public Point3d getSecondEndpoint() { return lineSegment3d.getSecondEndpoint(); }
public FrameLineSegment(ReferenceFrame referenceFrame, Point3d firstEndpoint, Point3d secondEndpoint) { super(referenceFrame, new LineSegment3d(firstEndpoint, secondEndpoint)); lineSegment3d = getGeometryObject(); }
/** * Tests whether the projection of the given point onto this line segment is located * between the two endpoints or exactly on an endpoint. * * @param point the query. Not modified. * @return {@code true} if the projection of the point is between the endpoints of this line segment, {@code false} otherwise. */ public boolean isBetweenEndpoints(Point3d point) { return isBetweenEndpoints(point, 0); }
/** * Computes the vector going from the first to the second endpoint of this line segment. * <p> * WARNING: This method generates garbage. * </p> * * @param normalize whether the direction vector is to be normalized. * @return the direction of this line segment. */ public Vector3d getDirectionCopy(boolean normalize) { Vector3d direction = new Vector3d(); getDirection(normalize, direction); return direction; }
public double length() { return lineSegment3d.length(); }
public double getDistance(FramePoint framePoint) { checkReferenceFrameMatch(framePoint); return lineSegment3d.distance(framePoint.getPoint()); }
public void getMidpoint(FramePoint midpointToPack) { checkReferenceFrameMatch(midpointToPack); lineSegment3d.getMidpoint(midpointToPack.getPoint()); }
public FrameLineSegment(ReferenceFrame referenceFrame) { super(referenceFrame, new LineSegment3d()); lineSegment3d = getGeometryObject(); }
public boolean isBetweenEndpoints(FramePoint point, double epsilon) { checkReferenceFrameMatch(point); return lineSegment3d.isBetweenEndpoints(point.getPoint(), epsilon); }
public void getDirection(boolean normalize, FrameVector directionToPack) { checkReferenceFrameMatch(directionToPack); lineSegment3d.getDirection(normalize, directionToPack.getVector()); }
private static OneDimensionalBounds[] computeOuterBounds(LineSegment3d lineSegment) { double[] aVals = new double[3]; lineSegment.getFirstEndpoint().get(aVals); double[] bVals = new double[3]; lineSegment.getSecondEndpoint().get(bVals); OneDimensionalBounds[] ret = new OneDimensionalBounds[3]; for (int i = 0; i < 3; i++) { ret[i] = new OneDimensionalBounds(Math.min(aVals[i], bVals[i]), Math.max(aVals[i], bVals[i])); } return ret; } public LineSegmentSearchVolume(Point2d point1, Point2d point2)
public void putLidarAtGraduallyMoreAccurateResolution(Point3d start, Point3d end) { HyperVolume line = new LineSegmentSearchVolume(new LineSegment3d(start, end)); double[] location = new double[] {end.getX(), end.getY(), end.getZ()}; this.put(location, true); this.leafAdded(new HyperCubeLeaf<Boolean>(true, location)); RecursableHyperTreeNode<Boolean, Void> endNode = this.getLeafNodeAtLocation(location); List<RecursableHyperTreeNode<Boolean, Void>> lineNodes = this.getHyperVolumeIntersection(line); lineNodes.remove(endNode); for (RecursableHyperTreeNode<Boolean, Void> node : lineNodes) { double[] intersectionWithBounds = line.intersectionWithBounds(new BoundsGetter(node).get()); HyperCubeLeaf<Boolean> hyperCubeLeaf = new HyperCubeLeaf<Boolean>(false, intersectionWithBounds); this.replacementPut(hyperCubeLeaf); } }
/** * Tests whether the projection of the given point onto this line segment is located * between the two endpoints with a given conservative tolerance {@code epsilon}: * <ul> * <li> if {@code epsilon > 0}, the point has to be between the endpoints and at a minimum * distance of {@code epsilon * this.length()} from the closest endpoint. * <li> if {@code epsilon < 0}, the point has to be between the endpoints or at a maximum * distance of {@code -epsilon * this.length()} from the closest endpoint. * <li> if {@code epsilon = 0}, the point has to be between the endpoints or equal to * one of the endpoints. * </ul> * * @param point the query. Not modified. * @param epsilon the tolerance to use. * @return {@code true} if the projection of the point is between the endpoints of this line segment, {@code false} otherwise. */ public boolean isBetweenEndpoints(Point3d point, double epsilon) { return isBetweenEndpoints(point.getX(), point.getY(), point.getZ(), epsilon); }
public LineSegmentSearchVolume(LineSegment3d lineSegment) { super(3, computeOuterBounds(lineSegment), null, true); dim = 3; pointA = new double[dim]; pointB = new double[dim]; scale = new double[dim]; lineSegment.getFirstEndpoint().get(pointA); lineSegment.getSecondEndpoint().get(pointB); for (int i=0;i<dim;i++) { scale[i]=1.0/(pointB[i]-pointA[i]); } } private static OneDimensionalBounds[] computeOuterBounds(LineSegment3d lineSegment)
protected LineSegment3d getLineSegment(int index, float range) { Vector3d origin = new Vector3d(); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.getTranslation(origin); return new LineSegment3d(new Point3d(origin), getPoint(index, range)); }