hellVertices.add(Vec3DDouble.newBuilder().setX(2).setY(0).setZ(0).build()); Shape hellShape = Shape.newBuilder().addAllFloor(hellVertices).build(); Pose hellPosition = Pose.newBuilder().setTranslation(Translation.newBuilder().setX(3).setY(1).setZ(0).build()) .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0).build()).build(); PlacementConfig hellPlacement = PlacementConfig.newBuilder().setPosition(hellPosition).setShape(hellShape).setLocationId(paradiseLocation.getId()).build(); stairwayVertices.add(Vec3DDouble.newBuilder().setX(4).setY(0).setZ(0).build()); Shape stairwayShape = Shape.newBuilder().addAllFloor(stairwayVertices).build(); Pose stairwayPosition = Pose.newBuilder().setTranslation(Translation.newBuilder().setX(1).setY(0).setZ(0).build()) .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0).build()).build(); PlacementConfig stairwayPlacement = PlacementConfig.newBuilder().setPosition(stairwayPosition).setShape(stairwayShape).setLocationId(paradiseLocation.getId()).build(); heavenVertices.add(Vec3DDouble.newBuilder().setX(2).setY(0).setZ(0).build()); Shape heavenShape = Shape.newBuilder().addAllFloor(heavenVertices).build(); Pose heavenPosition = Pose.newBuilder().setTranslation(Translation.newBuilder().setX(1).setY(1).setZ(0).build()) .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0).build()).build(); PlacementConfig heavenPlacement = PlacementConfig.newBuilder().setPosition(heavenPosition).setShape(heavenShape).setLocationId(paradiseLocation.getId()).build(); edenVertices.add(Vec3DDouble.newBuilder().setX(1).setY(0).setZ(0).build()); Shape edenShape = Shape.newBuilder().addAllFloor(edenVertices).build(); Pose edenPosition = Pose.newBuilder().setTranslation(Translation.newBuilder().setX(0).setY(2).setZ(0).build()) .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0).build()).build(); PlacementConfig edenPlacement = PlacementConfig.newBuilder().setPosition(edenPosition).setShape(edenShape).setLocationId(heavenLocation.getId()).build();
if (deviceUnitConfig.getPlacementConfig().hasPosition() && !dalUnitConfig.getPlacementConfig().getPosition().equals(deviceUnitConfig.getPlacementConfig().getPosition())) { dalUnitConfig.getPlacementConfigBuilder().setPosition(deviceUnitConfig.getPlacementConfig().getPosition()); modification = true;
/** * Updates the placement config based on the minimal coordinates and * dimensions of the bounding box. * * @param placementConfig the PlacementConfig to be updated. * @param minPosition the minimal coordinates of the new bounding box. * @param dimensions the dimensions of the bounding box (x: width, y: depth, * z: height). * @return the updated PlacementConfig including new Position, zero Rotation * and the updated bounding box. */ private static PlacementConfig updatePlacementConfig(PlacementConfig placementConfig, Point3d minPosition, Point3d dimensions) { PlacementConfig.Builder newBuilder = placementConfig.toBuilder(); newBuilder.setShape(newBuilder.getShapeBuilder() .setBoundingBox(AxisAlignedBoundingBox3DFloat.newBuilder() .setLeftFrontBottom(Translation.newBuilder().setX(0).setY(0).setZ(0)) .setWidth((float) dimensions.x) .setDepth((float) dimensions.y) .setHeight((float) dimensions.z))) .setPosition(Pose.newBuilder() .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0)) .setTranslation(Translation.newBuilder() .setX(minPosition.x) .setY(minPosition.y) .setZ(minPosition.z))); return newBuilder.build(); }
if (unitPlacement.hasPosition() && !unitPlacement.getPosition().equals(devicePlacement.getPosition())) { devicePlacement.setPosition(unitPlacement.getPosition()); modification = true;
/** * Gets the local rotation of the unit relative to its parent location. * * @return relative rotation * * @throws NotAvailableException is thrown if the unit config or parts of it are not available. */ default RotationType.Rotation getUnitRotation() throws NotAvailableException { try { if (!getConfig().hasPlacementConfig()) { throw new NotAvailableException("PlacementConfig"); } if (!getConfig().getPlacementConfig().hasPosition()) { throw new NotAvailableException("Position"); } return getConfig().getPlacementConfig().getPosition().getRotation(); } catch (final CouldNotPerformException ex) { throw new NotAvailableException("UnitRotation", ex); } }
/** * Gets the local position of the unit relative to its parent location. * * @return relative position * * @throws NotAvailableException is thrown if the unit config or parts of it are not available. */ default TranslationType.Translation getUnitPosition() throws NotAvailableException { try { if (!getConfig().hasPlacementConfig()) { throw new NotAvailableException("PlacementConfig"); } //todo release : rename PlacementConfig position into pose. if (!getConfig().getPlacementConfig().hasPosition()) { throw new NotAvailableException("Position"); } return getConfig().getPlacementConfig().getPosition().getTranslation(); } catch (final CouldNotPerformException ex) { throw new NotAvailableException("UnitPosition", ex); } }
@Override public void processData(String id, IdentifiableMessage<String, UnitConfig, UnitConfig.Builder> entry, ProtoBufMessageMap<String, UnitConfig, UnitConfig.Builder> entryMap, ProtoBufRegistry<String, UnitConfig, UnitConfig.Builder> registry) throws CouldNotPerformException, EntryModification { UnitConfig.Builder unitConfig = entry.getMessage().toBuilder(); if (unitConfig.getType() == UnitTemplateType.UnitTemplate.UnitType.LOCATION) { return; } if (unitConfig.hasPlacementConfig() && unitConfig.getPlacementConfig().hasPosition()) { final Pose pose = unitConfig.getPlacementConfig().getPosition(); if (pose.getTranslation().getX() == 0 && pose.getTranslation().getY() == 0 && pose.getTranslation().getZ() == 0) { unitConfig.getPlacementConfigBuilder().clearPosition(); throw new EntryModification(entry.setMessage(unitConfig), this); } } } }
private Pose generateDefaultInstance() { return Pose.newBuilder() .setRotation(Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0)) .setTranslation(Translation.newBuilder().setX(0).setY(0).setZ(0)).build(); } }
public static Transform3D transform(final PoseType.Pose position) { RotationType.Rotation pRotation = position.getRotation(); TranslationType.Translation pTranslation = position.getTranslation(); Quat4d jRotation = new Quat4d(pRotation.getQx(), pRotation.getQy(), pRotation.getQz(), pRotation.getQw()); Vector3d jTranslation = new Vector3d(pTranslation.getX(), pTranslation.getY(), pTranslation.getZ()); return new Transform3D(jRotation, jTranslation, 1.0); } }
public static PlacementConfig getDefaultPlacement(UnitConfig location) { Rotation rotation = Rotation.newBuilder().setQw(1).setQx(0).setQy(0).setQz(0).build(); Translation translation = Translation.newBuilder().setX(0).setY(0).setZ(0).build(); Pose pose = Pose.newBuilder().setRotation(rotation).setTranslation(translation).build(); return PlacementConfig.newBuilder().setPosition(pose).setLocationId(location.getId()).build(); }