public FrameLineSegment(ReferenceFrame referenceFrame) { super(referenceFrame, new LineSegment3d()); lineSegment3d = getGeometryObject(); }
public FrameLineSegment(ReferenceFrame referenceFrame, Point3d firstEndpoint, Point3d secondEndpoint) { super(referenceFrame, new LineSegment3d(firstEndpoint, secondEndpoint)); lineSegment3d = getGeometryObject(); }
/** * 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 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); } }
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)); }
public void putLidarAtMinimumResolution(Point3d start, Point3d end) { HyperVolume line = new LineSegmentSearchVolume(new LineSegment3d(start, end)); double[] location = new double[] {end.getX(), end.getY(), end.getZ()}; this.upRezz(location); 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); } lock(); this.mergeParentRecursively(getRootNode(), location); unlock(); }