public void setDollyZ(double cameraDollyZ) { this.dollyPosition.setZ(cameraDollyZ); }
public void setPointZ(double z) { this.point.setZ(z); }
public void setTrackingZ(double cameraTrackingZ) { this.trackingPosition.setZ(cameraTrackingZ); }
public void setHeight(double z) { this.footstepData.location.setZ(z);}
public void getScanPoint(int index, Point3d scanPointToPack) { index *= 3; scanPointToPack.setX(scan[index++]); scanPointToPack.setY(scan[index++]); scanPointToPack.setZ(scan[index++]); }
public static void main(String[] args) { Point3d frst = new Point3d(); frst.setX(0); frst.setY(0); frst.setZ(0); System.out.println("(" + frst.getxCoord() + "," + frst.getyCoord() + "," + frst.getzCoord() + ")"); Point3d secnd = new Point3d(); secnd.setX(2); secnd.setY(12); secnd.setZ(24); System.out.println("(" + secnd.getxCoord() + "," + secnd.getyCoord() + "," + secnd.getzCoord() + ")"); }
@Override protected void orthogonalProjectionShapeFrame(Point3d pointToCheckAndPack) { if (pointToCheckAndPack.getZ() < 0.0) pointToCheckAndPack.setZ(0.0); if (pointToCheckAndPack.getZ() > height) pointToCheckAndPack.setZ(height); double xyLengthSquared = pointToCheckAndPack.getX() * pointToCheckAndPack.getX() + pointToCheckAndPack.getY() * pointToCheckAndPack.getY(); if (xyLengthSquared > radius * radius) { double xyLength = Math.sqrt(xyLengthSquared); double scale = radius / xyLength; pointToCheckAndPack.setX(pointToCheckAndPack.getX() * scale); pointToCheckAndPack.setY(pointToCheckAndPack.getY() * scale); } }
public void getBasePosition(Point3d point3d) { point3d.setX(this.baseX.getDoubleValue()); point3d.setY(this.baseY.getDoubleValue()); point3d.setZ(this.baseZ.getDoubleValue()); }
public void getBasePosition(Point3d point3d) { point3d.setX(this.baseX.getDoubleValue()); point3d.setY(this.baseY.getDoubleValue()); point3d.setZ(this.baseZ.getDoubleValue()); }
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3d planeNormal) { Point3d position = footstep.getLocation(); Quat4d orientation = footstep.getOrientation(); RigidBodyTransform solePose = new RigidBodyTransform(); double yaw = RotationTools.computeYaw(orientation); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); }
@Override public boolean isPossibleGround(Point3d point, Point3d lidarOrigin) { Point3d footAvg = getMidFootPoint(); double footZ = footAvg.getZ(); footAvg.setZ(point.getZ()); double maxHeight = parameters.quadTreeZAboveFeet + point.distance(footAvg) * parameters.quadTreeZSlope; if (maxHeight > parameters.quadTreeZMax) { maxHeight = parameters.quadTreeZMax; } return (point.getZ() - footZ) < maxHeight; }
pointsOnSphere[planeIndex].setX(rSinTheta * Math.cos(phi)); pointsOnSphere[planeIndex].setY(rSinTheta * Math.sin(phi)); pointsOnSphere[planeIndex].setZ(sphereRadius * Math.cos(theta)); pointsOnSphere[planeIndex].add(sphereOrigin);
pointToAdd.setZ(coefficientsOfParabola[2] * deltaTimesI * deltaTimesI + coefficientsOfParabola[1] * deltaTimesI + coefficientsOfParabola[0]); pointsOnPath.add(new Point3d(pointToAdd));
/** * Computes if the point is in the region projected onto the world xy-plane. * @param x x-coordinate of the query. * @param y y-coordinate of the query. * @return true if the point is inside this region, false otherwise. */ public boolean isPointInsideByProjectionOntoXYPlane(double x, double y) { Point3d localPoint = new Point3d(); localPoint.setX(x); localPoint.setY(y); localPoint.setZ(getPlaneZGivenXY(x, y)); fromWorldToLocalTransform.transform(localPoint); return isPointInside(localPoint.getX(), localPoint.getY()); }
/** * Multiply a 3x3 matrix by a 3x1 vector. Since result is stored in vector, the matrix must be 3x3. * @param matrix * @param point */ public static void mult(DenseMatrix64F matrix, Point3d point) { if (matrix.numCols != 3 || matrix.numRows != 3) { throw new RuntimeException("Improperly sized matrices."); } double x = point.getX(); double y = point.getY(); double z = point.getZ(); point.setX(matrix.get(0, 0) * x + matrix.get(0, 1) * y + matrix.get(0, 2) * z); point.setY(matrix.get(1, 0) * x + matrix.get(1, 1) * y + matrix.get(1, 2) * z); point.setZ(matrix.get(2, 0) * x + matrix.get(2, 1) * y + matrix.get(2, 2) * z); }
/** * Transform the Point3d point by this transform and place result back in * point. * * @param point */ public final void transform(Point3d point) { double x = mat00 * point.getX() + mat01 * point.getY() + mat02 * point.getZ() + mat03; double y = mat10 * point.getX() + mat11 * point.getY() + mat12 * point.getZ() + mat13; point.setZ(mat20 * point.getX() + mat21 * point.getY() + mat22 * point.getZ() + mat23); point.setX(x); point.setY(y); }
/** * Transform the Point3d pointIn by this transform and place result in * pointOut. * * @param point */ public final void transform(Point3d pointIn, Point3d pointOut) { if (pointIn != pointOut) { pointOut.setX(mat00 * pointIn.getX() + mat01 * pointIn.getY() + mat02 * pointIn.getZ() + mat03); pointOut.setY(mat10 * pointIn.getX() + mat11 * pointIn.getY() + mat12 * pointIn.getZ() + mat13); pointOut.setZ(mat20 * pointIn.getX() + mat21 * pointIn.getY() + mat22 * pointIn.getZ() + mat23); } else { transform(pointIn); } }
/** * Projects the input ConvexPolygon2d to the plane defined by this PlanarRegion by translating each vertex in world z. * Then puts each vertex in local frame. In doing so, the area of the rotated polygon will actually increase on tilted PlanarRegions. * @param convexPolygonInWorld Polygon to project * @return new projected ConvexPolygon2d */ private ConvexPolygon2d projectPolygonVerticallyToRegion(ConvexPolygon2d convexPolygonInWorld) { ConvexPolygon2d projectedPolygon = new ConvexPolygon2d(); Point3d snappedVertex3d = new Point3d(); for (int i = 0; i < convexPolygonInWorld.getNumberOfVertices(); i++) { Point2d originalVertex = convexPolygonInWorld.getVertex(i); // Find the vertex 3d that is snapped to the plane following z-world. snappedVertex3d.setX(originalVertex.getX()); snappedVertex3d.setY(originalVertex.getY()); snappedVertex3d.setZ(getPlaneZGivenXY(originalVertex.getX(), originalVertex.getY())); // Transform to local coordinates fromWorldToLocalTransform.transform(snappedVertex3d); // Add the snapped vertex to the snapped polygon projectedPolygon.addVertex(snappedVertex3d.getX(), snappedVertex3d.getY()); } projectedPolygon.update(); return projectedPolygon; }