/** * Sets the z-component of this vector. * * @param z the z-component. */ @Override public void setZ(double z) { vector.setZ(z); }
/** * Computes the convex hull of the intersections of footPolygonInWorldFrame when snapped to planarRegion using * snapTransform * * @param planarRegion * @param footPolygon foot polygon in world frame * @param snapTransform * @return intersection polygon in region frame */ public static ConvexPolygon2D getConvexHullOfPolygonIntersections(PlanarRegion planarRegion, ConvexPolygon2D footPolygon, RigidBodyTransform snapTransform) { ArrayList<ConvexPolygon2D> intersections = new ArrayList<>(); ConvexPolygon2D footPolygonInPlaneFrame = new ConvexPolygon2D(); RigidBodyTransform inverseSnapTransform = new RigidBodyTransform(snapTransform); inverseSnapTransform.invert(); for (int i = 0; i < footPolygon.getNumberOfVertices(); i++) { Point2DReadOnly vertex = footPolygon.getVertex(i); Vector4D transformPoint = new Vector4D(vertex.getX(), vertex.getY(), 0.0, 1.0); snapTransform.transform(transformPoint); transformPoint.setZ(0.0); footPolygonInPlaneFrame.addVertex(transformPoint.getX() + 1e-10, transformPoint.getY() + 1e-10); } footPolygonInPlaneFrame.update(); planarRegion.getPolygonIntersectionsWhenProjectedVertically(footPolygonInPlaneFrame, intersections); return getConvexHull(intersections); }
public static Vector4D nextVector4D(Random random, Tuple4DReadOnly lowerBound, Tuple4DReadOnly upperBound) { Vector4D ret = new Vector4D(); ret.setX(RandomNumbers.nextDouble(random, lowerBound.getX(), upperBound.getX())); ret.setY(RandomNumbers.nextDouble(random, lowerBound.getY(), upperBound.getY())); ret.setZ(RandomNumbers.nextDouble(random, lowerBound.getZ(), upperBound.getZ())); ret.setS(RandomNumbers.nextDouble(random, lowerBound.getS(), upperBound.getS())); return ret; }
vectorExpected.setX(matrix.getM00() * vectorOriginal.getX()); vectorExpected.setY(matrix.getM11() * vectorOriginal.getY()); vectorExpected.setZ(matrix.getM22() * vectorOriginal.getZ()); vectorExpected.setS(vectorOriginal.getS());